Ros2 port traffic light map (#99)

* remove colcon ignore

* ported CMakelists

* port package

* remove colcon ignore

* ported CMakelists

* port package

* ported traffic_light_map_detector to ROS2 - compiling

* port launch and config files, tidied up

* change rclcpp duration arguments

* launch file corrections

* lookupTransform uses exact timestamp

perception/traffic_light_map_based_detector/CMakeLists.txt

@@ -1,69 +1,36 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(traffic_light_map_based_detector)
 
-add_compile_options(-std=c++14)
+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 -Wno-unused-parameter)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  autoware_perception_msgs
-  autoware_planning_msgs
-  geometry_msgs
-  lanelet2_extension
-  roscpp
-  sensor_msgs
-  tf2
-  tf2_ros
-)
-find_package(Eigen3 REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS
-    autoware_perception_msgs
-    autoware_planning_msgs
-    geometry_msgs
-    lanelet2_extension
-    roscpp
-    sensor_msgs
-    tf2
-    tf2_ros
-)
+find_package(Eigen3 REQUIRED)
 
 ###########
 ## Build ##
 ###########
 
 include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
 )
 
-add_executable(traffic_light_map_based_detector_node
+ament_auto_add_executable(traffic_light_map_based_detector_node
   src/node.cpp
   src/main.cpp
 )
 
-add_dependencies(traffic_light_map_based_detector_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(traffic_light_map_based_detector_node
-  ${catkin_LIBRARIES}
-)
-
 #############
 ## Install ##
 #############
 
-install(
-  TARGETS
-    traffic_light_map_based_detector_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(
-  DIRECTORY
-    launch
-    config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
 )

perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.yaml

@@ -1,5 +1,7 @@
-max_vibration_pitch: 0.01745329251 # -0.5 ~ 0.5 deg
-max_vibration_yaw: 0.01745329251 # -0.5 ~ 0.5 deg
-max_vibration_height: 0.1 # -0.05 ~ 0.05 m
-max_vibration_width: 0.1 # -0.05 ~ 0.05 m
-max_vibration_depth: 0.1 # -0.05 ~ 0.05 m
+/**:
+  ros__parameters:
+    max_vibration_pitch: 0.01745329251   # -0.5 ~ 0.5 deg
+    max_vibration_yaw: 0.01745329251     # -0.5 ~ 0.5 deg
+    max_vibration_height: 0.1            # -0.05 ~ 0.05 m
+    max_vibration_width: 0.1             # -0.05 ~ 0.05 m
+    max_vibration_depth: 0.1             # -0.05 ~ 0.05 m

perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp

@@ -19,27 +19,27 @@
 
 #pragma once
 
-#include <ros/ros.h>
-
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_extension/regulatory_elements/autoware_traffic_light.h>
 #include <lanelet2_extension/utility/query.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
-#include <autoware_lanelet2_msgs/MapBin.h>
-#include <autoware_perception_msgs/TrafficLightRoiArray.h>
-#include <autoware_planning_msgs/Route.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <sensor_msgs/CameraInfo.h>
+#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_roi_array.hpp>
+#include <autoware_planning_msgs/msg/route.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
 #include <tf2_ros/transform_listener.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <rclcpp/rclcpp.hpp>
 
 #include <memory>
 
 namespace traffic_light
 {
-class MapBasedDetector
+class MapBasedDetector: public rclcpp::Node
 {
 public:
   MapBasedDetector();
@@ -62,20 +62,19 @@ class MapBasedDetector
   };
 
 private:
-  ros::NodeHandle nh_;
-  ros::NodeHandle pnh_;
-  ros::Subscriber map_sub_;
-  ros::Subscriber camera_info_sub_;
-  ros::Subscriber route_sub_;
-  ros::Publisher roi_pub_;
-  ros::Publisher viz_pub_;
+  rclcpp::Subscription<autoware_lanelet2_msgs::msg::MapBin>::SharedPtr map_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
+  rclcpp::Subscription<autoware_planning_msgs::msg::Route>::SharedPtr route_sub_;
+
+  rclcpp::Publisher<autoware_perception_msgs::msg::TrafficLightRoiArray>::SharedPtr roi_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr viz_pub_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
   using TrafficLightSet = std::set<lanelet::ConstLineString3d, IdLessThan>;
 
-  sensor_msgs::CameraInfo::ConstPtr camera_info_ptr_;
+  sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_ptr_;
   std::shared_ptr<TrafficLightSet> all_traffic_lights_ptr_;
   std::shared_ptr<TrafficLightSet> route_traffic_lights_ptr_;
 
@@ -84,28 +83,33 @@ class MapBasedDetector
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
   Config config_;
 
-  void mapCallback(const autoware_lanelet2_msgs::MapBin & input_msg);
-  void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & input_msg);
-  void routeCallback(const autoware_planning_msgs::Route::ConstPtr & input_msg);
+  void mapCallback(const autoware_lanelet2_msgs::msg::MapBin::ConstSharedPtr input_msg);
+  void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg);
+  void routeCallback(const autoware_planning_msgs::msg::Route::ConstSharedPtr input_msg);
   void isInVisibility(
     const TrafficLightSet & all_traffic_lights,
-    const geometry_msgs::Pose & camera_pose, const sensor_msgs::CameraInfo & camera_info,
+    const geometry_msgs::msg::Pose & camera_pose,
+    const sensor_msgs::msg::CameraInfo & camera_info,
     std::vector<lanelet::ConstLineString3d> & visible_traffic_lights);
   bool isInDistanceRange(
-    const geometry_msgs::Point & tl_point, const geometry_msgs::Point & camera_point,
+    const geometry_msgs::msg::Point & tl_point,
+    const geometry_msgs::msg::Point & camera_point,
     const double max_distance_range);
   bool isInAngleRange(
     const double & tl_yaw, const double & camera_yaw, const double max_angele_range);
   bool isInImageFrame(
-    const sensor_msgs::CameraInfo & camera_info, const geometry_msgs::Point & point);
+    const sensor_msgs::msg::CameraInfo & camera_info,
+    const geometry_msgs::msg::Point & point);
   double normalizeAngle(const double & angle);
   bool getTrafficLightRoi(
-    const geometry_msgs::Pose & camera_pose, const sensor_msgs::CameraInfo & camera_info,
-    const lanelet::ConstLineString3d traffic_light, const Config & config,
-    autoware_perception_msgs::TrafficLightRoi & tl_roi);
+    const geometry_msgs::msg::Pose & camera_pose,
+    const sensor_msgs::msg::CameraInfo & camera_info,
+    const lanelet::ConstLineString3d traffic_light,
+    const Config & config,
+    autoware_perception_msgs::msg::TrafficLightRoi & tl_roi);
   void publishVisibleTrafficLights(
-    const geometry_msgs::PoseStamped camera_pose_stamped,
+    const geometry_msgs::msg::PoseStamped camera_pose_stamped,
     const std::vector<lanelet::ConstLineString3d> & visible_traffic_lights,
-    const ros::Publisher & pub);
+    const rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub);
 };
 }  // namespace traffic_light

perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml

@@ -7,11 +7,10 @@
   <arg name="input/route" default="/planning/mission_planning/route" />
   <arg name="output/rois" default="output/rois" />
 
-  <node pkg="traffic_light_map_based_detector" type="traffic_light_map_based_detector_node" name="traffic_light_map_based_detector" output="screen">
-    <remap from="~input/vector_map" to="$(arg input/vector_map)" />
-    <remap from="~input/camera_info" to="$(arg input/camera_info)" />
-    <remap from="~input/route" to="$(arg input/route)" />
-    <remap from="~output/rois" to="$(arg output/rois)" />
-    <rosparam command="load" file="$(find traffic_light_map_based_detector)/config/traffic_light_map_based_detector.yaml" />
+  <node pkg="traffic_light_map_based_detector" exec="traffic_light_map_based_detector_node" name="traffic_light_map_based_detector" output="screen">
+    <remap from="/input/vector_map" to="$(var input/vector_map)" />
+    <remap from="/input/camera_info" to="$(var input/camera_info)" />
+    <remap from="/input/route" to="$(var input/route)" />
+    <remap from="/output/rois" to="$(var output/rois)" />
   </node>
-</launch>
+</launch>

perception/traffic_light_map_based_detector/package.xml

@@ -7,36 +7,20 @@
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <license>Apache2</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>autoware_perception_msgs</build_depend>
-  <build_depend>autoware_planning_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>lanelet2_extension</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_export_depend>autoware_perception_msgs</build_export_depend>
-  <build_export_depend>autoware_planning_msgs</build_export_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>lanelet2_extension</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
-  <build_export_depend>tf2</build_export_depend>
-  <build_export_depend>tf2_ros</build_export_depend>
-  <exec_depend>autoware_perception_msgs</exec_depend>
-  <exec_depend>autoware_planning_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>lanelet2_extension</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>tf2</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

perception/traffic_light_map_based_detector/src/main.cpp

@@ -17,15 +17,17 @@
  *
  */
 
-#include <ros/ros.h>
 #include <traffic_light_map_based_detector/node.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 int main(int argc, char ** argv)
-{
-  ros::init(argc, argv, "traffic_light_map_based_detector");
-  traffic_light::MapBasedDetector node;
-
-  ros::spin();
+{  
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<traffic_light::MapBasedDetector>();
+
+  rclcpp::spin(node);  
+  rclcpp::shutdown();
 
   return 0;
 }