Merge branch 'main' into feat/lidar_centerpoint_preprocessing_cuda

localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml

@@ -5,7 +5,7 @@
   <arg name="autoware_pose_covariance_modifier/output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance"/>
   <arg name="param_file" default="$(find-pkg-share autoware_pose_covariance_modifier)/config/pose_covariance_modifier.param.yaml"/>
 
-  <node pkg="autoware_pose_covariance_modifier" exec="autoware_pose_covariance_modifier_node" name="pose_covariance_modifier_node" output="screen">
+  <node pkg="autoware_pose_covariance_modifier" exec="autoware_pose_covariance_modifier_node" name="pose_covariance_modifier_node" output="both">
     <remap from="input_gnss_pose_with_cov_topic" to="$(var autoware_pose_covariance_modifier/input_gnss_pose_with_cov_topic)"/>
     <remap from="input_ndt_pose_with_cov_topic" to="$(var autoware_pose_covariance_modifier/input_ndt_pose_with_cov_topic)"/>
     <remap from="output_pose_with_covariance_topic" to="$(var autoware_pose_covariance_modifier/output_pose_with_covariance_topic)"/>

localization/yabloc/yabloc_image_processing/CMakeLists.txt

@@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED)
 # PCL
 find_package(PCL REQUIRED COMPONENTS common)
 
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/line_segment_detector/line_segment_detector_core.cpp
+  src/graph_segment/graph_segment_core.cpp
+  src/graph_segment/similar_area_searcher.cpp
+  src/segment_filter/segment_filter_core.cpp
+  src/undistort/undistort_node.cpp
+  src/line_segments_overlay/line_segments_overlay_core.cpp
+  src/lanelet2_overlay/lanelet2_overlay_core.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS})
+
 # ===================================================
 # Executable
-# line segment detector
-set(TARGET line_segment_detector_node)
-ament_auto_add_executable(${TARGET}
-  src/line_segment_detector/line_segment_detector_node.cpp
-  src/line_segment_detector/line_segment_detector_core.cpp)
-target_include_directories(${TARGET} PUBLIC include)
-target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS})
-target_link_libraries(${TARGET} ${OpenCV_LIBS})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::graph_segment::GraphSegment"
+  EXECUTABLE yabloc_graph_segment_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-# graph based segmentation
-set(TARGET graph_segment_node)
-ament_auto_add_executable(${TARGET}
-  src/graph_segment/graph_segment_node.cpp
-  src/graph_segment/graph_segment_core.cpp
-  src/graph_segment/similar_area_searcher.cpp)
-target_include_directories(${TARGET} PUBLIC include)
-target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS})
-target_link_libraries(${TARGET} ${OpenCV_LIBS})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay"
+  EXECUTABLE yabloc_lanelet2_overlay_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-# segment filter
-set(TARGET segment_filter_node)
-ament_auto_add_executable(${TARGET}
-  src/segment_filter/segment_filter_node.cpp
-  src/segment_filter/segment_filter_core.cpp)
-target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
-target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::line_segment_detector::LineSegmentDetector"
+  EXECUTABLE yabloc_line_segment_detector_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-# undistort
-set(TARGET undistort_node)
-ament_auto_add_executable(${TARGET}
-  src/undistort/undistort_node.cpp)
-target_link_libraries(${TARGET} ${OpenCV_LIBS})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay"
+  EXECUTABLE yabloc_line_segments_overlay_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-# line_segments_overlay
-set(TARGET line_segments_overlay_node)
-ament_auto_add_executable(${TARGET}
-  src/line_segments_overlay/line_segments_overlay_core.cpp
-  src/line_segments_overlay/line_segments_overlay_node.cpp)
-target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
-target_link_libraries(${TARGET} ${PCL_LIBRARIES})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::segment_filter::SegmentFilter"
+  EXECUTABLE yabloc_segment_filter_node
+  EXECUTOR SingleThreadedExecutor
+)
 
-# lanelet2_overlay
-set(TARGET lanelet2_overlay_node)
-ament_auto_add_executable(${TARGET}
-  src/lanelet2_overlay/lanelet2_overlay_core.cpp
-  src/lanelet2_overlay/lanelet2_overlay_node.cpp)
-target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
-target_link_libraries(${TARGET} ${PCL_LIBRARIES})
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "yabloc::undistort::UndistortNode"
+  EXECUTABLE yabloc_undistort_node
+  EXECUTOR SingleThreadedExecutor
+)
 
 # ===================================================
 ament_auto_package(INSTALL_TO_SHARE config launch)

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp

@@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node
 public:
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using Image = sensor_msgs::msg::Image;
-  GraphSegment();
+  explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   const float target_height_ratio_;

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp

@@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node
   using Image = sensor_msgs::msg::Image;
   using Float32Array = std_msgs::msg::Float32MultiArray;
 
-  Lanelet2Overlay();
+  explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   common::StaticTfSubscriber tf_subscriber_;

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp

@@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node
   using Image = sensor_msgs::msg::Image;
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
 
-  LineSegmentDetector();
+  explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   rclcpp::Subscription<Image>::SharedPtr sub_image_;

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp

@@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node
   using Image = sensor_msgs::msg::Image;
   using LineSegment = pcl::PointXYZLNormal;
   using LineSegments = pcl::PointCloud<LineSegment>;
-  LineSegmentsOverlay();
+  explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   void on_image(const Image::ConstSharedPtr & img_msg);

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp

@@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using Image = sensor_msgs::msg::Image;
 
-  SegmentFilter();
+  explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
 private:
   using ProjectFunc = std::function<std::optional<Eigen::Vector3f>(const Eigen::Vector3f &)>;

localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml

@@ -5,7 +5,7 @@
   <arg name="resized_info" default="/sensing/camera/undistorted/camera_info"/>
 
   <!-- undistort -->
-  <node name="undistort" pkg="yabloc_image_processing" exec="undistort_node" output="screen" args="--ros-args --log-level warn">
+  <node pkg="yabloc_image_processing" exec="yabloc_undistort_node" output="both">
     <param from="$(var undistort_param_path)"/>
 
     <remap from="~/input/image_raw" to="$(var src_image)"/>
@@ -18,7 +18,7 @@
   <!-- line segment detector -->
   <arg name="output_image_with_line_segments" default="image_with_line_segments"/>
   <arg name="output_line_segments_cloud" default="line_segments_cloud"/>
-  <node name="line_segment_detector" pkg="yabloc_image_processing" exec="line_segment_detector_node" output="screen" args="--ros-args --log-level warn">
+  <node pkg="yabloc_image_processing" exec="yabloc_line_segment_detector_node" output="both">
     <remap from="~/input/image_raw" to="$(var resized_image)"/>
     <remap from="~/debug/image_with_line_segments" to="$(var output_image_with_line_segments)"/>
     <remap from="~/output/line_segments_cloud" to="$(var output_line_segments_cloud)"/>
@@ -27,7 +27,7 @@
   <!-- graph based segmentation -->
   <arg name="output_graph_segmented" default="graph_segmented"/>
   <arg name="output_segmented_image" default="segmented_image"/>
-  <node name="graph_segment" pkg="yabloc_image_processing" exec="graph_segment_node" output="screen" args="--ros-args --log-level warn">
+  <node pkg="yabloc_image_processing" exec="yabloc_graph_segment_node" output="both">
     <param from="$(var graph_segment_param_path)"/>
     <remap from="~/input/image_raw" to="$(var resized_image)"/>
     <remap from="~/output/mask_image" to="$(var output_graph_segmented)"/>
@@ -40,7 +40,7 @@
   <arg name="output_projected_line_segments_cloud" default="projected_line_segments_cloud"/>
   <arg name="output_projected_image" default="projected_image"/>
   <arg name="output_debug_line_segments" default="debug/line_segments_cloud"/>
-  <node name="segment_filter" pkg="yabloc_image_processing" exec="segment_filter_node" output="screen" args="--ros-args --log-level info">
+  <node pkg="yabloc_image_processing" exec="yabloc_segment_filter_node" output="both">
     <param from="$(var segment_filter_param_path)"/>
 
     <remap from="~/input/line_segments_cloud" to="$(var input_line_segments_cloud)"/>

localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml

@@ -10,7 +10,7 @@
   <arg name="resized_info" description=""/>
 
   <!-- lanelet2 overlay monitor -->
-  <node name="lanelet2_overlay" pkg="yabloc_image_processing" exec="lanelet2_overlay_node" output="screen" args="--ros-args --log-level warn">
+  <node pkg="yabloc_image_processing" exec="yabloc_lanelet2_overlay_node" output="both">
     <remap from="~/input/image_raw" to="$(var resized_image)"/>
     <remap from="~/input/camera_info" to="$(var resized_info)"/>
     <remap from="~/input/pose" to="$(var input_overlaid_pose)"/>
@@ -23,7 +23,7 @@
   </node>
 
   <!-- line segments overlay monitor -->
-  <node name="line_segments_overlay" pkg="yabloc_image_processing" exec="line_segments_overlay_node" output="screen">
+  <node pkg="yabloc_image_processing" exec="yabloc_line_segments_overlay_node" output="both">
     <remap from="~/input/image_raw" to="$(var resized_image)"/>
     <remap from="~/input/line_segments" to="$(var input_debug_line_segments)"/>
     <remap from="~/debug/image_with_colored_line_segments" to="$(var output_image_with_colored_line_segments)"/>

localization/yabloc/yabloc_image_processing/package.xml

@@ -19,6 +19,7 @@
   <depend>cv_bridge</depend>
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tier4_autoware_utils</depend>

localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp

@@ -23,8 +23,8 @@
 
 namespace yabloc::graph_segment
 {
-GraphSegment::GraphSegment()
-: Node("graph_segment"),
+GraphSegment::GraphSegment(const rclcpp::NodeOptions & options)
+: Node("graph_segment", options),
   target_height_ratio_(declare_parameter<float>("target_height_ratio")),
   target_candidate_box_width_(declare_parameter<int>("target_candidate_box_width"))
 {
@@ -159,3 +159,6 @@ void GraphSegment::draw_and_publish_image(
 }
 
 }  // namespace yabloc::graph_segment
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment)

localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp

@@ -28,8 +28,8 @@
 
 namespace yabloc::lanelet2_overlay
 {
-Lanelet2Overlay::Lanelet2Overlay()
-: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40}
+Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options)
+: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40}
 {
   using std::placeholders::_1;
 
@@ -211,3 +211,6 @@ void Lanelet2Overlay::make_vis_marker(
 }
 
 }  // namespace yabloc::lanelet2_overlay
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay)

localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp

@@ -23,7 +23,8 @@
 
 namespace yabloc::line_segment_detector
 {
-LineSegmentDetector::LineSegmentDetector() : Node("line_detector")
+LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options)
+: Node("line_detector", options)
 {
   using std::placeholders::_1;
 
@@ -106,3 +107,6 @@ std::vector<cv::Mat> LineSegmentDetector::remove_too_outer_elements(
 }
 
 }  // namespace yabloc::line_segment_detector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector)

localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp

@@ -23,8 +23,8 @@
 
 namespace yabloc::line_segments_overlay
 {
-LineSegmentsOverlay::LineSegmentsOverlay()
-: Node("overlay_lanelet2"),
+LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options)
+: Node("line_segments_overlay", options),
   max_buffer_size_(static_cast<size_t>(declare_parameter<int>("max_buffer_size", 5)))
 {
   using std::placeholders::_1;
@@ -90,3 +90,6 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l
 }
 
 }  // namespace yabloc::line_segments_overlay
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay)

localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp

@@ -23,8 +23,8 @@
 
 namespace yabloc::segment_filter
 {
-SegmentFilter::SegmentFilter()
-: Node("segment_filter"),
+SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options)
+: Node("segment_filter", options),
   image_size_(declare_parameter<int>("image_size")),
   max_range_(declare_parameter<float>("max_range")),
   min_segment_length_(declare_parameter<float>("min_segment_length")),
@@ -282,3 +282,6 @@ std::set<int> SegmentFilter::filter_by_mask(
 }
 
 }  // namespace yabloc::segment_filter
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter)