Add support for multi-sensor scenarios

ros/launch/odometry.launch.py

@@ -42,9 +42,8 @@ def generate_launch_description():
             DeclareLaunchArgument("bagfile", default_value=""),
             DeclareLaunchArgument("visualize", default_value="true"),
             DeclareLaunchArgument("odom_frame", default_value="odom"),
-            DeclareLaunchArgument("child_frame", default_value="base_link"),
-            DeclareLaunchArgument("publish_odom_tf", default_value="true"),
-            DeclareLaunchArgument("publish_alias_tf", default_value="true"),
+            DeclareLaunchArgument("base_frame", default_value="base_footprint"),
+            DeclareLaunchArgument("publish_odom_tf", default_value="false"),
             # KISS-ICP parameters
             DeclareLaunchArgument("deskew", default_value="false"),
             DeclareLaunchArgument("max_range", default_value="100.0"),
@@ -60,7 +59,7 @@ def generate_launch_description():
                 parameters=[
                     {
                         "odom_frame": LaunchConfiguration("odom_frame"),
-                        "child_frame": LaunchConfiguration("child_frame"),
+                        "base_frame": LaunchConfiguration("base_frame"),
                         "max_range": LaunchConfiguration("max_range"),
                         "min_range": LaunchConfiguration("min_range"),
                         "deskew": LaunchConfiguration("deskew"),
@@ -69,7 +68,6 @@ def generate_launch_description():
                         "initial_threshold": 2.0,
                         "min_motion_th": 0.1,
                         "publish_odom_tf": LaunchConfiguration("publish_odom_tf"),
-                        "publish_alias_tf": LaunchConfiguration("publish_alias_tf"),
                     }
                 ],
             ),

ros/ros2/OdometryServer.cpp

@@ -22,6 +22,7 @@
 // SOFTWARE.
 #include <Eigen/Core>
 #include <memory>
+#include <sophus/se3.hpp>
 #include <utility>
 #include <vector>
 
@@ -54,9 +55,8 @@ using utils::PointCloud2ToEigen;
 OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     : rclcpp::Node("odometry_node", options) {
     // clang-format off
-    child_frame_ = declare_parameter<std::string>("child_frame", child_frame_);
+    base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
-    publish_alias_tf_ = declare_parameter<bool>("publish_alias_tf", publish_alias_tf_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
     config_.max_range = declare_parameter<double>("max_range", config_.max_range);
     config_.min_range = declare_parameter<double>("min_range", config_.min_range);
@@ -80,111 +80,94 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
         std::bind(&OdometryServer::RegisterFrame, this, std::placeholders::_1));
 
     // Initialize publishers
-    rclcpp::QoS qos(rclcpp::KeepLast{queue_size_});
-    odom_publisher_ = create_publisher<nav_msgs::msg::Odometry>("odometry", qos);
-    frame_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("frame", qos);
-    kpoints_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("keypoints", qos);
-    map_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("local_map", qos);
+    rclcpp::QoS qos((rclcpp::SystemDefaultsQoS()));
+    odom_publisher_ = create_publisher<nav_msgs::msg::Odometry>("/kiss/odometry", qos);
+    map_publisher_ = create_publisher<sensor_msgs::msg::PointCloud2>("/kiss/local_map", qos);
+    traj_publisher_ = create_publisher<nav_msgs::msg::Path>("/kiss/trajectory", qos);
+    path_msg_.header.frame_id = odom_frame_;
 
     // Initialize the transform broadcaster
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
-
-    // Initialize trajectory publisher
-    path_msg_.header.frame_id = odom_frame_;
-    traj_publisher_ = create_publisher<nav_msgs::msg::Path>("trajectory", qos);
-
-    // Broadcast a static transformation that links with identity the specified base link to the
-    // pointcloud_frame, basically to always be able to visualize the frame in rviz
-    if (publish_alias_tf_ && child_frame_ != "base_link") {
-        rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
-        options.qos_overriding_options = rclcpp::QosOverridingOptions{
-            rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::History,
-            rclcpp::QosPolicyKind::Reliability};
-        options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
-
-        static auto br = std::make_shared<tf2_ros::StaticTransformBroadcaster>(
-            *this, tf2_ros::StaticBroadcasterQoS(), options);
-
-        geometry_msgs::msg::TransformStamped alias_transform_msg;
-        alias_transform_msg.header.stamp = this->get_clock()->now();
-        alias_transform_msg.transform.translation.x = 0.0;
-        alias_transform_msg.transform.translation.y = 0.0;
-        alias_transform_msg.transform.translation.z = 0.0;
-        alias_transform_msg.transform.rotation.x = 0.0;
-        alias_transform_msg.transform.rotation.y = 0.0;
-        alias_transform_msg.transform.rotation.z = 0.0;
-        alias_transform_msg.transform.rotation.w = 1.0;
-        alias_transform_msg.header.frame_id = child_frame_;
-        alias_transform_msg.child_frame_id = "base_link";
-        br->sendTransform(alias_transform_msg);
-    }
+    tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    tf2_buffer_->setUsingDedicatedThread(true);
+    tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_buffer_);
 
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
 }
 
+Sophus::SE3d OdometryServer::CloudToBaseTf(const std::string &cloud_frame_id) const {
+    std::string err_msg;
+    if (tf2_buffer_->_frameExists(base_frame_) &&     //
+        tf2_buffer_->_frameExists(cloud_frame_id) &&  //
+        tf2_buffer_->canTransform(cloud_frame_id, base_frame_, tf2::TimePointZero, &err_msg)) {
+        try {
+            auto tf = tf2_buffer_->lookupTransform(cloud_frame_id, base_frame_, tf2::TimePointZero);
+            return tf2::transformToSophus(tf);
+        } catch (tf2::TransformException &ex) {
+            RCLCPP_WARN(this->get_logger(), "%s", ex.what());
+        }
+    }
+    RCLCPP_ERROR(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str());
+    return {};
+}
+
 void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
+    const auto cloud_frame_id = msg->header.frame_id;
     const auto points = PointCloud2ToEigen(msg);
     const auto timestamps = [&]() -> std::vector<double> {
         if (!config_.deskew) return {};
         return GetTimestamps(msg);
     }();
+    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
 
     // Register frame, main entry point to KISS-ICP pipeline
     const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);
 
-    // PublishPose
-    const auto pose = odometry_.poses().back();
+    // Compute the pose using KISS, ego-centric to the LiDAR
+    const Sophus::SE3d kiss_pose = odometry_.poses().back();
+
+    // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto pose = [&]() -> Sophus::SE3d {
+        if (egocentric_estimation) return kiss_pose;
+        const Sophus::SE3d cloud2base = CloudToBaseTf(cloud_frame_id);
+        return cloud2base.inverse() * kiss_pose * cloud2base;
+    }();
 
-    // Convert from Eigen to ROS types
-    const Eigen::Vector3d t_current = pose.translation();
-    const Eigen::Quaterniond q_current = pose.unit_quaternion();
+    // Header for point clouds and stuff seen from desired odom_frame
+    std_msgs::msg::Header odom_header = msg->header;
+    odom_header.frame_id = odom_frame_;
 
     // Broadcast the tf
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
-        transform_msg.header.stamp = msg->header.stamp;
-        transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = child_frame_;
-        transform_msg.transform.rotation.x = q_current.x();
-        transform_msg.transform.rotation.y = q_current.y();
-        transform_msg.transform.rotation.z = q_current.z();
-        transform_msg.transform.rotation.w = q_current.w();
-        transform_msg.transform.translation.x = t_current.x();
-        transform_msg.transform.translation.y = t_current.y();
-        transform_msg.transform.translation.z = t_current.z();
+        transform_msg.header = odom_header;
+        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
+        transform_msg.transform = tf2::sophusToTransform(pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
     // publish trajectory msg
     geometry_msgs::msg::PoseStamped pose_msg;
-    pose_msg.pose.orientation.x = q_current.x();
-    pose_msg.pose.orientation.y = q_current.y();
-    pose_msg.pose.orientation.z = q_current.z();
-    pose_msg.pose.orientation.w = q_current.w();
-    pose_msg.pose.position.x = t_current.x();
-    pose_msg.pose.position.y = t_current.y();
-    pose_msg.pose.position.z = t_current.z();
-    pose_msg.header.stamp = msg->header.stamp;
-    pose_msg.header.frame_id = odom_frame_;
+    pose_msg.header = odom_header;
+    pose_msg.pose = tf2::sophusToPose(pose);
     path_msg_.poses.push_back(pose_msg);
     traj_publisher_->publish(path_msg_);
 
     // publish odometry msg
     auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();
-    odom_msg->header = pose_msg.header;
-    odom_msg->child_frame_id = child_frame_;
+    odom_msg->header = odom_header;
     odom_msg->pose.pose = pose_msg.pose;
     odom_publisher_->publish(std::move(odom_msg));
 
-    // Publish KISS-ICP internal data, just for debugging
-    auto frame_header = msg->header;
-    frame_header.frame_id = child_frame_;
-    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, frame_header)));
-    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, frame_header)));
-
-    // Map is referenced to the odometry_frame
-    auto local_map_header = msg->header;
-    local_map_header.frame_id = odom_frame_;
-    map_publisher_->publish(std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header)));
+    // Publish KISS-ICP internal map, just for debugging
+    if (map_publisher_->get_subscription_count() > 0) {
+        const auto kiss_map = odometry_.LocalMap();
+        if (egocentric_estimation) {
+            map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header)));
+        } else {
+            const auto T = CloudToBaseTf(cloud_frame_id).inverse();
+            map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, T, odom_header)));
+        }
+    }
 }
 }  // namespace kiss_icp_ros

ros/ros2/OdometryServer.hpp

@@ -26,7 +26,9 @@
 #include "kiss_icp/pipeline/KissICP.hpp"
 
 // ROS 2
+#include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <nav_msgs/msg/odometry.hpp>
 #include <nav_msgs/msg/path.hpp>
@@ -45,12 +47,14 @@ class OdometryServer : public rclcpp::Node {
     /// Register new frame
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
 
-private:
-    /// Ros node stuff
-    size_t queue_size_{1};
+    /// If user ask, report the pose in the given child_frame
+    Sophus::SE3d CloudToBaseTf(const std::string &pointcloud_frame_id) const;
 
+private:
     /// Tools for broadcasting TFs.
     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 publish_odom_tf_;
     bool publish_alias_tf_;
 
@@ -59,8 +63,6 @@ class OdometryServer : public rclcpp::Node {
 
     /// Data publishers.
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr frame_publisher_;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr kpoints_publisher_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher_;
 
     /// Path publisher
@@ -73,7 +75,7 @@ class OdometryServer : public rclcpp::Node {
 
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};
-    std::string child_frame_{"base_link"};
+    std::string base_frame_{};
 };
 
 }  // namespace kiss_icp_ros

ros/ros2/Utils.hpp

@@ -27,13 +27,57 @@
 #include <cstddef>
 #include <memory>
 #include <regex>
+#include <sophus/se3.hpp>
 #include <string>
 #include <vector>
 
 // ROS 2
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
+namespace tf2 {
+
+inline geometry_msgs::msg::Transform sophusToTransform(const Sophus::SE3d &T) {
+    geometry_msgs::msg::Transform t;
+    t.translation.x = T.translation().x();
+    t.translation.y = T.translation().y();
+    t.translation.z = T.translation().z();
+
+    Eigen::Quaterniond q(T.so3().unit_quaternion());
+    t.rotation.x = q.x();
+    t.rotation.y = q.y();
+    t.rotation.z = q.z();
+    t.rotation.w = q.w();
+
+    return t;
+}
+
+inline geometry_msgs::msg::Pose sophusToPose(const Sophus::SE3d &T) {
+    geometry_msgs::msg::Pose t;
+    t.position.x = T.translation().x();
+    t.position.y = T.translation().y();
+    t.position.z = T.translation().z();
+
+    Eigen::Quaterniond q(T.so3().unit_quaternion());
+    t.orientation.x = q.x();
+    t.orientation.y = q.y();
+    t.orientation.z = q.z();
+    t.orientation.w = q.w();
+
+    return t;
+}
+
+inline Sophus::SE3d transformToSophus(const geometry_msgs::msg::TransformStamped &transform) {
+    const auto &t = transform.transform;
+    return Sophus::SE3d(
+        Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z),
+        Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z));
+}
+}  // namespace tf2
+
 namespace kiss_icp_ros::utils {
 
 using PointCloud2 = sensor_msgs::msg::PointCloud2;
@@ -173,6 +217,16 @@ inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::
     return msg;
 }
 
+inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
+                                                       const Sophus::SE3d &T,
+                                                       const Header &header) {
+    std::vector<Eigen::Vector3d> points_t;
+    points_t.resize(points.size());
+    std::transform(points.cbegin(), points.cend(), points_t.begin(),
+                   [&](const auto &point) { return T * point; });
+    return EigenToPointCloud2(points_t, header);
+}
+
 inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
                                                        const std::vector<double> &timestamps,
                                                        const Header &header) {
@@ -181,5 +235,4 @@ inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::
     FillPointCloud2Timestamp(timestamps, *msg);
     return msg;
 }
-
 }  // namespace kiss_icp_ros::utils