feat: add schema

Author: vividf

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -58,7 +58,7 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/utility/utilities.cpp
-  src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+  src/concatenate_data/concatenate_and_time_sync_node.cpp
   src/concatenate_data/combine_cloud_handler.cpp
   src/concatenate_data/cloud_collector.cpp
   src/concatenate_data/concatenate_pointclouds.cpp
@@ -104,7 +104,7 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
 # ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
-  EXECUTABLE concatenate_data_node)
+  EXECUTABLE concatenate_and_time_sync_node)
 
 # ========== CropBox ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter

sensing/pointcloud_preprocessor/config/concatenate_and_time_sync.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    maximum_queue_size: 5
+    timeout_sec: 0.5
+    offset_tolerance: 0.03
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/right/pointcloud_before_sync",
+                    "/sensing/lidar/left/pointcloud_before_sync",
+                    "/sensing/lidar/top/pointcloud_before_sync"
+                ]
+    output_frame: base_link
+    lidar_timestamp_offsets: [0.0, 0.03, 0.04]

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp

@@ -49,17 +49,17 @@
  *
  */
 
-#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
-#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
+#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODE_HPP_
+#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODE_HPP_
 
 #include <deque>
+#include <list>
 #include <memory>
 #include <mutex>
 #include <set>
 #include <string>
 #include <unordered_map>
 #include <vector>
-#include <list>
 
 // ROS includes
 #include "autoware_point_types/types.hpp"
@@ -92,24 +92,24 @@ namespace pointcloud_preprocessor
 class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 {
 public:
-
   explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options);
   virtual ~PointCloudConcatenateDataSynchronizerComponent() {}
   void publishClouds();
+
 private:
   struct Parameters
   {
     int maximum_queue_size;
     double timeout_sec;
+    double offset_tolerance;
     bool is_motion_compensated;
     bool publish_synchronized_pointcloud;
     bool keep_input_frame_in_synchronized_pointcloud;
     std::string synchronized_pointcloud_postfix;
     std::string input_twist_topic_type;
-    std::string output_frame;
     std::vector<std::string> input_topics;
+    std::string output_frame;
     std::vector<double> lidar_timestamp_offsets;
-    double offset_tolerance;
   } params_;
 
   std::set<std::string> missed_cloud_;
@@ -134,8 +134,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   diagnostic_updater::Updater updater_{this};
 
   void cloud_callback(
-    const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
-    const std::string & topic_name);
+    const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name);
   void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input);
   void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
   void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
@@ -145,4 +144,4 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 
 }  // namespace pointcloud_preprocessor
 
-#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
+#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODE_HPP_

sensing/pointcloud_preprocessor/launch/concatenate_and_time_sync_node.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+  <arg name="output" default="/sensing/lidar/concatenated/pointcloud"/>
+  <!-- Parameter -->
+  <arg name="param_file" default="$(find-pkg-share pointcloud_preprocessor)/config/concatenate_and_time_sync.param.yaml"/>
+  <node pkg="pointcloud_preprocessor" exec="concatenate_and_time_sync_node" name="concatenate_and_time_sync_node" output="screen">
+    <remap from="~/input/twist" to="$(var input/twist)"/>
+    <remap from="~/output" to="$(var output)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

sensing/pointcloud_preprocessor/schema/concatenate_and_time_sync.schema.json

@@ -0,0 +1,98 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Concatenate and Time Synchronize Node",
+  "type": "object",
+  "definitions": {
+    "distortion_corrector": {
+      "type": "object",
+      "properties": {
+        "maximum_queue_size": {
+          "type": "integer",
+          "default": 5,
+          "description": "Maximum size of the queue."
+        },
+        "timeout_sec": {
+          "type": "number",
+          "default": 0.0,
+          "description": "Timeout in seconds."
+        },
+        "offset_tolerance": {
+          "type": "number",
+          "default": 0.0,
+          "description": "Tolerance for offset."
+        },
+        "is_motion_compensated": {
+          "type": "boolean",
+          "default": true,
+          "description": "Flag to indicate if motion compensation is enabled."
+        },
+        "publish_synchronized_pointcloud": {
+          "type": "boolean",
+          "default": true,
+          "description": "Flag to indicate if synchronized point cloud should be published."
+        },
+        "keep_input_frame_in_synchronized_pointcloud": {
+          "type": "boolean",
+          "default": true,
+          "description": "Flag to indicate if input frame should be kept in synchronized point cloud."
+        },
+        "synchronized_pointcloud_postfix": {
+          "type": "string",
+          "default": "pointcloud",
+          "description": "Postfix for the synchronized point cloud."
+        },
+        "input_twist_topic_type": {
+          "type": "string",
+          "default": "twist",
+          "description": "Type of the input twist topic."
+        },
+        "input_topics": {
+          "type": "array",
+          "items": {
+            "type": "string"
+          },
+          "default": [],
+          "description": "List of input topics."
+        },
+        "output_frame": {
+          "type": "string",
+          "default": "",
+          "description": "Output frame."
+        },
+        "lidar_timestamp_offsets": {
+          "type": "array",
+          "items": {
+            "type": "number"
+          },
+          "default": [],
+          "description": "List of LiDAR timestamp offsets."
+        }
+      },
+      "required": [
+        "maximum_queue_size",
+        "timeout_sec",
+        "offset_tolerance",
+        "is_motion_compensated",
+        "publish_synchronized_pointcloud",
+        "keep_input_frame_in_synchronized_pointcloud",
+        "synchronized_pointcloud_postfix",
+        "input_twist_topic_type",
+        "input_topics",
+        "output_frame",
+        "lidar_timestamp_offsets"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/distortion_corrector"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

sensing/pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -52,7 +52,7 @@
 #include "pointcloud_preprocessor/concatenate_data/cloud_collector.hpp"
 
 #include "pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp"
-#include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp"
+#include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -71,13 +71,13 @@ CloudCollector::CloudCollector(
 {
   timestamp_ = 0.0;
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-   std::chrono::duration<double>(timeout_sec_));
-  
+    std::chrono::duration<double>(timeout_sec_));
+
   timer_ = rclcpp::create_timer(
-   concatenate_node_, concatenate_node_->get_clock(), period_ns, std::bind(&CloudCollector::combineClouds, this));
+    concatenate_node_, concatenate_node_->get_clock(), period_ns,
+    std::bind(&CloudCollector::combineClouds, this));
 }
 
-
 void CloudCollector::setTimeStamp(double timestamp)
 {
   timestamp_ = timestamp;
@@ -91,10 +91,9 @@ double CloudCollector::getTimeStamp()
 void CloudCollector::processCloud(
   std::string topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud)
 {
+  // TODO(vivid): do I need a check here to see if map already exists for same topic?
   topic_cloud_map_[topic_name] = cloud;
   if (topic_cloud_map_.size() == num_of_clouds_) combineClouds();
-
-
 }
 
 void CloudCollector::combineClouds()
@@ -103,7 +102,6 @@ void CloudCollector::combineClouds()
   concatenate_node_->publishClouds();
   std::lock_guard<std::mutex> lock(mutex_);
   deleteCollector();
-
 }
 
 void CloudCollector::deleteCollector()
@@ -119,8 +117,7 @@ void CloudCollector::deleteCollector()
 // debug
 void CloudCollector::printTimer()
 {
-  //std::cout << "time to ended: " << timer_->time_until_trigger().count() << std::endl;
+  std::cout << "time to ended: " << timer_->time_until_trigger().count() << std::endl;
 }
 
-
 }  // namespace pointcloud_preprocessor

sensing/pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp

@@ -67,11 +67,9 @@
 namespace pointcloud_preprocessor
 {
 
-
 CombineCloudHandler::CombineCloudHandler(
-    rclcpp::Node * node, std::vector<std::string> input_topics,
-     std::string output_frame, 
-    bool is_motion_compensated, bool keep_input_frame_in_synchronized_pointcloud)
+  rclcpp::Node * node, std::vector<std::string> input_topics, std::string output_frame,
+  bool is_motion_compensated, bool keep_input_frame_in_synchronized_pointcloud)
 : node_(node),
   tf_buffer_(node_->get_clock()),
   tf_listener_(tf_buffer_),
@@ -110,7 +108,6 @@ void CombineCloudHandler::processTwist(
   twist_ptr->header = input->header;
   twist_ptr->twist = input->twist.twist;
   twist_ptr_queue_.push_back(twist_ptr);
-
 }
 
 void CombineCloudHandler::processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input)
@@ -187,8 +184,7 @@ void CombineCloudHandler::resetCloud()
 }
 
 void CombineCloudHandler::combinePointClouds(
-  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> &
-    topic_to_cloud_map_)
+  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> & topic_to_cloud_map_)
 {
   std::vector<rclcpp::Time> pc_stamps;
   for (const auto & pair : topic_to_cloud_map_) {
@@ -198,44 +194,43 @@ void CombineCloudHandler::combinePointClouds(
   std::sort(pc_stamps.begin(), pc_stamps.end(), std::greater<rclcpp::Time>());
   const auto oldest_stamp = pc_stamps.back();
 
-
   std::unordered_map<rclcpp::Time, Eigen::Matrix4f, RclcppTimeHash_> transform_memo;
 
   for (const auto & pair : topic_to_cloud_map_) {
     std::string topic_name = pair.first;
     sensor_msgs::msg::PointCloud2::SharedPtr cloud = pair.second;
-    //std::cout << "in combination topic: " << topic_name << std::endl;
+    // std::cout << "in combination topic: " << topic_name << std::endl;
 
     auto transformed_cloud_ptr = std::make_shared<sensor_msgs::msg::PointCloud2>();
     if (output_frame_ != cloud->header.frame_id) {
-      if (!pcl_ros::transformPointCloud(output_frame_, *cloud, *transformed_cloud_ptr, tf_buffer_)) {
+      if (!pcl_ros::transformPointCloud(
+            output_frame_, *cloud, *transformed_cloud_ptr, tf_buffer_)) {
         RCLCPP_ERROR(
           node_->get_logger(),
           "[transformPointCloud] Error converting first input dataset from %s to %s.",
           cloud->header.frame_id.c_str(), output_frame_.c_str());
         return;
       }
-    }
-    else {
+    } else {
       transformed_cloud_ptr = cloud;
     }
 
-    auto start = std::chrono::high_resolution_clock::now();
-    auto transformed_delay_compensated_cloud_ptr = std::make_shared<sensor_msgs::msg::PointCloud2>();
-    
-    if(is_motion_compensated_) {
+    auto transformed_delay_compensated_cloud_ptr =
+      std::make_shared<sensor_msgs::msg::PointCloud2>();
+
+    if (is_motion_compensated_) {
       Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity();
       rclcpp::Time current_cloud_stamp = rclcpp::Time(cloud->header.stamp);
       for (const auto & stamp : pc_stamps) {
-        if(stamp >= current_cloud_stamp)
-          continue;
+        if (stamp >= current_cloud_stamp) continue;
 
         Eigen::Matrix4f new_to_old_transform;
         if (transform_memo.find(stamp) != transform_memo.end()) {
-            new_to_old_transform = transform_memo[stamp];
+          new_to_old_transform = transform_memo[stamp];
         } else {
-            new_to_old_transform = computeTransformToAdjustForOldTimestamp(stamp, current_cloud_stamp);
-            transform_memo[stamp] = new_to_old_transform;
+          new_to_old_transform =
+            computeTransformToAdjustForOldTimestamp(stamp, current_cloud_stamp);
+          transform_memo[stamp] = new_to_old_transform;
         }
         adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform;
         current_cloud_stamp = stamp;
@@ -244,8 +239,7 @@ void CombineCloudHandler::combinePointClouds(
         adjust_to_old_data_transform, *transformed_cloud_ptr,
         *transformed_delay_compensated_cloud_ptr);
 
-    }
-    else {
+    } else {
       transformed_delay_compensated_cloud_ptr = transformed_cloud_ptr;
     }
 
@@ -278,8 +272,6 @@ void CombineCloudHandler::combinePointClouds(
   concatenate_cloud_ptr_->header.stamp = oldest_stamp;
 }
 
-
-
 Eigen::Matrix4f CombineCloudHandler::computeTransformToAdjustForOldTimestamp(
   const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp)
 {
@@ -334,10 +326,10 @@ Eigen::Matrix4f CombineCloudHandler::computeTransformToAdjustForOldTimestamp(
   }
 
   Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
-  
+
   float cos_yaw = std::cos(yaw);
   float sin_yaw = std::sin(yaw);
-  
+
   transformation_matrix(0, 3) = x;
   transformation_matrix(1, 3) = y;
   transformation_matrix(0, 0) = cos_yaw;