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