port mission_planner to ROS2 (autowarefoundation#56)

* port mission_planner to ROS2

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix QoS for publishers

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

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
 )

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);

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

planning/mission_planner/include/mission_planner/mission_planner_base.h

@@ -18,54 +18,53 @@
 #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>
 #include <vector>
 
 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);
 };
 

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>

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!");
   }
 }