Skip to content

Commit

Permalink
refactor(behavior_path_planner): unify rtc_interface instance
Browse files Browse the repository at this point in the history 
Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 committed Mar 31, 2023
1 parent 70cf872 commit 5259725
Show file tree
Hide file tree
Showing 18 changed files with 107 additions and 209 deletions.
76 changes: 17 additions & 59 deletions ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,7 @@ class AvoidanceModule : public SceneModuleInterface
const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters);
#else
AvoidanceModule(
const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters,
std::shared_ptr<RTCInterface> & rtc_interface_left,
std::shared_ptr<RTCInterface> & rtc_interface_right);
const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters);
#endif

bool isExecutionRequested() const override;
Expand All @@ -72,35 +70,6 @@ class AvoidanceModule : public SceneModuleInterface
parameters_ = parameters;
}
#endif

void publishRTCStatus() override
{
rtc_interface_left_->publishCooperateStatus(clock_->now());
rtc_interface_right_->publishCooperateStatus(clock_->now());
}

bool isActivated() override
{
if (rtc_interface_left_->isRegistered(uuid_left_)) {
return rtc_interface_left_->isActivated(uuid_left_);
}
if (rtc_interface_right_->isRegistered(uuid_right_)) {
return rtc_interface_right_->isActivated(uuid_right_);
}
return false;
}

void lockRTCCommand() override
{
rtc_interface_left_->lockCommandUpdate();
rtc_interface_right_->lockCommandUpdate();
}

void unlockRTCCommand() override
{
rtc_interface_left_->unlockCommandUpdate();
rtc_interface_right_->unlockCommandUpdate();
}
std::shared_ptr<AvoidanceDebugMsgArray> get_debug_msg_array() const;

private:
Expand All @@ -118,29 +87,24 @@ class AvoidanceModule : public SceneModuleInterface

PathShifter path_shifter_;

std::shared_ptr<RTCInterface> rtc_interface_left_;
std::shared_ptr<RTCInterface> rtc_interface_right_;

RegisteredShiftLineArray left_shift_array_;
RegisteredShiftLineArray right_shift_array_;
UUID candidate_uuid_;
UUID uuid_left_;
UUID uuid_right_;

void updateCandidateRTCStatus(const CandidateOutput & candidate)
{
if (candidate.lateral_shift > 0.0) {
rtc_interface_left_->updateCooperateStatus(
uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
uuid_vec_.at(0), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_left_;
candidate_uuid_ = uuid_vec_.at(0);
return;
}
if (candidate.lateral_shift < 0.0) {
rtc_interface_right_->updateCooperateStatus(
uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
uuid_vec_.at(1), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_right_;
candidate_uuid_ = uuid_vec_.at(1);
return;
}

Expand All @@ -158,7 +122,7 @@ class AvoidanceModule : public SceneModuleInterface
calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
rtc_interface_left_->updateCooperateStatus(
rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
steering_factor_interface_ptr_->updateSteeringFactor(
Expand All @@ -172,7 +136,7 @@ class AvoidanceModule : public SceneModuleInterface
calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
rtc_interface_right_->updateCooperateStatus(
rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
right_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
steering_factor_interface_ptr_->updateSteeringFactor(
Expand All @@ -183,32 +147,26 @@ class AvoidanceModule : public SceneModuleInterface
}
}

void removeRTCStatus() override
{
rtc_interface_left_->clearCooperateStatus();
rtc_interface_right_->clearCooperateStatus();
}

void removeCandidateRTCStatus()
{
if (rtc_interface_left_->isRegistered(candidate_uuid_)) {
rtc_interface_left_->removeCooperateStatus(candidate_uuid_);
} else if (rtc_interface_right_->isRegistered(candidate_uuid_)) {
rtc_interface_right_->removeCooperateStatus(candidate_uuid_);
if (rtc_interface_ptr_vec_.at(0)->isRegistered(candidate_uuid_)) {
rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(candidate_uuid_);
} else if (rtc_interface_ptr_vec_.at(1)->isRegistered(candidate_uuid_)) {
rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(candidate_uuid_);
}
}

void removePreviousRTCStatusLeft()
{
if (rtc_interface_left_->isRegistered(uuid_left_)) {
rtc_interface_left_->removeCooperateStatus(uuid_left_);
if (rtc_interface_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) {
rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0));
}
}

void removePreviousRTCStatusRight()
{
if (rtc_interface_right_->isRegistered(uuid_right_)) {
rtc_interface_right_->removeCooperateStatus(uuid_right_);
if (rtc_interface_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) {
rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1));
}
}

Expand Down
7 changes: 1 addition & 6 deletions ...ng/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,17 +38,12 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface

std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_shared<AvoidanceModule>(
name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_);
return std::make_shared<AvoidanceModule>(name_, *node_, parameters_, {"left", "right"});
}

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

private:
std::shared_ptr<RTCInterface> rtc_interface_left_;

std::shared_ptr<RTCInterface> rtc_interface_right_;

std::shared_ptr<AvoidanceParameters> parameters_;

std::vector<std::shared_ptr<AvoidanceModule>> registered_modules_;
Expand Down
72 changes: 16 additions & 56 deletions ...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,7 @@ class LaneChangeModule : public SceneModuleInterface
#else
LaneChangeModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<LaneChangeParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface, Direction direction,
const std::shared_ptr<LaneChangeParameters> & parameters, Direction direction,
LaneChangeModuleType type);
#endif
bool isExecutionRequested() const override;
Expand All @@ -76,36 +75,7 @@ class LaneChangeModule : public SceneModuleInterface
void acceptVisitor(
[[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override;

#ifdef USE_OLD_ARCHITECTURE
void publishRTCStatus() override
{
rtc_interface_left_->publishCooperateStatus(clock_->now());
rtc_interface_right_->publishCooperateStatus(clock_->now());
}

bool isActivated() override
{
if (rtc_interface_left_->isRegistered(uuid_left_)) {
return rtc_interface_left_->isActivated(uuid_left_);
}
if (rtc_interface_right_->isRegistered(uuid_right_)) {
return rtc_interface_right_->isActivated(uuid_right_);
}
return false;
}

void lockRTCCommand() override
{
rtc_interface_left_->lockCommandUpdate();
rtc_interface_right_->lockCommandUpdate();
}

void unlockRTCCommand() override
{
rtc_interface_left_->unlockCommandUpdate();
rtc_interface_right_->unlockCommandUpdate();
}
#else
#ifndef USE_OLD_ARCHITECTURE
void updateModuleParams(const std::shared_ptr<LaneChangeParameters> & parameters)
{
parameters_ = parameters;
Expand All @@ -129,10 +99,6 @@ class LaneChangeModule : public SceneModuleInterface
bool is_activated_{false};

#ifdef USE_OLD_ARCHITECTURE
std::shared_ptr<RTCInterface> rtc_interface_left_;
std::shared_ptr<RTCInterface> rtc_interface_right_;
UUID uuid_left_;
UUID uuid_right_;
UUID candidate_uuid_;
#else
Direction direction_{Direction::NONE};
Expand All @@ -144,32 +110,32 @@ class LaneChangeModule : public SceneModuleInterface
#ifdef USE_OLD_ARCHITECTURE
void waitApprovalLeft(const double start_distance, const double finish_distance)
{
rtc_interface_left_->updateCooperateStatus(
uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now());
rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
uuid_vec_.at(0), isExecutionReady(), start_distance, finish_distance, clock_->now());
is_waiting_approval_ = true;
}

void waitApprovalRight(const double start_distance, const double finish_distance)
{
rtc_interface_right_->updateCooperateStatus(
uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now());
rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
uuid_vec_.at(1), isExecutionReady(), start_distance, finish_distance, clock_->now());
is_waiting_approval_ = true;
}

void updateRTCStatus(const CandidateOutput & candidate)
{
if (candidate.lateral_shift > 0.0) {
rtc_interface_left_->updateCooperateStatus(
uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_vec_.at(0)->updateCooperateStatus(
uuid_vec_.at(0), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_left_;
candidate_uuid_ = uuid_vec_.at(0);
return;
}
if (candidate.lateral_shift < 0.0) {
rtc_interface_right_->updateCooperateStatus(
uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_vec_.at(1)->updateCooperateStatus(
uuid_vec_.at(1), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_right_;
candidate_uuid_ = uuid_vec_.at(1);
return;
}

Expand All @@ -178,23 +144,17 @@ class LaneChangeModule : public SceneModuleInterface
"Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change);
}

void removeRTCStatus() override
{
rtc_interface_left_->clearCooperateStatus();
rtc_interface_right_->clearCooperateStatus();
}

void removePreviousRTCStatusLeft()
{
if (rtc_interface_left_->isRegistered(uuid_left_)) {
rtc_interface_left_->removeCooperateStatus(uuid_left_);
if (rtc_interface_ptr_vec_.at(0)->isRegistered(uuid_vec_.at(0))) {
rtc_interface_ptr_vec_.at(0)->removeCooperateStatus(uuid_vec_.at(0));
}
}

void removePreviousRTCStatusRight()
{
if (rtc_interface_right_->isRegistered(uuid_right_)) {
rtc_interface_right_->removeCooperateStatus(uuid_right_);
if (rtc_interface_ptr_vec_.at(1)->isRegistered(uuid_vec_.at(1))) {
rtc_interface_ptr_vec_.at(1)->removeCooperateStatus(uuid_vec_.at(1));
}
}
#endif
Expand Down
4 changes: 1 addition & 3 deletions .../behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,12 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_shared<LaneChangeModule>(
name_, *node_, parameters_, rtc_interface_, direction_, type_);
name_, *node_, parameters_, {"left", "right"}, direction_, type_);
}

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

private:
std::shared_ptr<RTCInterface> rtc_interface_;

std::shared_ptr<LaneChangeParameters> parameters_;

std::vector<std::shared_ptr<LaneChangeModule>> registered_modules_;
Expand Down
4 changes: 1 addition & 3 deletions ...ing/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,16 +36,14 @@ class PullOutModuleManager : public SceneModuleManagerInterface

std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_shared<PullOutModule>(name_, *node_, parameters_, rtc_interface_);
return std::make_shared<PullOutModule>(name_, *node_, parameters_);
}

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

private:
std::shared_ptr<PullOutParameters> parameters_;

std::shared_ptr<RTCInterface> rtc_interface_;

std::vector<std::shared_ptr<PullOutModule>> registered_modules_;
};

Expand Down
3 changes: 1 addition & 2 deletions ...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,7 @@ class PullOutModule : public SceneModuleInterface
#else
PullOutModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<PullOutParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface);
const std::shared_ptr<PullOutParameters> & parameters);

void updateModuleParams(const std::shared_ptr<PullOutParameters> & parameters)
{
Expand Down
4 changes: 1 addition & 3 deletions ...ng/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,16 +36,14 @@ class PullOverModuleManager : public SceneModuleManagerInterface

std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_shared<PullOverModule>(name_, *node_, parameters_, rtc_interface_);
return std::make_shared<PullOverModule>(name_, *node_, parameters_);
}

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

private:
std::shared_ptr<PullOverParameters> parameters_;

std::shared_ptr<RTCInterface> rtc_interface_;

std::vector<std::shared_ptr<PullOverModule>> registered_modules_;
};

Expand Down
3 changes: 1 addition & 2 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,7 @@ class PullOverModule : public SceneModuleInterface
#else
PullOverModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<PullOverParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface);
const std::shared_ptr<PullOverParameters> & parameters);

void updateModuleParams(const std::shared_ptr<PullOverParameters> & parameters)
{
Expand Down
Loading

0 comments on commit 5259725

Please sign in to comment.