Port map loader (#44)

* port map_loader to ROS2 Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix unintended change Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Update map/map_loader/CMakeLists.txt Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
kenji-miyake · Oct 27, 2020 · b93a865 · b93a865
1 parent 280cdc7
commit b93a865
Show file tree

Hide file tree

Showing 10 changed files with 106 additions and 132 deletions.
diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt
@@ -1,69 +1,23 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(map_loader)
 
-add_compile_options(-std=c++14)
-
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  lanelet2_extension
-  pcl_ros
-  roscpp
-  std_msgs
-  tf2_geometry_msgs
-  tf2_ros
-  visualization_msgs
-)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 
+find_package(PCL REQUIRED COMPONENTS io)
 find_package(Boost REQUIRED COMPONENTS filesystem)
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS
-    geometry_msgs
-    std_msgs
-    tf2_geometry_msgs
-    visualization_msgs
-  DEPENDS
-    Boost
-)
-
-include_directories(
-  include
-  ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-add_executable(pointcloud_map_loader src/pointcloud_map_loader/main.cpp src/pointcloud_map_loader/pointcloud_map_loader_node.cpp)
-target_link_libraries(pointcloud_map_loader ${Boost_LIBRARIES} ${catkin_LIBRARIES})
-add_dependencies(pointcloud_map_loader ${catkin_EXPORTED_TARGETS})
+ament_auto_add_executable(pointcloud_map_loader src/pointcloud_map_loader/main.cpp src/pointcloud_map_loader/pointcloud_map_loader_node.cpp)
+target_link_libraries(pointcloud_map_loader ${Boost_LIBRARIES} ${PCL_LIBRARIES})
 
-add_executable(lanelet2_map_loader src/lanelet2_map_loader/lanelet2_map_loader.cpp)
-target_link_libraries(lanelet2_map_loader ${catkin_LIBRARIES})
-add_dependencies(lanelet2_map_loader ${catkin_EXPORTED_TARGETS})
+ament_auto_add_executable(lanelet2_map_loader src/lanelet2_map_loader/lanelet2_map_loader.cpp)
 
-add_executable(lanelet2_map_visualization src/lanelet2_map_loader/lanelet2_map_visualization.cpp)
-target_link_libraries(lanelet2_map_visualization ${catkin_LIBRARIES})
-add_dependencies(lanelet2_map_visualization ${catkin_EXPORTED_TARGETS})
-
-## Install executables and/or libraries
-install(
-  TARGETS
-    pointcloud_map_loader
-    lanelet2_map_loader
-    lanelet2_map_visualization
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
+ament_auto_add_executable(lanelet2_map_visualization src/lanelet2_map_loader/lanelet2_map_visualization.cpp)
 
-## Install files
-install(
-  DIRECTORY
-    launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_auto_package(INSTALL_TO_SHARE
+  launch
 )
diff --git a/map/map_loader/COLCON_IGNORE b/map/map_loader/COLCON_IGNORE
diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.h b/map/map_loader/include/map_loader/pointcloud_map_loader_node.h
@@ -33,20 +33,17 @@
 #include <string>
 #include <vector>
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
-#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
-class PointCloudMapLoaderNode
+class PointCloudMapLoaderNode : public rclcpp::Node
 {
 public:
   explicit PointCloudMapLoaderNode(const std::vector<std::string> & pcd_paths);
 
 private:
-  ros::NodeHandle nh_{""};
-  ros::NodeHandle private_nh_{"~"};
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pointcloud_map_;
 
-  ros::Publisher pub_pointcloud_map_;
-
-  sensor_msgs::PointCloud2 loadPCDFiles(const std::vector<std::string> & pcd_paths);
+  sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector<std::string> & pcd_paths);
 };
diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml
@@ -4,13 +4,13 @@
   <arg name="lanelet2_map_marker_topic" default="vector_map_marker"/>
   <arg name="center_line_resolution" default="5.0"/>
 
-  <node pkg="map_loader" type="lanelet2_map_loader" name="lanelet2_map_loader" args="$(arg file_name)">
-    <remap from="~output/lanelet2_map" to="$(arg lanelet2_map_topic)" />
-    <param name="center_line_resolution" value="$(arg center_line_resolution)" />
+  <node pkg="map_loader" exec="lanelet2_map_loader" name="lanelet2_map_loader" args="$(var file_name)">
+    <remap from="output/lanelet2_map" to="$(var lanelet2_map_topic)" />
+    <param name="center_line_resolution" value="$(var center_line_resolution)" />
   </node>
-  <node pkg="map_loader" type="lanelet2_map_visualization" name="lanelet2_map_visualization">
-    <remap from="~input/lanelet2_map" to="$(arg lanelet2_map_topic)" />
-    <remap from="~output/lanelet2_map_marker" to="$(arg lanelet2_map_marker_topic)" />
+  <node pkg="map_loader" exec="lanelet2_map_visualization" name="lanelet2_map_visualization">
+    <remap from="input/lanelet2_map" to="$(var lanelet2_map_topic)" />
+    <remap from="output/lanelet2_map_marker" to="$(var lanelet2_map_marker_topic)" />
   </node>
 
 </launch>
diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml
@@ -1,7 +1,7 @@
 <launch>
   <arg name="pointcloud_map_path" />
 
-  <node pkg="map_loader" type="pointcloud_map_loader" name="pointcloud_map_loader" args="$(arg pointcloud_map_path)">
-    <remap from="~output/pointcloud_map" to="/map/pointcloud_map" />
+  <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader" args="$(var pointcloud_map_path)">
+    <remap from="output/pointcloud_map" to="/map/pointcloud_map" />
   </node>
 </launch>
diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml
@@ -1,24 +1,27 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>map_loader</name>
   <version>0.1.0</version>
   <description>The map_loader package</description>
   <maintainer email="ryohsuke.mitsudome@tier4.jp">mitsudome-r</maintainer>
   <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
   <license>Apache 2</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-filesystem-dev</depend>
   <depend>libpcl-all-dev</depend>
-  <depend>pcl_ros</depend>
-  <depend>roscpp</depend>
+  <depend>pcl_conversions</depend>
   <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
-  <depend>tf</depend>
   <depend>visualization_msgs</depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader.cpp
@@ -17,7 +17,7 @@
  *
  */
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_io/Io.h>
@@ -28,31 +28,32 @@
 #include <lanelet2_extension/utility/message_conversion.h>
 #include <lanelet2_extension/utility/utilities.h>
 
-#include <autoware_lanelet2_msgs/MapBin.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
 
 #include <string>
 
-void printUsage()
+void printUsage(const rclcpp::Logger & logger)
 {
-  ROS_ERROR_STREAM("Usage:");
-  ROS_ERROR_STREAM("rosrun map_loader lanelet2_map_loader [.OSM]");
-  ROS_ERROR_STREAM(
-    "rosrun map_loader lanelet2_map_loader download [X] [Y]: WARNING not implemented");
+  RCLCPP_ERROR_STREAM(
+    logger,
+    "Usage:" << std::endl
+             << "rosrun map_loader lanelet2_map_loader [.OSM]" << std::endl
+             << "rosrun map_loader lanelet2_map_loader download [X] [Y]: WARNING not implemented");
 }
 
 int main(int argc, char ** argv)
 {
-  ros::init(argc, argv, "lanelet_map_loader");
-  ros::NodeHandle pnh("~");
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("lanelet_map_loader");
 
   if (argc < 2) {
-    printUsage();
+    printUsage(node->get_logger());
     return EXIT_FAILURE;
   }
 
   std::string mode(argv[1]);
   if (mode == "download" && argc < 4) {
-    printUsage();
+    printUsage(node->get_logger());
     return EXIT_FAILURE;
   }
 
@@ -64,32 +65,36 @@ int main(int argc, char ** argv)
   lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
 
   for (const auto & error : errors) {
-    ROS_ERROR_STREAM(error);
+    RCLCPP_ERROR_STREAM(node->get_logger(), error);
   }
   if (!errors.empty()) {
     return EXIT_FAILURE;
   }
 
   double center_line_resolution;
-  pnh.param<double>("center_line_resolution", center_line_resolution, 5.0);
+  center_line_resolution = node->declare_parameter("center_line_resolution", 5.0);
   lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
 
   std::string format_version, map_version;
   lanelet::io_handlers::AutowareOsmParser::parseVersions(
     lanelet2_filename, &format_version, &map_version);
 
-  ros::Publisher map_bin_pub =
-    pnh.advertise<autoware_lanelet2_msgs::MapBin>("output/lanelet2_map", 1, true);
-  autoware_lanelet2_msgs::MapBin map_bin_msg;
-  map_bin_msg.header.stamp = ros::Time::now();
+  rclcpp::QoS durable_qos{1};
+  durable_qos.transient_local();
+  const auto map_bin_pub =
+    node->create_publisher<autoware_lanelet2_msgs::msg::MapBin>("output/lanelet2_map", durable_qos);
+  autoware_lanelet2_msgs::msg::MapBin map_bin_msg;
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  map_bin_msg.header.stamp = ros_clock.now();
   map_bin_msg.header.frame_id = "map";
   map_bin_msg.format_version = format_version;
   map_bin_msg.map_version = map_version;
   lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
 
-  map_bin_pub.publish(map_bin_msg);
+  map_bin_pub->publish(map_bin_msg);
 
-  ros::spin();
+  rclcpp::spin(node);
 
+  rclcpp::shutdown();
   return 0;
 }
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization.cpp
@@ -17,13 +17,13 @@
  *
  */
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
-#include <visualization_msgs/MarkerArray.h>
-
-#include <autoware_lanelet2_msgs/MapBin.h>
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_projection/UTM.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
 #include <lanelet2_extension/utility/message_conversion.h>
@@ -33,28 +33,29 @@
 #include <vector>
 
 static bool g_viz_lanelets_centerline = true;
-static ros::Publisher g_map_pub;
+static rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr g_map_pub;
+static std::shared_ptr<rclcpp::Logger> g_logger;
 
 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());
 }
 
-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;
   cl->b = b;
   cl->a = a;
 }
 
-void binMapCallback(autoware_lanelet2_msgs::MapBin msg)
+void binMapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr msg)
 {
   lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap);
 
-  lanelet::utils::conversion::fromBinMsg(msg, viz_lanelet_map);
-  ROS_INFO("Map is loaded\n");
+  lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map);
+  RCLCPP_INFO((*g_logger), "Map is loaded\n");
 
   // get lanelets etc to visualize
   lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map);
@@ -75,7 +76,7 @@ void binMapCallback(autoware_lanelet2_msgs::MapBin msg)
     lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map);
   lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map);
 
-  std_msgs::ColorRGBA cl_road, cl_cross, cl_ll_borders, cl_stoplines, cl_trafficlights,
+  std_msgs::msg::ColorRGBA cl_road, cl_cross, cl_ll_borders, cl_stoplines, cl_trafficlights,
     cl_detection_areas, cl_parking_lots, cl_parking_spaces;
   setColor(&cl_road, 0.2, 0.7, 0.7, 0.3);
   setColor(&cl_cross, 0.2, 0.7, 0.2, 0.3);
@@ -86,7 +87,7 @@ void binMapCallback(autoware_lanelet2_msgs::MapBin msg)
   setColor(&cl_parking_lots, 0.7, 0.7, 0.0, 0.3);
   setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6);
 
-  visualization_msgs::MarkerArray map_marker_array;
+  visualization_msgs::msg::MarkerArray map_marker_array;
 
   insertMarkerArray(
     &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray(
@@ -118,19 +119,24 @@ void binMapCallback(autoware_lanelet2_msgs::MapBin msg)
     &map_marker_array,
     lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces));
 
-  g_map_pub.publish(map_marker_array);
+  g_map_pub->publish(map_marker_array);
 }
 
 int main(int argc, char ** argv)
 {
-  ros::init(argc, argv, "lanelet2_map_visualizer");
-  ros::NodeHandle pnh("~");
-  ros::Subscriber bin_map_sub;
-
-  bin_map_sub = pnh.subscribe("input/lanelet2_map", 1, binMapCallback);
-  g_map_pub = pnh.advertise<visualization_msgs::MarkerArray>("output/lanelet2_map_marker", 1, true);
-
-  ros::spin();
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("lanelet2_map_visualizer");
+  auto bin_map_sub = node->create_subscription<autoware_lanelet2_msgs::msg::MapBin>(
+    "input/lanelet2_map", rclcpp::QoS{1}, std::bind(&binMapCallback, std::placeholders::_1));
+
+  rclcpp::QoS durable_qos{1};
+  durable_qos.transient_local();
+  g_map_pub = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+    "output/lanelet2_map_marker", durable_qos);
+
+  g_logger = std::make_shared<rclcpp::Logger>(node->get_logger());
+  rclcpp::spin(node);
+  rclcpp::shutdown();
 
   return 0;
 }