fix(mission_planner/behavior_path_planner): use transient local for m…

…odified goal (autowarefoundation#5012) (#852) fix(mission_planner/behavior_path_planner): use trasient local for modified goal Signed-off-by: kosuke55 <[email protected]>
tier4 · Sep 19, 2023 · 8d0d52a · 8d0d52a
1 parent f4f4a14
commit 8d0d52a
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 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
@@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   turn_signal_publisher_ =
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
-  modified_goal_publisher_ = create_publisher<PoseWithUuidStamped>("~/output/modified_goal", 1);
+  const auto durable_qos = rclcpp::QoS(1).transient_local();
+  modified_goal_publisher_ =
+    create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
   stop_reason_publisher_ = create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/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
@@ -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<HADMapBin>(
-    "input/vector_map", qos_transient_local,
+    "input/vector_map", durable_qos,
     std::bind(&MissionPlanner::onMap, this, std::placeholders::_1));
+  sub_modified_goal_ = create_subscription<PoseWithUuidStamped>(
+    "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<MarkerArray>("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
@@ -72,8 +72,10 @@ class MissionPlanner : public rclcpp::Node
 
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
   rclcpp::Subscription<HADMapBin>::SharedPtr vector_map_subscriber_;
+  rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_modified_goal_;
   Odometry::ConstSharedPtr odometry_;
   void on_odometry(const Odometry::ConstSharedPtr msg);
+  void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg);
 
   rclcpp::Publisher<MarkerArray>::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<ModifiedGoal>::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);