diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 2a6364d6113d4..653c95b060163 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -129,16 +129,7 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface ModuleStatus updateState() override; protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override - { - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); - } + void updateRTCStatus(const double start_distance, const double finish_distance) override; }; class LaneChangeBTInterface : public LaneChangeInterface @@ -178,16 +169,7 @@ class LaneChangeBTModule : public LaneChangeBTInterface const std::shared_ptr & parameters); protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override - { - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); - } + void updateRTCStatus(const double start_distance, const double finish_distance) override; }; class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index ccfcf76736d03..9889a94648942 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -356,6 +356,18 @@ ModuleStatus AvoidanceByLaneChangeInterface::updateState() return current_state_; } +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + LaneChangeBTInterface::LaneChangeBTInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -460,6 +472,18 @@ LaneChangeBTModule::LaneChangeBTModule( std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} { } + +void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + ExternalRequestLaneChangeLeftBTModule::ExternalRequestLaneChangeLeftBTModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters)