fix(mission_planner): fix initialize function (autowarefoundation#2566)

* fix(mission_planner): fix initialize function Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
sujithvemi · Dec 26, 2022 · fe12eb8 · fe12eb8
1 parent 5070130
commit fe12eb8
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -107,13 +107,10 @@ double project_goal_to_map(
 namespace mission_planner::lanelet2
 {
 
-void DefaultPlanner::initialize(rclcpp::Node * node)
+void DefaultPlanner::initialize_common(rclcpp::Node * node)
 {
   is_graph_ready_ = false;
   node_ = node;
-  map_subscriber_ = node_->create_subscription<HADMapBin>(
-    "input/vector_map", rclcpp::QoS{10}.transient_local(),
-    std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1));
 
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   pub_goal_footprint_marker_ =
@@ -123,10 +120,17 @@ void DefaultPlanner::initialize(rclcpp::Node * node)
   param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0);
 }
 
+void DefaultPlanner::initialize(rclcpp::Node * node)
+{
+  initialize_common(node);
+  map_subscriber_ = node_->create_subscription<HADMapBin>(
+    "input/vector_map", rclcpp::QoS{10}.transient_local(),
+    std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1));
+}
+
 void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg)
 {
-  is_graph_ready_ = false;
-  node_ = node;
+  initialize_common(node);
   map_callback(msg);
 }
 

diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
@@ -66,6 +66,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   rclcpp::Subscription<HADMapBin>::SharedPtr map_subscriber_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_goal_footprint_marker_;
 
+  void initialize_common(rclcpp::Node * node);
   void map_callback(const HADMapBin::ConstSharedPtr msg);
   bool check_goal_footprint(
     const lanelet::ConstLanelet & current_lanelet,