Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor(behavior_path_planner): unify rtc_interface_ptr in SceneModuleInterface #3242

Merged
merged 6 commits into from
Apr 4, 2023
Merged

refactor(behavior_path_planner): unify rtc_interface_ptr in SceneModuleInterface #3242

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
52ad6a8
refactor(behavior_path_planner): unify rtc_interface instance
takayuki5168 Mar 31, 2023
5534eeb
fix bug
takayuki5168 Apr 1, 2023
3659416
fix bug
takayuki5168 Apr 3, 2023
4691ced
fix
takayuki5168 Apr 3, 2023
5cca4b5
minor change
takayuki5168 Apr 3, 2023
8433338
fix build
takayuki5168 Apr 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
76 changes: 18 additions & 58 deletions ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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