Refactor ROS parametrization

ros/CMakeLists.txt

@@ -63,7 +63,7 @@ ament_target_dependencies(
   geometry_msgs
   tf2_ros)
 
-rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node)
+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}/)

ros/launch/odometry.launch.py

@@ -20,9 +20,8 @@
 # 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.
-
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.actions import ExecuteProcess
 from launch.conditions import IfCondition
 from launch.substitutions import (
     LaunchConfiguration,
@@ -33,58 +32,105 @@
 from launch_ros.substitutions import FindPackageShare
 
 
+# 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 = 5.0
+    deskew: bool = False
+
+    #  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
+
+
 def generate_launch_description():
-    current_pkg = FindPackageShare("kiss_icp")
+    use_sim_time = LaunchConfiguration("use_sim_time", default="true")
+
+    # tf tree configuration, these are the likely 3 parameters to change and nothing else
+    base_frame = LaunchConfiguration("base_frame", default="")
+    odom_frame = LaunchConfiguration("odom_frame", default="odom")
+    publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True)
+
+    # ROS configuration
+    pointcloud_topic = LaunchConfiguration("topic")
+    visualize = LaunchConfiguration("visualize", default="true")
+
+    # Optional ros bag play
+    bagfile = LaunchConfiguration("bagfile", default="")
+
+    # KISS-ICP node
+    kiss_icp_node = Node(
+        package="kiss_icp",
+        executable="kiss_icp_node",
+        name="kiss_icp_node",
+        output="screen",
+        remappings=[
+            ("pointcloud_topic", pointcloud_topic),
+        ],
+        parameters=[
+            {
+                # ROS node configuration
+                "base_frame": base_frame,
+                "odom_frame": odom_frame,
+                "publish_odom_tf": publish_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,
+            },
+        ],
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        output="screen",
+        arguments=[
+            "-d",
+            PathJoinSubstitution([FindPackageShare("kiss_icp"), "rviz", "kiss_icp.rviz"]),
+        ],
+        condition=IfCondition(visualize),
+    )
+
+    bagfile_play = ExecuteProcess(
+        cmd=["ros2", "bag", "play", bagfile],
+        output="screen",
+        condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])),
+    )
     return LaunchDescription(
         [
-            # ROS 2 parameters
-            DeclareLaunchArgument("topic", description="sensor_msg/PointCloud2 topic to process"),
-            DeclareLaunchArgument("bagfile", default_value=""),
-            DeclareLaunchArgument("visualize", default_value="true"),
-            DeclareLaunchArgument("odom_frame", default_value="odom"),
-            DeclareLaunchArgument("base_frame", default_value=""),
-            DeclareLaunchArgument("publish_odom_tf", default_value="true"),
-            # KISS-ICP parameters
-            DeclareLaunchArgument("deskew", default_value="false"),
-            DeclareLaunchArgument("max_range", default_value="100.0"),
-            DeclareLaunchArgument("min_range", default_value="5.0"),
-            # This thing is still not suported: https://github.com/ros2/launch/issues/290#issuecomment-1438476902
-            #  DeclareLaunchArgument("voxel_size", default_value=None),
-            Node(
-                package="kiss_icp",
-                executable="odometry_node",
-                name="odometry_node",
-                output="screen",
-                remappings=[("pointcloud_topic", LaunchConfiguration("topic"))],
-                parameters=[
-                    {
-                        "odom_frame": LaunchConfiguration("odom_frame"),
-                        "base_frame": LaunchConfiguration("base_frame"),
-                        "max_range": LaunchConfiguration("max_range"),
-                        "min_range": LaunchConfiguration("min_range"),
-                        "deskew": LaunchConfiguration("deskew"),
-                        #  "voxel_size": LaunchConfiguration("voxel_size"),
-                        "max_points_per_voxel": 20,
-                        "initial_threshold": 2.0,
-                        "min_motion_th": 0.1,
-                        "publish_odom_tf": LaunchConfiguration("publish_odom_tf"),
-                        "visualize": LaunchConfiguration("visualize"),
-                    }
-                ],
-            ),
-            Node(
-                package="rviz2",
-                executable="rviz2",
-                output={"both": "log"},
-                arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "kiss_icp.rviz"])],
-                condition=IfCondition(LaunchConfiguration("visualize")),
-            ),
-            ExecuteProcess(
-                cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")],
-                output="screen",
-                condition=IfCondition(
-                    PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"])
-                ),
-            ),
+            kiss_icp_node,
+            rviz_node,
+            bagfile_play,
         ]
     )

ros/src/OdometryServer.cpp

@@ -52,11 +52,13 @@ using utils::GetTimestamps;
 using utils::PointCloud2ToEigen;
 
 OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
-    : rclcpp::Node("odometry_node", options) {
+    : rclcpp::Node("kiss_icp_node", options) {
     base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
-    publish_debug_clouds_ = declare_parameter<bool>("visualize", publish_debug_clouds_);
+    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);
@@ -68,6 +70,11 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     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");
@@ -163,6 +170,13 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
     odom_msg.header.stamp = stamp;
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
+    odom_msg.pose.covariance.fill(0.0);
+    odom_msg.pose.covariance[0] = position_covariance_;
+    odom_msg.pose.covariance[7] = position_covariance_;
+    odom_msg.pose.covariance[14] = position_covariance_;
+    odom_msg.pose.covariance[21] = orientation_covariance_;
+    odom_msg.pose.covariance[28] = orientation_covariance_;
+    odom_msg.pose.covariance[35] = orientation_covariance_;
     odom_publisher_->publish(std::move(odom_msg));
 }
 

ros/src/OdometryServer.hpp

@@ -85,6 +85,10 @@ class OdometryServer : public rclcpp::Node {
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};
     std::string base_frame_{};
+
+    /// Covariance diagonal
+    double position_covariance_;
+    double orientation_covariance_;
 };
 
 }  // namespace kiss_icp_ros