feat [ROS]: Allow to set parameters from a yaml file (#467)

Author: leroycorentin

ros/CMakeLists.txt

@@ -67,6 +67,6 @@ ament_target_dependencies(
 rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node)
 
 install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME})
-install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY launch rviz config DESTINATION share/${PROJECT_NAME}/)
 
 ament_package()

ros/README.md

@@ -30,6 +30,33 @@ and then,
 ros2 bag play <path>*.bag
 ```
 
+A preconfigured Rviz2 window is launched by default. To disable it, you can set the `visualize` launch argument to `false`:
+```sh
+ros2 launch kiss_icp odometry.launch.py topic:=<topic_name> visualize:=false
+```
+
+### Configuration
+
+The parameters for the KISS-ICP algorithm itself are written in a yaml configuration file. They are meant to be set before launching the node. An **example** yaml file is given in config/config.yaml. The file parsed by default is located in the share directory of the kiss_icp package (in your workspace that would be `install/kiss_icp/share/kiss_icp/config/config.yaml`), but any file path can be provided with the `config_file` launch argument, e.g.: `ros2 launch kiss_icp odometry.launch.py config_file:=/path/to/my/config_file.yaml`.
+
+The ROS-related parameters can be set via command-line when launching the node:
+* `base_frame`
+* `lidar_odom_frame`
+* `publish_odom_tf`
+* `invert_odom_tf`
+* `position_covariance`
+* `orientation_covariance`
+
+You can set them like so:
+```sh
+ros2 launch kiss_icp odometry.launch.py topic:=<topic_name> <parameter_name>:=<parameter_value>
+```
+For example:
+```sh
+ros2 launch kiss_icp odometry.launch.py topic:=/lidar/points base_frame:=lidar_cloud
+```
+Note that if you set the ROS-related parameters in the yaml configuration file and via command-line, the values in the yaml file will be selected in the end.
+
 ## Out of source builds
 
 Good news! If you don't have git or you don't need to change the core KISS-ICP library, you can just

ros/config/config.yaml

@@ -0,0 +1,34 @@
+# NOTE: Please note that this is just an example of the advanced configuration for the KISS-ICP
+# pipeline and it's not really meant to use for any particular dataset, is just to expose other
+# options that are disabled by default
+
+kiss_icp_node:
+  ros__parameters:
+    ####    /!\ If you set these ROS-related parameters in the file, they will overwrite the corresponding launch arguments   ####
+
+    # base_frame: ""
+    # lidar_odom_frame: "odom_lidar"
+    # publish_odom_tf: True
+    # invert_odom_tf: True
+    # position_covariance: 0.1
+    # orientation_covariance: 0.1
+
+    ####    Core KISS-ICP parameters    ####
+
+    data:
+      deskew: True
+      max_range: 100.0
+      min_range: 0.0
+
+    mapping:
+      # voxel_size: 1.0 # <- optional, default = max_range / 100.0
+      max_points_per_voxel: 20
+
+    adaptive_threshold:
+      initial_threshold: 2.0
+      min_motion_th: 0.1
+
+    registration:
+      max_num_iterations: 500 # <- optional
+      convergence_criterion: 0.0001 # <- optional
+      max_num_threads: 0 # <- optional, 0 means automatic

ros/launch/odometry.launch.py

@@ -20,6 +20,9 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
+import os
+
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import ExecuteProcess
 from launch.conditions import IfCondition
@@ -31,32 +34,11 @@
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
+PACKAGE_NAME = "kiss_icp"
 
-# This configuration parameters are not exposed thorught the launch system, meaning you can't modify
-# those throw the ros launch CLI. If you need to change these values, you could write your own
-# launch file and modify the 'parameters=' block from the Node class.
-class config:
-    # Preprocessing
-    max_range: float = 100.0
-    min_range: float = 0.0
-    deskew: bool = True
-
-    #  Mapping parameters
-    voxel_size: float = max_range / 100.0
-    max_points_per_voxel: int = 20
-
-    # Adaptive threshold
-    initial_threshold: float = 2.0
-    min_motion_th: float = 0.1
-
-    # Registration
-    max_num_iterations: int = 500  #
-    convergence_criterion: float = 0.0001
-    max_num_threads: int = 0
-
-    # Covariance diagonal values
-    position_covariance: float = 0.1
-    orientation_covariance: float = 0.1
+default_config_file = os.path.join(
+    get_package_share_directory(PACKAGE_NAME), "config", "config.yaml"
+)
 
 
 def generate_launch_description():
@@ -75,9 +57,14 @@ def generate_launch_description():
     publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True)
     invert_odom_tf = LaunchConfiguration("invert_odom_tf", default=True)
 
+    position_covariance = LaunchConfiguration("position_covariance", default=0.1)
+    orientation_covariance = LaunchConfiguration("orientation_covariance", default=0.1)
+
+    config_file = LaunchConfiguration("config_file", default=default_config_file)
+
     # KISS-ICP node
     kiss_icp_node = Node(
-        package="kiss_icp",
+        package=PACKAGE_NAME,
         executable="kiss_icp_node",
         name="kiss_icp_node",
         output="screen",
@@ -91,26 +78,13 @@ def generate_launch_description():
                 "lidar_odom_frame": lidar_odom_frame,
                 "publish_odom_tf": publish_odom_tf,
                 "invert_odom_tf": invert_odom_tf,
-                # KISS-ICP configuration
-                "max_range": config.max_range,
-                "min_range": config.min_range,
-                "deskew": config.deskew,
-                "max_points_per_voxel": config.max_points_per_voxel,
-                "voxel_size": config.voxel_size,
-                # Adaptive threshold
-                "initial_threshold": config.initial_threshold,
-                "min_motion_th": config.min_motion_th,
-                # Registration
-                "max_num_iterations": config.max_num_iterations,
-                "convergence_criterion": config.convergence_criterion,
-                "max_num_threads": config.max_num_threads,
-                # Fixed covariances
-                "position_covariance": config.position_covariance,
-                "orientation_covariance": config.orientation_covariance,
                 # ROS CLI arguments
                 "publish_debug_clouds": visualize,
                 "use_sim_time": use_sim_time,
+                "position_covariance": position_covariance,
+                "orientation_covariance": orientation_covariance,
             },
+            config_file,
         ],
     )
     rviz_node = Node(
@@ -119,7 +93,7 @@ def generate_launch_description():
         output="screen",
         arguments=[
             "-d",
-            PathJoinSubstitution([FindPackageShare("kiss_icp"), "rviz", "kiss_icp.rviz"]),
+            PathJoinSubstitution([FindPackageShare(PACKAGE_NAME), "rviz", "kiss_icp.rviz"]),
         ],
         condition=IfCondition(visualize),
     )

ros/src/OdometryServer.cpp

@@ -73,34 +73,8 @@ using utils::PointCloud2ToEigen;
 
 OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     : rclcpp::Node("kiss_icp_node", options) {
-    base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
-    lidar_odom_frame_ = declare_parameter<std::string>("lidar_odom_frame", lidar_odom_frame_);
-    publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
-    invert_odom_tf_ = declare_parameter<bool>("invert_odom_tf", invert_odom_tf_);
-    publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
-    position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
-    orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);
-
     kiss_icp::pipeline::KISSConfig config;
-    config.max_range = declare_parameter<double>("max_range", config.max_range);
-    config.min_range = declare_parameter<double>("min_range", config.min_range);
-    config.deskew = declare_parameter<bool>("deskew", config.deskew);
-    config.voxel_size = declare_parameter<double>("voxel_size", config.max_range / 100.0);
-    config.max_points_per_voxel =
-        declare_parameter<int>("max_points_per_voxel", config.max_points_per_voxel);
-    config.initial_threshold =
-        declare_parameter<double>("initial_threshold", config.initial_threshold);
-    config.min_motion_th = declare_parameter<double>("min_motion_th", config.min_motion_th);
-    config.max_num_iterations =
-        declare_parameter<int>("max_num_iterations", config.max_num_iterations);
-    config.convergence_criterion =
-        declare_parameter<double>("convergence_criterion", config.convergence_criterion);
-    config.max_num_threads = declare_parameter<int>("max_num_threads", config.max_num_threads);
-    if (config.max_range < config.min_range) {
-        RCLCPP_WARN(get_logger(),
-                    "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
-        config.min_range = 0.0;
-    }
+    initializeParameters(config);
 
     // Construct the main KISS-ICP odometry node
     kiss_icp_ = std::make_unique<kiss_icp::pipeline::KissICP>(config);
@@ -128,6 +102,57 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
 }
 
+void OdometryServer::initializeParameters(kiss_icp::pipeline::KISSConfig &config) {
+    RCLCPP_INFO(this->get_logger(), "Initializing parameters");
+
+    base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
+    RCLCPP_INFO(this->get_logger(), "\tBase frame: %s", base_frame_.c_str());
+    lidar_odom_frame_ = declare_parameter<std::string>("lidar_odom_frame", lidar_odom_frame_);
+    RCLCPP_INFO(this->get_logger(), "\tLiDAR odometry frame: %s", lidar_odom_frame_.c_str());
+    publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
+    RCLCPP_INFO(this->get_logger(), "\tPublish odometry transform: %d", publish_odom_tf_);
+    invert_odom_tf_ = declare_parameter<bool>("invert_odom_tf", invert_odom_tf_);
+    RCLCPP_INFO(this->get_logger(), "\tInvert odometry transform: %d", invert_odom_tf_);
+    publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
+    RCLCPP_INFO(this->get_logger(), "\tPublish debug clouds: %d", publish_debug_clouds_);
+    position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
+    RCLCPP_INFO(this->get_logger(), "\tPosition covariance: %.2f", position_covariance_);
+    orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);
+    RCLCPP_INFO(this->get_logger(), "\tOrientation covariance: %.2f", orientation_covariance_);
+
+    config.max_range = declare_parameter<double>("data.max_range", config.max_range);
+    RCLCPP_INFO(this->get_logger(), "\tMax range: %.2f", config.max_range);
+    config.min_range = declare_parameter<double>("data.min_range", config.min_range);
+    RCLCPP_INFO(this->get_logger(), "\tMin range: %.2f", config.min_range);
+    config.deskew = declare_parameter<bool>("data.deskew", config.deskew);
+    RCLCPP_INFO(this->get_logger(), "\tDeskew: %d", config.deskew);
+    config.voxel_size = declare_parameter<double>("mapping.voxel_size", config.max_range / 100.0);
+    RCLCPP_INFO(this->get_logger(), "\tVoxel size: %.2f", config.voxel_size);
+    config.max_points_per_voxel =
+        declare_parameter<int>("mapping.max_points_per_voxel", config.max_points_per_voxel);
+    RCLCPP_INFO(this->get_logger(), "\tMax points per voxel: %d", config.max_points_per_voxel);
+    config.initial_threshold =
+        declare_parameter<double>("adaptive_threshold.initial_threshold", config.initial_threshold);
+    RCLCPP_INFO(this->get_logger(), "\tInitial threshold: %.2f", config.initial_threshold);
+    config.min_motion_th =
+        declare_parameter<double>("adaptive_threshold.min_motion_th", config.min_motion_th);
+    RCLCPP_INFO(this->get_logger(), "\tMin motion threshold: %.2f", config.min_motion_th);
+    config.max_num_iterations =
+        declare_parameter<int>("registration.max_num_iterations", config.max_num_iterations);
+    RCLCPP_INFO(this->get_logger(), "\tMax number of iterations: %d", config.max_num_iterations);
+    config.convergence_criterion = declare_parameter<double>("registration.convergence_criterion",
+                                                             config.convergence_criterion);
+    RCLCPP_INFO(this->get_logger(), "\tConvergence criterion: %.2f", config.convergence_criterion);
+    config.max_num_threads =
+        declare_parameter<int>("registration.max_num_threads", config.max_num_threads);
+    RCLCPP_INFO(this->get_logger(), "\tMax number of threads: %d", config.max_num_threads);
+    if (config.max_range < config.min_range) {
+        RCLCPP_WARN(get_logger(),
+                    "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
+        config.min_range = 0.0;
+    }
+}
+
 void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
     const auto cloud_frame_id = msg->header.frame_id;
     const auto points = PointCloud2ToEigen(msg);

ros/src/OdometryServer.hpp

@@ -45,6 +45,10 @@ class OdometryServer : public rclcpp::Node {
     explicit OdometryServer(const rclcpp::NodeOptions &options);
 
 private:
+    /// Declare ROS parameters and set the associated variables (in this class and in the provided
+    /// config object)
+    void initializeParameters(kiss_icp::pipeline::KISSConfig &config);
+
     /// Register new frame
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);