refactor(pointcloud_to_laserscan): rework parameter

CMakeLists.txt

@@ -47,4 +47,5 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
+    config
 )

README.md

@@ -17,18 +17,13 @@ This ROS 2 component projects `sensor_msgs/msg/PointCloud2` messages into `senso
 
 ### Parameters
 
-* `min_height` (double, default: 0.0) - The minimum height to sample in the point cloud in meters.
-* `max_height` (double, default: 1.0) - The maximum height to sample in the point cloud in meters.
-* `angle_min` (double, default: -π/2) - The minimum scan angle in radians.
-* `angle_max` (double, default: π/2) - The maximum scan angle in radians.
-* `angle_increment` (double, default: π/360) - Resolution of laser scan in radians per ray.
-* `queue_size` (double, default: detected number of cores) - Input point cloud queue size.
-* `scan_time` (double, default: 1.0/30.0) - The scan rate in seconds. Only used to populate the scan_time field of the output laser scan message.
-* `range_min` (double, default: 0.45) - The minimum ranges to return in meters.
-* `range_max` (double, default: 4.0) - The maximum ranges to return in meters.
-* `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
-* `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
-* `use_inf` (boolean, default: true) - If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf.
+#### pointcloud\_to\_laserscan\_node
+
+{{ json_to_markdown("src/schema/pointcloud_to_laserscan.schema.json") }}
+
+#### laserscan\_to\_pointcloud\_node
+
+{{ json_to_markdown("src/schema/laserscan_to_pointcloud.schema.json") }}
 
 ## pointcloud\_to\_laserscan::LaserScanToPointCloudNode
 

config/laserscan_to_pointcloud_node.param.yaml

@@ -0,0 +1,5 @@
+**/:
+  ros__parameters:
+    target_frame: ""
+    transform_tolerance: 0.01 
+    queue_size: 0

config/pointcloud_to_laserscan_node.param.yaml

@@ -0,0 +1,18 @@
+**/:
+  ros__parameters:
+    target_frame: ""
+    transform_tolerance: 0.01
+    queue_size: 0
+    min_height: 0.0 
+    max_height: 1.0
+    # -$M_PI / 2)
+    angle_min: -1.5707963267948966
+    # $_MPI / 2
+    angle_max: 1.5707963267948966
+    # $M_PI / 360
+    angle_increment: 0.008726646259971648
+    scan_time: 0.03333333333333333
+    range_min: 0.45
+    range_max: 4.0
+    inf_epsilon: 1.0
+    use_inf: true

include/pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp

@@ -73,6 +73,9 @@ class LaserScanToPointCloudNode : public rclcpp::Node
   ~LaserScanToPointCloudNode() override;
 
 private:
+  static constexpr int QUEUE_SIZE_AUTO = 0;
+  static constexpr double NUM_LIMIT_AUTO = 0.0;
+
   void scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg);
 
   void subscriptionListenerThreadLoop();

include/pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp

@@ -77,6 +77,8 @@ class PointCloudToLaserScanNode : public rclcpp::Node
   ~PointCloudToLaserScanNode() override;
 
 private:
+  static constexpr int QUEUE_SIZE_AUTO_ = 0;
+
   void cloudCallback(PointCloud2::ConstSharedPtr cloud_msg);
 
   void subscriptionListenerThreadLoop();

launch/sample_laserscan_to_pointcloud_launch.py

@@ -1,13 +1,20 @@
+import os
+
+import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import ExecuteProcess
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import FindPackageShare, aunchConfiguration
 from launch_ros.actions import Node
 
-import yaml
-
 
 def generate_launch_description():
+    param_file = os.path.join(
+        FindPackageShare('pointcloud_to_laserscan').find('laserscan_to_pointcloud_node'),
+        'param',
+        'laserscan_to_pointcloud.param.yaml'
+    )
+
     return LaunchDescription([
         DeclareLaunchArgument(
             name='scanner', default_value='scanner',
@@ -38,6 +45,6 @@ def generate_launch_description():
             name='laserscan_to_pointcloud',
             remappings=[('scan_in', [LaunchConfiguration(variable_name='scanner'), '/scan']),
                         ('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
-            parameters=[{'target_frame': 'scan', 'transform_tolerance': 0.01}]
+            parameters=[param_file]
         ),
     ])

launch/sample_pointcloud_to_laserscan_launch.py

@@ -1,10 +1,16 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import FindPackageShare, LaunchConfiguration
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
+    param_file = os.path.join(
+        FindPackageShare('pointcloud_to_laserscan').find('pointcloud_to_laserscan_node'),
+        'param',
+        'pointcloud_to_laserscan_node.param.yaml'
+    )
+
     return LaunchDescription([
         DeclareLaunchArgument(
             name='scanner', default_value='scanner',
@@ -31,20 +37,7 @@ def generate_launch_description():
                 ('~/output/ray', [LaunchConfiguration('scanner'), '/ray']),
                 ('~/output/stixel', [LaunchConfiguration('scanner'), '/stixel'])
             ],
-            parameters=[{
-                'target_frame': 'cloud',
-                'transform_tolerance': 0.01,
-                'min_height': 0.0,
-                'max_height': 1.0,
-                'angle_min': -1.5708,  # -M_PI/2
-                'angle_max': 1.5708,  # M_PI/2
-                'angle_increment': 0.0087,  # M_PI/360.0
-                'scan_time': 0.3333,
-                'range_min': 0.45,
-                'range_max': 4.0,
-                'use_inf': True,
-                'inf_epsilon': 1.0
-            }],
+            parameters=[param],
             name='pointcloud_to_laserscan'
         )
     ])

schema/laserscan_to_pointcloud_node.schema.json

@@ -0,0 +1,42 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Laserscan to Pointcloud Node ",
+  "type": "object",
+  "definitions": {
+    "laserscan_to_pointcloud_node": {
+      "type": "object",
+      "properties": {
+        "queue_size": {
+          "description": "Input point cloud queue size",
+          "type": "int",
+          "default": 0
+        },
+        "target_frame": {
+          "description": "If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud",
+          "type": "string",
+          "default": null
+        },
+        "transform_tolerance": {
+          "description": "Time tolerance for transform lookups. Only used if a target_frame is provided",
+          "type": "number",
+          "default": 0.01
+        }
+      },
+      "required": [
+        "queue_size"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/laserscan_to_pointcloud_node"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

schema/pointcloud_to_laserscan_node.schema.json

@@ -0,0 +1,100 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Laserscan to Pointcloud Node ",
+  "type": "object",
+  "definitions": {
+    "laserscan_to_pointcloud_node": {
+      "type": "object",
+      "properties": {
+        "min_height": {
+          "description": "The minimum height to sample in the point cloud in meters",
+          "type": "number",
+          "default": 0.0
+        },
+        "max_height": {
+          "description": " The maximum height to sample in the point cloud in meters",
+          "type": "number",
+          "default": 1.0
+        },
+        "angle_min": {
+          "description": "The minimum scan angle in radians",
+          "type": "number",
+          "default": -1.5707963267948966
+        },
+        "angle_max": {
+          "description": "The maximum scan angle in radians",
+          "type": "number",
+          "default": 1.5707963267948966
+        },
+        "angle_increment": {
+          "description": "Resolution of laser scan in radians per ray",
+          "type": "number",
+          "default": 0.008726646259971648
+        },
+        "queue_size": {
+          "description": "Input point cloud queue size",
+          "type": "int",
+          "default": 0
+        },
+        "scan_time": {
+          "description": "The scan rate in seconds. Only used to populate the scan_time field of the output laser scan message",
+          "type": "number",
+          "default": 0.03333333333333333
+        },
+        "range_min": {
+          "description": "The minimum ranges to return in meters",
+          "type": "number",
+          "default": 0.45
+        },
+        "range_max": {
+          "description": "The maximum ranges to return in meters",
+          "type": "number",
+          "default": 4.0
+        },
+        "target_frame": {
+          "description": "If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud",
+          "type": "string",
+          "default": null
+        },
+        "transform_tolerance": {
+          "description": "Time tolerance for transform lookups. Only used if a target_frame is provided",
+          "type": "number",
+          "default": 0.01
+        },
+        "inf_epsilon": {
+            "description": "Precison tolerance limit, only used if use_inf is true",
+            "type": "double",
+            "default": 1.0
+        },
+        "use_inf": {
+          "description": "If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf",
+          "type": "bool",
+          "default": true
+        }
+      },
+      "required": [
+        "min_height",
+        "max_height",
+        "angle_min",
+        "angle_max",
+        "angle_increment",
+        "queue_size",
+        "scan_time",
+        "range_min",
+        "range_max"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/laserscan_to_pointcloud_node"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

src/laserscan_to_pointcloud_node.cpp

@@ -62,12 +62,16 @@ namespace pointcloud_to_laserscan
 LaserScanToPointCloudNode::LaserScanToPointCloudNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("laserscan_to_pointcloud", options)
 {
-  target_frame_ = this->declare_parameter("target_frame", "");
-  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  target_frame_ = this->declare_parameter<std::string>("target_frame");
+  tolerance_ = this->declare_parameter<double>("transform_tolerance");
   // TODO(hidmic): adjust default input queue size based on actual concurrency levels
   // achievable by the associated executor
-  input_queue_size_ = this->declare_parameter(
-    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
+  input_queue_size_ = this->declare_parameter<int>("queue_size");
+
+  // use all cores if needed 
+  if (input_queue_size_ == QUEUE_SIZE_AUTO) {
+    input_queue_size_ = static_cast<int>(std::thread::hardware_concurrency());
+  }
 
   pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
 

src/pointcloud_to_laserscan_node.cpp

@@ -126,22 +126,27 @@ namespace pointcloud_to_laserscan
 PointCloudToLaserScanNode::PointCloudToLaserScanNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("pointcloud_to_laserscan", options)
 {
-  target_frame_ = this->declare_parameter("target_frame", "");
-  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  target_frame_ = this->declare_parameter<std::string>("target_frame");
+  tolerance_ = this->declare_parameter<double>("transform_tolerance");
   // TODO(hidmic): adjust default input queue size based on actual concurrency levels
   // achievable by the associated executor
-  input_queue_size_ = this->declare_parameter(
-    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
-  min_height_ = this->declare_parameter("min_height", std::numeric_limits<double>::min());
-  max_height_ = this->declare_parameter("max_height", std::numeric_limits<double>::max());
-  angle_min_ = this->declare_parameter("angle_min", -M_PI);
-  angle_max_ = this->declare_parameter("angle_max", M_PI);
-  angle_increment_ = this->declare_parameter("angle_increment", M_PI / 180.0);
-  scan_time_ = this->declare_parameter("scan_time", 1.0 / 30.0);
-  range_min_ = this->declare_parameter("range_min", 0.0);
-  range_max_ = this->declare_parameter("range_max", std::numeric_limits<double>::max());
-  inf_epsilon_ = this->declare_parameter("inf_epsilon", 1.0);
-  use_inf_ = this->declare_parameter("use_inf", true);
+  input_queue_size_ = this->declare_parameter<int>(
+    "queue_size");
+  min_height_ = this->declare_parameter<double>("min_height");
+  max_height_ = this->declare_parameter<double>("max_height");
+  angle_min_ = this->declare_parameter<double>("angle_min");
+  angle_max_ = this->declare_parameter<double>("angle_max");
+  angle_increment_ = this->declare_parameter<double>("angle_increment");
+  scan_time_ = this->declare_parameter<double>("scan_time");
+  range_min_ = this->declare_parameter<double>("range_min");
+  range_max_ = this->declare_parameter<double>("range_max");
+  inf_epsilon_ = this->declare_parameter<double>("inf_epsilon");
+  use_inf_ = this->declare_parameter<bool>("use_inf");
+
+  // use all cores if needed 
+  if (input_queue_size_ == QUEUE_SIZE_AUTO_) {
+    input_queue_size_ = static_cast<int>(std::thread::hardware_concurrency());
+  }
 
   laserscan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
     "~/output/laserscan", rclcpp::SensorDataQoS());