Skip to content

Commit

Permalink
refactor(behavior_path_planner): unify rtc_interface_ptr in SceneModu…
Browse files Browse the repository at this point in the history 
…leInterface (#3242)

* refactor(behavior_path_planner): unify rtc_interface instance

Signed-off-by: Takayuki Murooka <[email protected]>

* fix bug

Signed-off-by: Takayuki Murooka <[email protected]>

* fix bug

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* minor change

Signed-off-by: Takayuki Murooka <[email protected]>

* fix build

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 authored Apr 4, 2023
1 parent 11b73eb commit e664bc4
Show file tree
Hide file tree
Showing 26 changed files with 194 additions and 234 deletions.
76 changes: 18 additions & 58 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 @@ -30,6 +30,7 @@
#include <algorithm>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

Expand All @@ -50,8 +51,7 @@ class AvoidanceModule : public SceneModuleInterface
#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::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
#endif

bool isExecutionRequested() const override;
Expand All @@ -72,35 +72,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 +89,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_map_.at("left")->updateCooperateStatus(
uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_left_;
candidate_uuid_ = uuid_map_.at("left");
return;
}
if (candidate.lateral_shift < 0.0) {
rtc_interface_right_->updateCooperateStatus(
uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_right_;
candidate_uuid_ = uuid_map_.at("right");
return;
}

Expand All @@ -158,7 +124,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_map_.at("left")->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 +138,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_map_.at("right")->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 +149,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_map_.at("left")->isRegistered(candidate_uuid_)) {
rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_);
} else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) {
rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_);
}
}

void removePreviousRTCStatusLeft()
{
if (rtc_interface_left_->isRegistered(uuid_left_)) {
rtc_interface_left_->removeCooperateStatus(uuid_left_);
if (rtc_interface_ptr_map_.at("left")->isRegistered(uuid_map_.at("left"))) {
rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left"));
}
}

void removePreviousRTCStatusRight()
{
if (rtc_interface_right_->isRegistered(uuid_right_)) {
rtc_interface_right_->removeCooperateStatus(uuid_right_);
if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) {
rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right"));
}
}

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_, rtc_interface_ptr_map_);
}

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
73 changes: 17 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 @@ -60,8 +60,8 @@ class LaneChangeModule : public SceneModuleInterface
LaneChangeModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<LaneChangeParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface, Direction direction,
LaneChangeModuleType type);
const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map,
Direction direction, LaneChangeModuleType type);
#endif
bool isExecutionRequested() const override;
bool isExecutionReady() const override;
Expand All @@ -76,36 +76,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 +100,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 +111,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_map_.at("left")->updateCooperateStatus(
uuid_map_.at("left"), 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_map_.at("right")->updateCooperateStatus(
uuid_map_.at("right"), 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_map_.at("left")->updateCooperateStatus(
uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_left_;
candidate_uuid_ = uuid_map_.at("left");
return;
}
if (candidate.lateral_shift < 0.0) {
rtc_interface_right_->updateCooperateStatus(
uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
candidate.finish_distance_to_path_change, clock_->now());
candidate_uuid_ = uuid_right_;
candidate_uuid_ = uuid_map_.at("right");
return;
}

Expand All @@ -178,23 +145,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_map_.at("left")->isRegistered(uuid_map_.at("left"))) {
rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left"));
}
}

void removePreviousRTCStatusRight()
{
if (rtc_interface_right_->isRegistered(uuid_right_)) {
rtc_interface_right_->removeCooperateStatus(uuid_right_);
if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) {
rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right"));
}
}
#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_, rtc_interface_ptr_map_, 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_, rtc_interface_ptr_map_);
}

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: 2 additions & 1 deletion ...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 @@ -36,6 +36,7 @@
#include <deque>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -71,7 +72,7 @@ class PullOutModule : public SceneModuleInterface
PullOutModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<PullOutParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface);
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);

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_, rtc_interface_ptr_map_);
}

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: 2 additions & 1 deletion ...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 @@ -35,6 +35,7 @@
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -93,7 +94,7 @@ class PullOverModule : public SceneModuleInterface
PullOverModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<PullOverParameters> & parameters,
const std::shared_ptr<RTCInterface> & rtc_interface);
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);

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

0 comments on commit e664bc4

Please sign in to comment.