From 062cc4434d16a591f565fe6c9aacb2fdb8615a3d Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 27 Oct 2023 13:09:24 +0200 Subject: [PATCH] Add support for multi-sensor scenarios (#232) * Fix TF tree usage in ROS 2 wrapper Signed-off-by: Ignacio Vizzo * Transform the pose instead of the pointcloud * remove include * Remove default base_frame parameter value This will make KISS-ICP work ego-centric as default * Remove unused variable * Do not reuse pose_msg.pose * I'm not ever sure why we did that in first instance * Be more explicit about the stamps and frame_id of the headers * Extract publishing mechanism to a new function (#246) * Convert class member function to more useful utility And fix a small bug, the order was of the transformation before was the opposite, and therefore we were obtaining base2cloud. Since we multiply by both sides we can't really see the difference, but it was conceptually wrong. * Bring back debugging clouds to ros driver * re-arange logic on when and when not to publish clouds * fix lofic * Update rviz2 * Loosing my mind already with this debugging info * Backport tf multisensor fix (#245) * Build system changes for tf fix * Modify params for tf fix * Add ROS 1 tf fixes similar to ROS 2 * Update rviz config * Remove unused debug publishers * Remove unnecessary smart pointers * Update ROS 1 to match ROS 2 changes * Fix style * Remove sophus from build system Fixing now the CI is a big pain * Remove unnecessary alias --------- Signed-off-by: Ignacio Vizzo Co-authored-by: Tim Player Co-authored-by: raw_t <37455909+tizianoGuadagnino@users.noreply.github.com> Co-authored-by: tizianoGuadagnino --- ros/CMakeLists.txt | 2 + ros/launch/odometry.launch | 9 +- ros/launch/odometry.launch.py | 7 +- ros/ros1/OdometryServer.cpp | 171 ++++++++++++--------- ros/ros1/OdometryServer.hpp | 29 +++- ros/ros1/Utils.hpp | 55 ++++++- ros/ros2/OdometryServer.cpp | 172 ++++++++++++--------- ros/ros2/OdometryServer.hpp | 27 +++- ros/ros2/Utils.hpp | 55 ++++++- ros/rviz/kiss_icp_ros1.rviz | 8 +- ros/rviz/kiss_icp_ros2.rviz | 276 +++++++++++++++++++++------------- 11 files changed, 546 insertions(+), 265 deletions(-) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index cac21008..d76df67f 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -47,6 +47,7 @@ if("$ENV{ROS_VERSION}" STREQUAL "1") COMPONENTS geometry_msgs nav_msgs sensor_msgs + geometry_msgs roscpp rosbag std_msgs @@ -84,6 +85,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") rclcpp_components nav_msgs sensor_msgs + geometry_msgs tf2_ros) rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node) diff --git a/ros/launch/odometry.launch b/ros/launch/odometry.launch index 68bdc37a..4dc9a26b 100644 --- a/ros/launch/odometry.launch +++ b/ros/launch/odometry.launch @@ -4,10 +4,9 @@ - + - - + @@ -20,9 +19,9 @@ - + - + diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 948052f9..470f2e07 100644 --- a/ros/launch/odometry.launch.py +++ b/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("base_frame", default_value=""), DeclareLaunchArgument("publish_odom_tf", default_value="true"), - DeclareLaunchArgument("publish_alias_tf", default_value="true"), # 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,7 @@ 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"), + "visualize": LaunchConfiguration("visualize"), } ], ), diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp index 241f72b7..3cf3c480 100644 --- a/ros/ros1/OdometryServer.cpp +++ b/ros/ros1/OdometryServer.cpp @@ -51,11 +51,11 @@ using utils::GetTimestamps; using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) - : nh_(nh), pnh_(pnh) { - pnh_.param("child_frame", child_frame_, child_frame_); + : nh_(nh), pnh_(pnh), tf2_listener_(tf2_ros::TransformListener(tf2_buffer_)) { + pnh_.param("base_frame", base_frame_, base_frame_); pnh_.param("odom_frame", odom_frame_, odom_frame_); - pnh_.param("publish_alias_tf", publish_alias_tf_, true); - pnh_.param("publish_odom_tf", publish_odom_tf_, true); + pnh_.param("publish_odom_tf", publish_odom_tf_, false); + pnh_.param("visualize", publish_debug_clouds_, publish_debug_clouds_); pnh_.param("max_range", config_.max_range, config_.max_range); pnh_.param("min_range", config_.min_range, config_.min_range); pnh_.param("deskew", config_.deskew, config_.deskew); @@ -76,102 +76,139 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &OdometryServer::RegisterFrame, this); // Initialize publishers - odom_publisher_ = pnh_.advertise("odometry", queue_size_); - frame_publisher_ = pnh_.advertise("frame", queue_size_); - kpoints_publisher_ = pnh_.advertise("keypoints", queue_size_); - map_publisher_ = pnh_.advertise("local_map", queue_size_); - - // Initialize trajectory publisher - path_msg_.header.frame_id = odom_frame_; - traj_publisher_ = pnh_.advertise("trajectory", queue_size_); - - // 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") { - static tf2_ros::StaticTransformBroadcaster br; - geometry_msgs::TransformStamped alias_transform_msg; - alias_transform_msg.header.stamp = ros::Time::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); + odom_publisher_ = pnh_.advertise("/kiss/odometry", queue_size_); + traj_publisher_ = pnh_.advertise("/kiss/trajectory", queue_size_); + if (publish_debug_clouds_) { + frame_publisher_ = pnh_.advertise("/kiss/frame", queue_size_); + kpoints_publisher_ = + pnh_.advertise("/kiss/keypoints", queue_size_); + map_publisher_ = pnh_.advertise("/kiss/local_map", queue_size_); } + // Initialize the transform buffer + tf2_buffer_.setUsingDedicatedThread(true); + path_msg_.header.frame_id = odom_frame_; // publish odometry msg ROS_INFO("KISS-ICP ROS 1 Odometry Node Initialized"); } +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_._frameExists(source_frame) && // + tf2_buffer_._frameExists(target_frame) && // + tf2_buffer_.canTransform(target_frame, source_frame, ros::Time(0), &err_msg)) { + try { + auto tf = tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0)); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + } + } + ROS_WARN("Failed to find tf between %s and %s. Reason=%s", target_frame.c_str(), + source_frame.c_str(), err_msg.c_str()); + return {}; +} + void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; const auto points = PointCloud2ToEigen(msg); const auto timestamps = [&]() -> std::vector { 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 = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * kiss_pose * cloud2base.inverse(); + }(); + + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id); + } +} - // Convert from Eigen to ROS types - const Eigen::Vector3d t_current = pose.translation(); - const Eigen::Quaterniond q_current = pose.unit_quaternion(); +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id) { + // Header for point clouds and stuff seen from desired odom_frame // Broadcast the tf if (publish_odom_tf_) { geometry_msgs::TransformStamped transform_msg; - transform_msg.header.stamp = msg->header.stamp; + transform_msg.header.stamp = 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.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.transform = tf2::sophusToTransform(pose); tf_broadcaster_.sendTransform(transform_msg); } // publish trajectory msg geometry_msgs::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.stamp = stamp; pose_msg.header.frame_id = odom_frame_; + 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(); - odom_msg->header = pose_msg.header; - odom_msg->child_frame_id = child_frame_; - 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))); + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); + odom_publisher_.publish(odom_msg); } + +void OdometryServer::PublishClouds(const std::vector frame, + const std::vector keypoints, + const ros::Time &stamp, + const std::string &cloud_frame_id) { + std_msgs::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto kiss_map = odometry_.LocalMap(); + + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; + + frame_publisher_.publish(*EigenToPointCloud2(frame, cloud_header)); + kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud_header)); + map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header)); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + frame_publisher_.publish(*EigenToPointCloud2(frame, cloud2odom, odom_header)); + kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud2odom, odom_header)); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_.publish(*EigenToPointCloud2(kiss_map, cloud2base, odom_header)); + } else { + map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header)); + } +} + } // namespace kiss_icp_ros int main(int argc, char **argv) { diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp index bcd5ea83..3786844f 100644 --- a/ros/ros1/OdometryServer.hpp +++ b/ros/ros1/OdometryServer.hpp @@ -29,7 +29,11 @@ #include #include #include +#include #include +#include + +#include namespace kiss_icp_ros { @@ -42,6 +46,21 @@ class OdometryServer { /// Register new frame void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg); + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const std::vector frame, + const std::vector keypoints, + const ros::Time &stamp, + const std::string &cloud_frame_id); + + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + /// Ros node stuff ros::NodeHandle nh_; ros::NodeHandle pnh_; @@ -49,19 +68,21 @@ class OdometryServer { /// Tools for broadcasting TFs. tf2_ros::TransformBroadcaster tf_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; bool publish_odom_tf_; - bool publish_alias_tf_; + bool publish_debug_clouds_; /// Data subscribers. ros::Subscriber pointcloud_sub_; /// Data publishers. ros::Publisher odom_publisher_; - ros::Publisher traj_publisher_; - nav_msgs::Path path_msg_; ros::Publisher frame_publisher_; ros::Publisher kpoints_publisher_; ros::Publisher map_publisher_; + ros::Publisher traj_publisher_; + nav_msgs::Path path_msg_; /// KISS-ICP kiss_icp::pipeline::KissICP odometry_; @@ -69,7 +90,7 @@ class OdometryServer { /// Global/map coordinate frame. std::string odom_frame_{"odom"}; - std::string child_frame_{"base_link"}; + std::string base_frame_{}; }; } // namespace kiss_icp_ros diff --git a/ros/ros1/Utils.hpp b/ros/ros1/Utils.hpp index 50944455..ed4d833f 100644 --- a/ros/ros1/Utils.hpp +++ b/ros/ros1/Utils.hpp @@ -27,13 +27,57 @@ #include #include #include +#include #include #include // ROS 1 headers +#include +#include +#include #include #include +namespace tf2 { + +inline geometry_msgs::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::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::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::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::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::PointCloud2; using PointField = sensor_msgs::PointField; @@ -171,6 +215,16 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector 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 EigenToPointCloud2(const std::vector &points, const std::vector ×tamps, const Header &header) { @@ -179,5 +233,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector #include +#include #include #include @@ -54,10 +55,10 @@ using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) : rclcpp::Node("odometry_node", options) { // clang-format off - child_frame_ = declare_parameter("child_frame", child_frame_); + base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); - publish_alias_tf_ = declare_parameter("publish_alias_tf", publish_alias_tf_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); + publish_debug_clouds_ = declare_parameter("visualize", publish_debug_clouds_); config_.max_range = declare_parameter("max_range", config_.max_range); config_.min_range = declare_parameter("min_range", config_.min_range); config_.deskew = declare_parameter("deskew", config_.deskew); @@ -80,111 +81,136 @@ 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("odometry", qos); - frame_publisher_ = create_publisher("frame", qos); - kpoints_publisher_ = create_publisher("keypoints", qos); - map_publisher_ = create_publisher("local_map", qos); + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS())); + odom_publisher_ = create_publisher("/kiss/odometry", qos); + traj_publisher_ = create_publisher("/kiss/trajectory", qos); + path_msg_.header.frame_id = odom_frame_; + if (publish_debug_clouds_) { + frame_publisher_ = create_publisher("/kiss/frame", qos); + kpoints_publisher_ = + create_publisher("/kiss/keypoints", qos); + map_publisher_ = create_publisher("/kiss/local_map", qos); + } // Initialize the transform broadcaster tf_broadcaster_ = std::make_unique(*this); - - // Initialize trajectory publisher - path_msg_.header.frame_id = odom_frame_; - traj_publisher_ = create_publisher("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> 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( - *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(this->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + tf2_listener_ = std::make_unique(*tf2_buffer_); RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); } +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_->_frameExists(source_frame) && // + tf2_buffer_->_frameExists(target_frame) && // + tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) { + try { + auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + } + } + RCLCPP_WARN(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 { 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 = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * kiss_pose * cloud2base.inverse(); + }(); - // Convert from Eigen to ROS types - const Eigen::Vector3d t_current = pose.translation(); - const Eigen::Quaterniond q_current = pose.unit_quaternion(); + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id); + } +} - // Broadcast the tf +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + // Broadcast the tf --- if (publish_odom_tf_) { geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.header.stamp = msg->header.stamp; + transform_msg.header.stamp = 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.child_frame_id = base_frame_.empty() ? 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.stamp = stamp; pose_msg.header.frame_id = odom_frame_; + 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(); - odom_msg->header = pose_msg.header; - odom_msg->child_frame_id = child_frame_; - odom_msg->pose.pose = pose_msg.pose; + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); odom_publisher_->publish(std::move(odom_msg)); +} + +void OdometryServer::PublishClouds(const std::vector frame, + const std::vector keypoints, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + std_msgs::msg::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto kiss_map = odometry_.LocalMap(); - // 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))); + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::msg::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; - // 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))); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header))); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, odom_header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, odom_header))); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, odom_header))); + } else { + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); + } } } // namespace kiss_icp_ros diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 919a2c2e..13726d3a 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -26,12 +26,15 @@ #include "kiss_icp/pipeline/KissICP.hpp" // ROS 2 +#include #include +#include #include #include #include #include +#include namespace kiss_icp_ros { @@ -45,14 +48,28 @@ 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}; + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const std::vector frame, + const std::vector keypoints, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + +private: /// Tools for broadcasting TFs. std::unique_ptr tf_broadcaster_; + std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_listener_; bool publish_odom_tf_; - bool publish_alias_tf_; + bool publish_debug_clouds_; /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; @@ -73,7 +90,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 diff --git a/ros/ros2/Utils.hpp b/ros/ros2/Utils.hpp index 36dd685f..0ae32975 100644 --- a/ros/ros2/Utils.hpp +++ b/ros/ros2/Utils.hpp @@ -27,13 +27,57 @@ #include #include #include +#include #include #include // ROS 2 +#include +#include +#include #include #include +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 EigenToPointCloud2(const std::vector EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector 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 EigenToPointCloud2(const std::vector &points, const std::vector ×tamps, const Header &header) { @@ -181,5 +235,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector