diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 1d37b820efff1..08da1260b000c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + odometry_(nullptr), + map_ptr_(nullptr), reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) @@ -86,7 +88,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - odometry_ = nullptr; sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); @@ -97,9 +98,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + sub_vector_map_ = create_subscription( "input/vector_map", qos_transient_local, - std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -141,7 +142,7 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha reroute_availability_ = msg; } -void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 7b0427f6d01b1..fd1b317970749 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -73,11 +73,13 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; @@ -126,9 +128,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - 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(