Add LiDAR odometry to tf tree (#371)

* Add LiDAR odometry to tf tree

* Transform the internal LiDAR odometry map to lidar frame

I honestly do not know why this is not working, but we can see in the
future. It's just to save so computation in the ROS node

* Style

ros/launch/odometry.launch.py

@@ -62,18 +62,19 @@ class config:
 def generate_launch_description():
     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="")
 
+    # tf tree configuration, these are the likely parameters to change and nothing else
+    base_frame = LaunchConfiguration("base_frame", default="")  # (base_link/base_footprint)
+    lidar_odom_frame = LaunchConfiguration("lidar_odom_frame", default="odom_lidar")
+    publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True)
+    invert_odom_tf = LaunchConfiguration("invert_odom_tf", default=True)
+
     # KISS-ICP node
     kiss_icp_node = Node(
         package="kiss_icp",
@@ -87,8 +88,9 @@ def generate_launch_description():
             {
                 # ROS node configuration
                 "base_frame": base_frame,
-                "odom_frame": odom_frame,
+                "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,
@@ -123,10 +125,11 @@ def generate_launch_description():
     )
 
     bagfile_play = ExecuteProcess(
-        cmd=["ros2", "bag", "play", bagfile],
+        cmd=["ros2", "bag", "play", "--rate", "1", bagfile, "--clock", "1000.0"],
         output="screen",
         condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])),
     )
+
     return LaunchDescription(
         [
             kiss_icp_node,

ros/rviz/kiss_icp.rviz

@@ -75,7 +75,7 @@ Visualization Manager:
           Color: 61; 229; 255
           Color Transformer: FlatColor
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Max Intensity: 4096
@@ -96,7 +96,7 @@ Visualization Manager:
             Value: /kiss/keypoints
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -133,60 +133,49 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: point_clouds
-    - Class: rviz_common/Group
-      Displays:
-        - Angle Tolerance: 0.10000000149011612
-          Class: rviz_default_plugins/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: true
-          Keep: 100
-          Name: odometry
-          Position Tolerance: 0.10000000149011612
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.10000000149011612
-            Color: 255; 25; 0
-            Head Length: 0.30000001192092896
-            Head Radius: 0.10000000149011612
-            Shaft Length: 1
-            Shaft Radius: 0.05000000074505806
-            Value: Axes
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Best Effort
-            Value: /kiss/odometry
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
           Value: true
-      Enabled: false
-      Name: odometry
-    - Class: rviz_default_plugins/Axes
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: false
       Enabled: true
-      Length: 1
-      Name: odom_frame
-      Radius: 0.10000000149011612
-      Reference Frame: odom
+      Keep: 100
+      Name: LiDAR Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /kiss/odometry
       Value: true
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
-    Fixed Frame: odom
+    Fixed Frame: odom_lidar
     Frame Rate: 30
   Name: root
   Tools:
@@ -229,25 +218,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 178.319580078125
+      Distance: 51.88520812988281
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -68.01193237304688
-        Y: 258.05126953125
-        Z: -27.22064781188965
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.49020177125930786
-      Target Frame: <Fixed Frame>
+      Pitch: 0.7853981852531433
+      Target Frame: odom_lidar
       Value: Orbit (rviz_default_plugins)
-      Yaw: 3.340390920639038
+      Yaw: 0.7853981852531433
     Saved: ~
 Window Geometry:
   Displays:

ros/src/OdometryServer.cpp

@@ -74,8 +74,9 @@ 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_);
-    odom_frame_ = declare_parameter<std::string>("odom_frame", odom_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);
@@ -152,6 +153,7 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
     const auto cloud_frame_id = header.frame_id;
     const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
+    const auto moving_frame = egocentric_estimation ? cloud_frame_id : base_frame_;
     const auto pose = [&]() -> Sophus::SE3d {
         if (egocentric_estimation) return kiss_pose;
         const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
@@ -162,16 +164,23 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
         transform_msg.header.stamp = header.stamp;
-        transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
-        transform_msg.transform = tf2::sophusToTransform(pose);
+        if (invert_odom_tf_) {
+            transform_msg.header.frame_id = moving_frame;
+            transform_msg.child_frame_id = lidar_odom_frame_;
+            transform_msg.transform = tf2::sophusToTransform(pose.inverse());
+        } else {
+            transform_msg.header.frame_id = lidar_odom_frame_;
+            transform_msg.child_frame_id = moving_frame;
+            transform_msg.transform = tf2::sophusToTransform(pose);
+        }
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
     // publish odometry msg
     nav_msgs::msg::Odometry odom_msg;
     odom_msg.header.stamp = header.stamp;
-    odom_msg.header.frame_id = odom_frame_;
+    odom_msg.header.frame_id = lidar_odom_frame_;
+    odom_msg.child_frame_id = moving_frame;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
     odom_msg.pose.covariance.fill(0.0);
     odom_msg.pose.covariance[0] = position_covariance_;

ros/src/OdometryServer.hpp

@@ -61,6 +61,7 @@ class OdometryServer : public rclcpp::Node {
     std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     std::unique_ptr<tf2_ros::Buffer> tf2_buffer_;
     std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
+    bool invert_odom_tf_;
     bool publish_odom_tf_;
     bool publish_debug_clouds_;
 
@@ -77,7 +78,7 @@ class OdometryServer : public rclcpp::Node {
     std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;
 
     /// Global/map coordinate frame.
-    std::string odom_frame_{"odom"};
+    std::string lidar_odom_frame_{"odom_lidar"};
     std::string base_frame_{};
 
     /// Covariance diagonal