From 8a4618942ca96377bfdbcec6e12d0404aad136b2 Mon Sep 17 00:00:00 2001 From: Tim Player Date: Fri, 20 Oct 2023 05:02:42 -0700 Subject: [PATCH] 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 --- ros/CMakeLists.txt | 9 +- ros/launch/odometry.launch | 9 +- ros/package.xml | 1 + ros/ros1/OdometryServer.cpp | 172 ++++++++++++++++++++++-------------- ros/ros1/OdometryServer.hpp | 29 +++++- ros/ros1/Utils.hpp | 56 +++++++++++- ros/rviz/kiss_icp_ros1.rviz | 8 +- 7 files changed, 201 insertions(+), 83 deletions(-) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 07d4a5b1..19a6c21a 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -47,11 +47,13 @@ if("$ENV{ROS_VERSION}" STREQUAL "1") COMPONENTS geometry_msgs nav_msgs sensor_msgs + geometry_msgs roscpp rosbag std_msgs tf2 - tf2_ros) + tf2_ros + Sophus) catkin_package() # ROS 1 node @@ -71,6 +73,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(tf2_ros REQUIRED) + find_package(sophus REQUIRED) # ROS 2 node add_library(odometry_component SHARED ros2/OdometryServer.cpp) @@ -84,7 +87,9 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") rclcpp_components nav_msgs sensor_msgs - tf2_ros) + geometry_msgs + tf2_ros + Sophus) 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/package.xml b/ros/package.xml index 0a444965..1cc74cac 100644 --- a/ros/package.xml +++ b/ros/package.xml @@ -51,6 +51,7 @@ sensor_msgs tf2 tf2_ros + sophus catkin diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp index 241f72b7..2c99dea2 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,140 @@ 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 Vector3dVector frame, + const Vector3dVector 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..8b585154 100644 --- a/ros/ros1/OdometryServer.hpp +++ b/ros/ros1/OdometryServer.hpp @@ -29,7 +29,10 @@ #include #include #include +#include #include +#include +#include namespace kiss_icp_ros { @@ -42,6 +45,22 @@ 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) + using Vector3dVector = kiss_icp::pipeline::KissICP::Vector3dVector; + void PublishClouds(const Vector3dVector frame, + const Vector3dVector 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..d34ddd03 100644 --- a/ros/ros1/Utils.hpp +++ b/ros/ros1/Utils.hpp @@ -27,13 +27,58 @@ #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 +216,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 +234,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector