diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 127b04e8b3445..d92567de42463 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -59,7 +59,7 @@ class AvoidanceModule : public SceneModuleInterface rtc_interface_right_.publishCooperateStatus(clock_->now()); } - bool isActivated() const override + bool isActivated() override { if (rtc_interface_left_.isRegistered(uuid_left_)) { return rtc_interface_left_.isActivated(uuid_left_); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 41cbcbbbc1d61..fcf9e635f4595 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -98,7 +98,7 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_.publishCooperateStatus(clock_->now()); } - bool isActivated() const override + bool isActivated() override { if (rtc_interface_left_.isRegistered(uuid_left_)) { return rtc_interface_left_.isActivated(uuid_left_); @@ -124,6 +124,8 @@ class LaneChangeModule : public SceneModuleInterface UUID uuid_left_; UUID uuid_right_; + bool is_activated_ = false; + void waitApprovalLeft(const double distance) { rtc_interface_left_.updateCooperateStatus( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 4ec7448733652..702088107c831 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -203,7 +203,7 @@ class SceneModuleInterface /** * @brief Return true if the activation command is received */ - virtual bool isActivated() const + virtual bool isActivated() { if (!rtc_interface_ptr_) { return true; @@ -270,7 +270,7 @@ class SceneModuleInterface void clearWaitingApproval() { is_waiting_approval_ = false; } - UUID generateUUID() + static UUID generateUUID() { // Generate random number UUID uuid; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 9f20a2aa28be2..85dc43e205f66 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -50,8 +50,8 @@ AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters}, - rtc_interface_left_(node, "avoidance_left"), - rtc_interface_right_(node, "avoidance_right"), + rtc_interface_left_(&node, "avoidance_left"), + rtc_interface_right_(&node, "avoidance_right"), uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index f90aa3a9a1d1a..40d237ac41878 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -39,8 +39,8 @@ LaneChangeModule::LaneChangeModule( const std::string & name, rclcpp::Node & node, const LaneChangeParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters}, - rtc_interface_left_(node, "lane_change_left"), - rtc_interface_right_(node, "lane_change_right"), + rtc_interface_left_(&node, "lane_change_left"), + rtc_interface_right_(&node, "lane_change_right"), uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { @@ -50,6 +50,7 @@ BehaviorModuleOutput LaneChangeModule::run() { RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); current_state_ = BT::NodeStatus::RUNNING; + is_activated_ = isActivated(); const auto output = plan(); const auto turn_signal_info = output.turn_signal_info; if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -475,7 +476,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const return false; } - if (!isActivated()) { + if (!is_activated_) { return false; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index c042b6083a204..24f10cfefcaf7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -40,7 +40,7 @@ PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters} { - rtc_interface_ptr_ = std::make_shared(node, "pull_out"); + rtc_interface_ptr_ = std::make_shared(&node, "pull_out"); } BehaviorModuleOutput PullOutModule::run() diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b493fd9bcc6b4..7a44c8c9ccbdb 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -39,7 +39,7 @@ PullOverModule::PullOverModule( const std::string & name, rclcpp::Node & node, const PullOverParameters & parameters) : SceneModuleInterface{name, node}, parameters_{parameters} { - rtc_interface_ptr_ = std::make_shared(node, "pull_over"); + rtc_interface_ptr_ = std::make_shared(&node, "pull_over"); } BehaviorModuleOutput PullOverModule::run() diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 59868d6996504..dc129687e9b42 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -269,7 +269,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface { public: SceneModuleManagerInterfaceWithRTC(rclcpp::Node & node, const char * module_name) - : SceneModuleManagerInterface(node, module_name), rtc_interface_(node, module_name) + : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name) { } diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index df9dc4d1c2430..6e3139254323d 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -26,6 +26,7 @@ #include "tier4_rtc_msgs/srv/cooperate_commands.hpp" #include +#include #include #include @@ -43,14 +44,14 @@ using unique_identifier_msgs::msg::UUID; class RTCInterface { public: - RTCInterface(rclcpp::Node & node, const std::string & name); + RTCInterface(rclcpp::Node * node, const std::string & name); void publishCooperateStatus(const rclcpp::Time & stamp); void updateCooperateStatus( const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); - bool isActivated(const UUID & uuid) const; - bool isRegistered(const UUID & uuid) const; + bool isActivated(const UUID & uuid); + bool isRegistered(const UUID & uuid); private: void onCooperateCommandService( @@ -61,6 +62,8 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_statuses_; rclcpp::Service::SharedPtr srv_commands_; + std::mutex mutex_; + rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::Logger logger_; Module module_; CooperateStatusArray registered_status_; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index c4e0ed0bc374d..3285a547a1a6f 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -68,19 +68,22 @@ Module getModuleType(const std::string & module_name) namespace rtc_interface { -RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name) -: logger_{node.get_logger().get_child("RTCInterface[" + name + "]")} +RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name) +: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")} { using std::placeholders::_1; using std::placeholders::_2; // Publisher - pub_statuses_ = node.create_publisher("~/" + name + "/cooperate_status", 1); + pub_statuses_ = + node->create_publisher("~/" + name + "/cooperate_status", 1); // Service - srv_commands_ = node.create_service( + callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_commands_ = node->create_service( "~/" + name + "/cooperate_commands", - std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2)); + std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2), + rmw_qos_profile_services_default, callback_group_); // Module module_ = getModuleType(name); @@ -88,6 +91,7 @@ RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name) void RTCInterface::publishCooperateStatus(const rclcpp::Time & stamp) { + std::lock_guard lock(mutex_); registered_status_.stamp = stamp; pub_statuses_->publish(registered_status_); } @@ -96,6 +100,7 @@ void RTCInterface::onCooperateCommandService( const CooperateCommands::Request::SharedPtr request, const CooperateCommands::Response::SharedPtr responses) { + std::lock_guard lock(mutex_); for (const auto & command : request->commands) { CooperateResponse response; response.uuid = command.uuid; @@ -122,6 +127,7 @@ void RTCInterface::onCooperateCommandService( void RTCInterface::updateCooperateStatus( const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp) { + std::lock_guard lock(mutex_); // Find registered status which has same uuid auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), @@ -148,6 +154,7 @@ void RTCInterface::updateCooperateStatus( void RTCInterface::removeCooperateStatus(const UUID & uuid) { + std::lock_guard lock(mutex_); // Find registered status which has same uuid and erase it const auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), @@ -163,10 +170,15 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid) "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); } -void RTCInterface::clearCooperateStatus() { registered_status_.statuses.clear(); } +void RTCInterface::clearCooperateStatus() +{ + std::lock_guard lock(mutex_); + registered_status_.statuses.clear(); +} -bool RTCInterface::isActivated(const UUID & uuid) const +bool RTCInterface::isActivated(const UUID & uuid) { + std::lock_guard lock(mutex_); const auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), [uuid](auto & s) { return s.uuid == uuid; }); @@ -180,8 +192,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const return false; } -bool RTCInterface::isRegistered(const UUID & uuid) const +bool RTCInterface::isRegistered(const UUID & uuid) { + std::lock_guard lock(mutex_); const auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), [uuid](auto & s) { return s.uuid == uuid; });