Merge branch 'main' of github.com:PRBonn/kiss-icp into main

python/kiss_icp/tools/visualizer.py

@@ -29,7 +29,7 @@
 
 import numpy as np
 
-YELLOW = np.array([1, 0.706, 0])
+CYAN = np.array([0.24, 0.898, 1])
 RED = np.array([128, 0, 0]) / 255.0
 BLACK = np.array([0, 0, 0]) / 255.0
 BLUE = np.array([0.4, 0.5, 0.9])
@@ -198,7 +198,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Source hot frame
         if self.render_source:
             self.source.points = self.o3d.utility.Vector3dVector(source)
-            self.source.paint_uniform_color(YELLOW)
+            self.source.paint_uniform_color(CYAN)
             if self.global_view:
                 self.source.transform(pose)
         else:
@@ -207,7 +207,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Keypoints
         if self.render_keypoints:
             self.keypoints.points = self.o3d.utility.Vector3dVector(keypoints)
-            self.keypoints.paint_uniform_color(YELLOW)
+            self.keypoints.paint_uniform_color(CYAN)
             if self.global_view:
                 self.keypoints.transform(pose)
         else:

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/rviz/kiss_icp.rviz

@@ -23,7 +23,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: frame_deskew
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -38,7 +38,7 @@ Visualization Manager:
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 0
+          Color: 61; 229; 255
           Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
@@ -52,7 +52,7 @@ Visualization Manager:
           Selectable: true
           Size (Pixels): 3
           Size (m): 0.019999999552965164
-          Style: Flat Squares
+          Style: Points
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -84,8 +84,8 @@ Visualization Manager:
           Name: kiss_keypoints
           Position Transformer: XYZ
           Selectable: true
-          Size (Pixels): 10
-          Size (m): 1
+          Size (Pixels): 3
+          Size (m): 0.20000000298023224
           Style: Points
           Topic:
             Depth: 5
@@ -100,14 +100,14 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 98.31531524658203
-            Min Value: 0.6587954163551331
+            Max Value: 14.369325637817383
+            Min Value: 0.20239250361919403
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: AxisColor
+          Color: 102; 102; 102
+          Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -174,7 +174,7 @@ Visualization Manager:
             Reliability Policy: Best Effort
             Value: /kiss/odometry
           Value: true
-      Enabled: true
+      Enabled: false
       Name: odometry
     - Class: rviz_default_plugins/Axes
       Enabled: true
@@ -185,7 +185,7 @@ Visualization Manager:
       Value: true
   Enabled: true
   Global Options:
-    Background Color: 48; 48; 48
+    Background Color: 0; 0; 0
     Fixed Frame: odom
     Frame Rate: 30
   Name: root
@@ -229,16 +229,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 39.488563537597656
+      Distance: 178.319580078125
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.482842445373535
-        Y: 4.3972296714782715
-        Z: 10.242613792419434
+        X: -68.01193237304688
+        Y: 258.05126953125
+        Z: -27.22064781188965
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false