diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt
index f1edfc977eb64..8ae2eb7dfca2d 100644
--- a/planning/mission_planner/CMakeLists.txt
+++ b/planning/mission_planner/CMakeLists.txt
@@ -1,50 +1,26 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(mission_planner)
 
-add_compile_options(-std=c++14)
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wno-unused-parameter)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  autoware_planning_msgs
-  geometry_msgs
-  lanelet2_extension
-  roscpp
-  tf2_ros
-  tf2_geometry_msgs
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS autoware_planning_msgs lanelet2_extension roscpp tf2_ros
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-add_executable(mission_planner
+ament_auto_add_executable(mission_planner
   lib/mission_planner_base.cpp
   src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
   src/mission_planner_lanelet2/route_handler.cpp
   src/mission_planner_lanelet2/mission_planner_main.cpp
   src/mission_planner_lanelet2/utility_functions.cpp
 )
-add_dependencies(mission_planner
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-target_link_libraries(mission_planner
-  ${catkin_LIBRARIES}
-)
-
-install(TARGETS mission_planner
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
 
-install(
-  DIRECTORY
-    launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_auto_package(INSTALL_TO_SHARE
+  launch
 )
diff --git a/planning/mission_planner/COLCON_IGNORE b/planning/mission_planner/COLCON_IGNORE
deleted file mode 100644
index e69de29bb2d1d..0000000000000
diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.h b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.h
index 4e301f159a2d0..b52039308b7a0 100644
--- a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.h
+++ b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.h
@@ -18,11 +18,11 @@
 #define MISSION_PLANNER_LANELET2_IMPL_MISSION_PLANNER_LANELET2_H
 
 // ROS
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <tf2_ros/transform_listener.h>
 
 // Autoware
-#include <autoware_lanelet2_msgs/MapBin.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
 #include <mission_planner/lanelet2_impl/route_handler.h>
 #include <mission_planner/mission_planner_base.h>
 
@@ -34,7 +34,7 @@
 // others
 #include <string>
 
-using RouteSections = std::vector<autoware_planning_msgs::RouteSection>;
+using RouteSections = std::vector<autoware_planning_msgs::msg::RouteSection>;
 
 namespace mission_planner
 {
@@ -50,20 +50,20 @@ class MissionPlannerLanelet2 : public MissionPlanner
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
 
-  ros::Subscriber map_subscriber_;
+  rclcpp::Subscription<autoware_lanelet2_msgs::msg::MapBin>::SharedPtr map_subscriber_;
 
-  void mapCallback(const autoware_lanelet2_msgs::MapBin & msg);
+  void mapCallback(const autoware_lanelet2_msgs::msg::MapBin::ConstSharedPtr msg);
   bool isGoalValid() const;
 
   // virtual functions
   bool isRoutingGraphReady() const;
-  autoware_planning_msgs::Route planRoute();
-  void visualizeRoute(const autoware_planning_msgs::Route & route) const;
+  autoware_planning_msgs::msg::Route planRoute();
+  void visualizeRoute(const autoware_planning_msgs::msg::Route & route) const;
 
   // routing
   bool planPathBetweenCheckpoints(
-    const geometry_msgs::PoseStamped & start_checkpoint,
-    const geometry_msgs::PoseStamped & goal_checkpoint,
+    const geometry_msgs::msg::PoseStamped & start_checkpoint,
+    const geometry_msgs::msg::PoseStamped & goal_checkpoint,
     lanelet::ConstLanelets * path_lanelets_ptr) const;
   lanelet::ConstLanelets getMainLanelets(
     const lanelet::ConstLanelets & path_lanelets, const RouteHandler & lanelet_sequence_finder);
diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.h b/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.h
index fb5ff62d688c3..b97460757ed15 100644
--- a/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.h
+++ b/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.h
@@ -18,13 +18,15 @@
 #define MISSION_PLANNER_LANELET2_IMPL_UTILITY_FUNCTIONS_H
 #include <string>
 
-#include <geometry_msgs/Pose.h>
-#include <std_msgs/ColorRGBA.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/LaneletSequence.h>
 
+#include <rclcpp/rclcpp.hpp>
+
 #include <unordered_set>
 #include <vector>
 bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id);
@@ -40,11 +42,11 @@ bool exists(const std::vector<T> & vectors, const T & item)
   return false;
 }
 
-void setColor(std_msgs::ColorRGBA * cl, double r, double g, double b, double a);
+void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a);
 void insertMarkerArray(
-  visualization_msgs::MarkerArray * a1, const visualization_msgs::MarkerArray & a2);
-std::string toString(const geometry_msgs::Pose & pose);
+  visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
+std::string toString(const geometry_msgs::msg::Pose & pose);
 bool getClosestLanelet(
-  const geometry_msgs::Pose & search_pose, const lanelet::LaneletMapPtr & lanelet_map,
-  lanelet::Lanelet * closest_lanelet, double distance_thresh = 10.0);
+  const geometry_msgs::msg::Pose & search_pose, const lanelet::LaneletMapPtr & lanelet_map,
+  lanelet::Lanelet * closest_lanelet, const rclcpp::Logger & logger, double distance_thresh = 10.0);
 #endif  // MISSION_PLANNER_LANELET2_IMPL_UTILITY_FUNCTIONS_H
diff --git a/planning/mission_planner/include/mission_planner/mission_planner_base.h b/planning/mission_planner/include/mission_planner/mission_planner_base.h
index b57b1add270cd..b95106347f03e 100644
--- a/planning/mission_planner/include/mission_planner/mission_planner_base.h
+++ b/planning/mission_planner/include/mission_planner/mission_planner_base.h
@@ -18,13 +18,14 @@
 #define MISSION_PLANNER_MISSION_PLANNER_BASE_H
 
 // ROS
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <tf2_ros/transform_listener.h>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 // Autoware
-#include <autoware_planning_msgs/Route.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <autoware_planning_msgs/msg/route.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 
 // others
 #include <string>
@@ -32,40 +33,38 @@
 
 namespace mission_planner
 {
-class MissionPlanner
+class MissionPlanner : public rclcpp::Node
 {
 protected:
-  MissionPlanner();
+  MissionPlanner(const std::string & node_name);
 
-  geometry_msgs::PoseStamped goal_pose_;
-  geometry_msgs::PoseStamped start_pose_;
-  std::vector<geometry_msgs::PoseStamped> checkpoints_;
+  geometry_msgs::msg::PoseStamped goal_pose_;
+  geometry_msgs::msg::PoseStamped start_pose_;
+  std::vector<geometry_msgs::msg::PoseStamped> checkpoints_;
 
   std::string base_link_frame_;
   std::string map_frame_;
 
-  ros::NodeHandle pnh_;
-
-  ros::Publisher marker_publisher_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
 
   virtual bool isRoutingGraphReady() const = 0;
-  virtual autoware_planning_msgs::Route planRoute() = 0;
-  virtual void visualizeRoute(const autoware_planning_msgs::Route & route) const = 0;
-  virtual void publishRoute(const autoware_planning_msgs::Route & route) const;
+  virtual autoware_planning_msgs::msg::Route planRoute() = 0;
+  virtual void visualizeRoute(const autoware_planning_msgs::msg::Route & route) const = 0;
+  virtual void publishRoute(const autoware_planning_msgs::msg::Route & route) const;
 
 private:
-  ros::Publisher route_publisher_;
-  ros::Subscriber goal_subscriber_;
-  ros::Subscriber checkpoint_subscriber_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::Route>::SharedPtr route_publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_subscriber_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr checkpoint_subscriber_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  bool getEgoVehiclePose(geometry_msgs::PoseStamped * ego_vehicle_pose);
-  void goalPoseCallback(const geometry_msgs::PoseStampedConstPtr & goal_msg_ptr);
-  void checkpointCallback(const geometry_msgs::PoseStampedConstPtr & checkpoint_msg_ptr);
+  bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
+  void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr);
+  void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr);
   bool transformPose(
-    const geometry_msgs::PoseStamped & input_pose, geometry_msgs::PoseStamped * output_pose,
+    const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose,
     const std::string target_frame);
 };
 
diff --git a/planning/mission_planner/launch/mission_planning.launch.xml b/planning/mission_planner/launch/mission_planning.launch.xml
index 2b49a5cbd1241..1811190735019 100644
--- a/planning/mission_planner/launch/mission_planning.launch.xml
+++ b/planning/mission_planner/launch/mission_planning.launch.xml
@@ -5,13 +5,13 @@
   <arg name="map_topic_name" default="/map/vector_map" />
   <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker" />
 
-  <node pkg="mission_planner" type="mission_planner" name="mission_planner" output="screen">
+  <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
     <param name="map_frame" value="map" />
     <param name="base_link_frame" value="base_link" />
-    <remap from="~input/vector_map" to="$(arg map_topic_name)" />
-    <remap from="~input/goal_pose" to="$(arg goal_topic_name)" />
-    <remap from="~input/checkpoint" to="$(arg checkpoint_topic_name)" />
-    <remap from="~output/route" to="$(arg rout_topic_name)" />
-    <remap from="~debug/route_marker" to="$(arg visualization_topic_name)" />
+    <remap from="input/vector_map" to="$(var map_topic_name)" />
+    <remap from="input/goal_pose" to="$(var goal_topic_name)" />
+    <remap from="input/checkpoint" to="$(var checkpoint_topic_name)" />
+    <remap from="output/route" to="$(var rout_topic_name)" />
+    <remap from="debug/route_marker" to="$(var visualization_topic_name)" />
   </node>
 </launch>
\ No newline at end of file
diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp
index 93d929e8675b0..5405687814d7e 100644
--- a/planning/mission_planner/lib/mission_planner_base.cpp
+++ b/planning/mission_planner/lib/mission_planner_base.cpp
@@ -16,7 +16,7 @@
 #include <mission_planner/mission_planner_base.h>
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker_array.h>
 
 #include <lanelet2_routing/Route.h>
 
@@ -26,23 +26,29 @@
 
 namespace mission_planner
 {
-MissionPlanner::MissionPlanner() : pnh_("~"), tf_listener_(tf_buffer_)
+MissionPlanner::MissionPlanner(const std::string & node_name)
+: Node(node_name), tf_buffer_(get_clock()), tf_listener_(tf_buffer_)
 {
-  pnh_.param<std::string>("map_frame", map_frame_, "map");
-  pnh_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
+  map_frame_ = declare_parameter("map_frame", "map");
+  base_link_frame_ = declare_parameter("base_link_frame", "base_link");
 
-  goal_subscriber_ = pnh_.subscribe("input/goal_pose", 10, &MissionPlanner::goalPoseCallback, this);
-  checkpoint_subscriber_ =
-    pnh_.subscribe("input/checkpoint", 10, &MissionPlanner::checkpointCallback, this);
+  using std::placeholders::_1;
 
-  route_publisher_ = pnh_.advertise<autoware_planning_msgs::Route>("output/route", 1, true);
+  goal_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
+    "input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1));
+  checkpoint_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
+    "input/checkpoint", 10, std::bind(&MissionPlanner::checkpointCallback, this, _1));
+
+  rclcpp::QoS durable_qos{1};
+  durable_qos.transient_local();
+  route_publisher_ = create_publisher<autoware_planning_msgs::msg::Route>("output/route", durable_qos);
   marker_publisher_ =
-    pnh_.advertise<visualization_msgs::MarkerArray>("debug/route_marker", 1, true);
+    create_publisher<visualization_msgs::msg::MarkerArray>("debug/route_marker", durable_qos);
 }
 
-bool MissionPlanner::getEgoVehiclePose(geometry_msgs::PoseStamped * ego_vehicle_pose)
+bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose)
 {
-  geometry_msgs::PoseStamped base_link_origin;
+  geometry_msgs::msg::PoseStamped base_link_origin;
   base_link_origin.header.frame_id = base_link_frame_;
   base_link_origin.pose.position.x = 0;
   base_link_origin.pose.position.y = 0;
@@ -57,76 +63,77 @@ bool MissionPlanner::getEgoVehiclePose(geometry_msgs::PoseStamped * ego_vehicle_
 }
 
 bool MissionPlanner::transformPose(
-  const geometry_msgs::PoseStamped & input_pose, geometry_msgs::PoseStamped * output_pose,
+  const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose,
   const std::string target_frame)
 {
-  geometry_msgs::TransformStamped transform;
+  geometry_msgs::msg::TransformStamped transform;
   try {
-    transform = tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, ros::Time(0));
+    transform = tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero);
     tf2::doTransform(input_pose, *output_pose, transform);
     return true;
   } catch (tf2::TransformException & ex) {
-    ROS_WARN("%s", ex.what());
+    RCLCPP_WARN(get_logger(), "%s", ex.what());
     return false;
   }
 }
 
-void MissionPlanner::goalPoseCallback(const geometry_msgs::PoseStampedConstPtr & goal_msg_ptr)
+void MissionPlanner::goalPoseCallback(
+  const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr)
 {
   // set start pose
   if (!getEgoVehiclePose(&start_pose_)) {
-    ROS_ERROR("Failed to get ego vehicle pose in map frame. Aborting mission planning");
+    RCLCPP_ERROR(get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning");
     return;
   }
   // set goal pose
   if (!transformPose(*goal_msg_ptr, &goal_pose_, map_frame_)) {
-    ROS_ERROR("Failed to get goal pose in map frame. Aborting mission planning");
+    RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning");
     return;
   }
 
-  ROS_INFO("New goal pose is set. Reset checkpoints.");
+  RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints.");
   checkpoints_.clear();
   checkpoints_.push_back(start_pose_);
   checkpoints_.push_back(goal_pose_);
 
   if (!isRoutingGraphReady()) {
-    ROS_ERROR("RoutingGraph is not ready. Aborting mission planning");
+    RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning");
     return;
   }
 
-  autoware_planning_msgs::Route route = planRoute();
+  autoware_planning_msgs::msg::Route route = planRoute();
   publishRoute(route);
 }  // namespace mission_planner
 
 void MissionPlanner::checkpointCallback(
-  const geometry_msgs::PoseStampedConstPtr & checkpoint_msg_ptr)
+  const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr)
 {
   if (checkpoints_.size() < 2) {
-    ROS_ERROR("You must set start and goal before setting checkpoints. Aborting mission planning");
+    RCLCPP_ERROR(get_logger(), "You must set start and goal before setting checkpoints. Aborting mission planning");
     return;
   }
 
-  geometry_msgs::PoseStamped transformed_checkpoint;
+  geometry_msgs::msg::PoseStamped transformed_checkpoint;
   if (!transformPose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) {
-    ROS_ERROR("Failed to get checkpoint pose in map frame. Aborting mission planning");
+    RCLCPP_ERROR(get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning");
     return;
   }
 
   // insert checkpoint before goal
   checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint);
 
-  autoware_planning_msgs::Route route = planRoute();
+  autoware_planning_msgs::msg::Route route = planRoute();
   publishRoute(route);
 }
 
-void MissionPlanner::publishRoute(const autoware_planning_msgs::Route & route) const
+void MissionPlanner::publishRoute(const autoware_planning_msgs::msg::Route & route) const
 {
   if (!route.route_sections.empty()) {
-    ROS_INFO("Route successfuly planned. Publishing...");
-    route_publisher_.publish(route);
+    RCLCPP_INFO(get_logger(), "Route successfuly planned. Publishing...");
+    route_publisher_->publish(route);
     visualizeRoute(route);
   } else {
-    ROS_ERROR("Calculated route is empty!");
+    RCLCPP_ERROR(get_logger(), "Calculated route is empty!");
   }
 }
 
diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml
index e61cfd9b3b91e..af694b93438c9 100644
--- a/planning/mission_planner/package.xml
+++ b/planning/mission_planner/package.xml
@@ -8,13 +8,16 @@
 
   <license>Apache 2</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <depend>autoware_planning_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
-  <depend>roscpp</depend>
+  <depend>rclcpp</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
index cb1f21a1c3d5d..4b05baf99889c 100644
--- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
+++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
@@ -20,7 +20,6 @@
 
 #include <tf2/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <visualization_msgs/MarkerArray.h>
 
 #include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_routing/Route.h>
@@ -123,23 +122,24 @@ bool isInParkingLot(
 
 namespace mission_planner
 {
-MissionPlannerLanelet2::MissionPlannerLanelet2() : is_graph_ready_(false)
+MissionPlannerLanelet2::MissionPlannerLanelet2()
+: MissionPlanner("mission_planner_node"), is_graph_ready_(false)
 {
-  map_subscriber_ =
-    pnh_.subscribe("input/vector_map", 10, &MissionPlannerLanelet2::mapCallback, this);
+  using std::placeholders::_1;
+  map_subscriber_ = create_subscription<autoware_lanelet2_msgs::msg::MapBin>("input/vector_map", 10, std::bind(&MissionPlannerLanelet2::mapCallback, this, _1));
 }
 
-void MissionPlannerLanelet2::mapCallback(const autoware_lanelet2_msgs::MapBin & msg)
+void MissionPlannerLanelet2::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::ConstSharedPtr msg)
 {
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
-    msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
+    *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
   is_graph_ready_ = true;
 }
 
 bool MissionPlannerLanelet2::isRoutingGraphReady() const { return (is_graph_ready_); }
 
-void MissionPlannerLanelet2::visualizeRoute(const autoware_planning_msgs::Route & route) const
+void MissionPlannerLanelet2::visualizeRoute(const autoware_planning_msgs::msg::Route & route) const
 {
   lanelet::ConstLanelets route_lanelets;
   lanelet::ConstLanelets end_lanelets;
@@ -160,14 +160,14 @@ void MissionPlannerLanelet2::visualizeRoute(const autoware_planning_msgs::Route
     }
   }
 
-  std_msgs::ColorRGBA cl_route, cl_ll_borders, cl_end, cl_normal, cl_goal;
+  std_msgs::msg::ColorRGBA cl_route, cl_ll_borders, cl_end, cl_normal, cl_goal;
   setColor(&cl_route, 0.0, 0.7, 0.2, 0.2);
   setColor(&cl_goal, 0.0, 0.7, 0.7, 0.2);
   setColor(&cl_end, 0.0, 0.2, 0.7, 0.2);
   setColor(&cl_normal, 0.0, 0.7, 0.2, 0.2);
   setColor(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999);
 
-  visualization_msgs::MarkerArray route_marker_array;
+  visualization_msgs::msg::MarkerArray route_marker_array;
   insertMarkerArray(
     &route_marker_array,
     lanelet::visualization::laneletsBoundaryAsMarkerArray(route_lanelets, cl_ll_borders, false));
@@ -183,13 +183,13 @@ void MissionPlannerLanelet2::visualizeRoute(const autoware_planning_msgs::Route
   insertMarkerArray(
     &route_marker_array,
     lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal));
-  marker_publisher_.publish(route_marker_array);
+  marker_publisher_->publish(route_marker_array);
 }
 
 bool MissionPlannerLanelet2::isGoalValid() const
 {
   lanelet::Lanelet closest_lanelet;
-  if (!getClosestLanelet(goal_pose_.pose, lanelet_map_ptr_, &closest_lanelet)) {
+  if (!getClosestLanelet(goal_pose_.pose, lanelet_map_ptr_, &closest_lanelet, get_logger())) {
     return false;
   }
   const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position);
@@ -222,20 +222,22 @@ bool MissionPlannerLanelet2::isGoalValid() const
   return false;
 }
 
-autoware_planning_msgs::Route MissionPlannerLanelet2::planRoute()
+autoware_planning_msgs::msg::Route MissionPlannerLanelet2::planRoute()
 {
   std::stringstream ss;
   for (const auto & checkpoint : checkpoints_) {
     ss << "x: " << checkpoint.pose.position.x << " "
        << "y: " << checkpoint.pose.position.y << std::endl;
   }
-  ROS_INFO_STREAM("start planning route with checkpoints: " << std::endl << ss.str());
+  RCLCPP_INFO_STREAM(
+    get_logger(), "start planning route with checkpoints: " << std::endl
+                                                            << ss.str());
 
-  autoware_planning_msgs::Route route_msg;
+  autoware_planning_msgs::msg::Route route_msg;
   RouteSections route_sections;
 
   if (!isGoalValid()) {
-    ROS_WARN("Goal is not valid! Please check position and angle of goal_pose");
+    RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose");
     return route_msg;
   }
 
@@ -256,10 +258,11 @@ autoware_planning_msgs::Route MissionPlannerLanelet2::planRoute()
   }
 
   if (isRouteLooped(route_sections)) {
-    ROS_WARN("Loop detected within route! Be aware that looped route is not debugged!");
+    RCLCPP_WARN(
+      get_logger(), "Loop detected within route! Be aware that looped route is not debugged!");
   }
 
-  route_msg.header.stamp = ros::Time::now();
+  route_msg.header.stamp = this->now();
   route_msg.header.frame_id = map_frame_;
   route_msg.route_sections = route_sections;
   route_msg.goal_pose = goal_pose_.pose;
@@ -268,16 +271,16 @@ autoware_planning_msgs::Route MissionPlannerLanelet2::planRoute()
 }
 
 bool MissionPlannerLanelet2::planPathBetweenCheckpoints(
-  const geometry_msgs::PoseStamped & start_checkpoint,
-  const geometry_msgs::PoseStamped & goal_checkpoint,
+  const geometry_msgs::msg::PoseStamped & start_checkpoint,
+  const geometry_msgs::msg::PoseStamped & goal_checkpoint,
   lanelet::ConstLanelets * path_lanelets_ptr) const
 {
   lanelet::Lanelet start_lanelet;
-  if (!getClosestLanelet(start_checkpoint.pose, lanelet_map_ptr_, &start_lanelet)) {
+  if (!getClosestLanelet(start_checkpoint.pose, lanelet_map_ptr_, &start_lanelet, get_logger())) {
     return false;
   }
   lanelet::Lanelet goal_lanelet;
-  if (!getClosestLanelet(goal_checkpoint.pose, lanelet_map_ptr_, &goal_lanelet)) {
+  if (!getClosestLanelet(goal_checkpoint.pose, lanelet_map_ptr_, &goal_lanelet, get_logger())) {
     return false;
   }
 
@@ -285,13 +288,13 @@ bool MissionPlannerLanelet2::planPathBetweenCheckpoints(
   lanelet::Optional<lanelet::routing::Route> optional_route =
     routing_graph_ptr_->getRoute(start_lanelet, goal_lanelet, 0);
   if (!optional_route) {
-    ROS_ERROR_STREAM(
-      "Failed to find a proper path!"
-      << std::endl
-      << "start checkpoint: " << toString(start_pose_.pose) << std::endl
-      << "goal checkpoint: " << toString(goal_pose_.pose) << std::endl
-      << "start lane id: " << start_lanelet.id() << std::endl
-      << "goal lane id: " << goal_lanelet.id() << std::endl);
+    RCLCPP_ERROR_STREAM(
+      get_logger(), "Failed to find a proper path!"
+                      << std::endl
+                      << "start checkpoint: " << toString(start_pose_.pose) << std::endl
+                      << "goal checkpoint: " << toString(goal_pose_.pose) << std::endl
+                      << "start lane id: " << start_lanelet.id() << std::endl
+                      << "goal lane id: " << goal_lanelet.id() << std::endl);
     return false;
   }
 
@@ -322,7 +325,7 @@ RouteSections MissionPlannerLanelet2::createRouteSections(
   if (main_path.empty()) return route_sections;
 
   for (const auto & main_llt : main_path) {
-    autoware_planning_msgs::RouteSection route_section_msg;
+    autoware_planning_msgs::msg::RouteSection route_section_msg;
     lanelet::ConstLanelets route_section_lanelets = route_handler.getNeighborsWithinRoute(main_llt);
     route_section_msg.preferred_lane_id = main_llt.id();
     for (const auto & section_llt : route_section_lanelets) {
diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_main.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_main.cpp
index bbaabb1ca49b3..cda25c19217fd 100644
--- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_main.cpp
+++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_main.cpp
@@ -15,15 +15,17 @@
  */
 
 #include <mission_planner/lanelet2_impl/mission_planner_lanelet2.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 int main(int argc, char ** argv)
 {
-  ros::init(argc, argv, "mission_planner_node");
+  rclcpp::init(argc, argv);
 
-  mission_planner::MissionPlannerLanelet2 mission_planner;
+  auto node = std::make_shared<mission_planner::MissionPlannerLanelet2>();
 
-  ros::spin();
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
 
   return 0;
 }
diff --git a/planning/mission_planner/src/mission_planner_lanelet2/route_handler.cpp b/planning/mission_planner/src/mission_planner_lanelet2/route_handler.cpp
index cbdbae2036f78..ea27875db7f83 100644
--- a/planning/mission_planner/src/mission_planner_lanelet2/route_handler.cpp
+++ b/planning/mission_planner/src/mission_planner_lanelet2/route_handler.cpp
@@ -15,7 +15,7 @@
  */
 
 #include <mission_planner/lanelet2_impl/route_handler.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 namespace mission_planner
 {
@@ -143,7 +143,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter(
   lanelet_sequence_forward.push_back(lanelet);
 
   lanelet::ConstLanelet current_lanelet = lanelet;
-  while (ros::ok()) {
+  while (rclcpp::ok()) {
     lanelet::ConstLanelet next_lanelet;
     if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) {
       break;
@@ -164,7 +164,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
   }
 
   lanelet::ConstLanelet current_lanelet = lanelet;
-  while (ros::ok()) {
+  while (rclcpp::ok()) {
     lanelet::ConstLanelet prev_lanelet;
     if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) {
       break;
@@ -190,7 +190,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(const lanelet::ConstLane
   lanelet_sequence_backward = getLaneletSequenceUpTo(lanelet);
 
   // loop check
-  if (lanelet_sequence_forward.empty() > 1 && lanelet_sequence_backward.empty() > 1) {
+  if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) {
     if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) {
       return lanelet_sequence_forward;
     }
@@ -254,7 +254,7 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo(
   }
 
   lanelet::ConstLanelet current_lanelet = lanelet;
-  while (ros::ok()) {
+  while (rclcpp::ok()) {
     lanelet::ConstLanelet prev_lanelet;
     if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) {
       break;
@@ -283,7 +283,7 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceAfter(
   lane_sequence_forward.push_back(lanelet);
 
   lanelet::ConstLanelet current_lanelet = lanelet;
-  while (ros::ok()) {
+  while (rclcpp::ok()) {
     lanelet::ConstLanelet next_lanelet;
     if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) {
       break;
diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp
index b0eb6d2e0f8e7..4d76211bc38d2 100644
--- a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp
+++ b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp
@@ -16,21 +16,21 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 #include <mission_planner/lanelet2_impl/utility_functions.h>
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id)
 {
   return set.find(id) != set.end();
 }
 
-std::string toString(const geometry_msgs::Pose & pose)
+std::string toString(const geometry_msgs::msg::Pose & pose)
 {
   std::stringstream ss;
   ss << "(" << pose.position.x << ", " << pose.position.y << "," << pose.position.z << ")";
   return ss.str();
 }
 
-void setColor(std_msgs::ColorRGBA * cl, double r, double g, double b, double a)
+void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a)
 {
   cl->r = r;
   cl->g = g;
@@ -39,7 +39,7 @@ void setColor(std_msgs::ColorRGBA * cl, double r, double g, double b, double a)
 }
 
 void insertMarkerArray(
-  visualization_msgs::MarkerArray * a1, const visualization_msgs::MarkerArray & a2)
+  visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
 {
   a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
 }
@@ -62,8 +62,8 @@ std::vector<std::pair<double, lanelet::Lanelet>> excludeSubtypeLaneletsWithDista
 }
 
 bool getClosestLanelet(
-  const geometry_msgs::Pose & search_pose, const lanelet::LaneletMapPtr & lanelet_map_ptr_,
-  lanelet::Lanelet * closest_lanelet, double distance_thresh)
+  const geometry_msgs::msg::Pose & search_pose, const lanelet::LaneletMapPtr & lanelet_map_ptr_,
+  lanelet::Lanelet * closest_lanelet, const rclcpp::Logger & logger, double distance_thresh)
 {
   lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
   std::vector<std::pair<double, lanelet::Lanelet>> nearest_lanelet =
@@ -71,13 +71,13 @@ bool getClosestLanelet(
   const auto nearest_road_lanelet =
     excludeSubtypeLaneletsWithDistance(nearest_lanelet, lanelet::AttributeValueString::Crosswalk);
   if (nearest_road_lanelet.empty()) {
-    ROS_ERROR_STREAM(
+    RCLCPP_ERROR_STREAM(logger,
       "Failed to find the closest lane!" << std::endl
                                          << "search point: " << toString(search_pose) << std::endl);
     return false;
   }
   if (nearest_road_lanelet.front().first > distance_thresh) {
-    ROS_ERROR_STREAM(
+    RCLCPP_ERROR_STREAM(logger,
       "Closest lane is too far away!" << std::endl
                                       << "search point: " << toString(search_pose) << std::endl
                                       << "lane id: " << nearest_lanelet.front().second.id());