From 8d0d52afbd57d5442ee77839e22766c4cb7ea6d4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 23:16:00 +0900 Subject: [PATCH] fix(mission_planner/behavior_path_planner): use transient local for modified goal (#5012) (#852) fix(mission_planner/behavior_path_planner): use trasient local for modified goal Signed-off-by: kosuke55 --- .../src/behavior_path_planner_node.cpp | 4 +++- .../src/mission_planner/mission_planner.cpp | 9 +++++---- .../src/mission_planner/mission_planner.hpp | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 31da3e892ffe5..3f33bcd7b64dc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); - modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + const auto durable_qos = rclcpp::QoS(1).transient_local(); + modified_goal_publisher_ = + create_publisher("~/output/modified_goal", durable_qos); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..41fe1d552b9f4 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -90,12 +90,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + const auto durable_qos = rclcpp::QoS(1).transient_local(); vector_map_subscriber_ = create_subscription( - "input/vector_map", qos_transient_local, + "input/vector_map", durable_qos, std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + sub_modified_goal_ = create_subscription( + "input/modified_goal", durable_qos, + std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -110,7 +112,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); - adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); change_state(RouteState::Message::UNSET); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..ad8ca9cb07fbf 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -72,8 +72,10 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; Odometry::ConstSharedPtr odometry_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -124,8 +126,6 @@ class MissionPlanner : public rclcpp::Node HADMapBin::ConstSharedPtr map_ptr_{nullptr}; void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; - void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res);