From 0cad6473abcd7fb4af86d24c7f11ae5fac06dd10 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 3 Oct 2023 14:41:11 -0400 Subject: [PATCH 01/19] Fix TF tree usage in ROS 2 wrapper Signed-off-by: Ignacio Vizzo --- ros/launch/odometry.launch.py | 8 +- ros/ros2/OdometryServer.cpp | 142 +++++++++++++++++----------------- ros/ros2/OdometryServer.hpp | 12 ++- ros/ros2/Utils.hpp | 44 +++++++++++ ros/rviz/kiss_icp_ros2.rviz | 47 ++++++++++- 5 files changed, 168 insertions(+), 85 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 948052f9..519229fb 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("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"), } ], ), diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 25256bdf..8db7b3e4 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -22,6 +22,8 @@ // SOFTWARE. #include #include +#include +#include #include #include @@ -54,9 +56,8 @@ 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_); config_.max_range = declare_parameter("max_range", config_.max_range); config_.min_range = declare_parameter("min_range", config_.min_range); @@ -80,50 +81,62 @@ 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); + frame_publisher_ = create_publisher("/kiss/frame", qos); + kpoints_publisher_ = create_publisher("/kiss/keypoints", qos); + map_publisher_ = create_publisher("/kiss/local_map", qos); + traj_publisher_ = create_publisher("/kiss/trajectory", qos); + path_msg_.header.frame_id = odom_frame_; // 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::BaseLinkToCloudTf(const std::string &cloud_frame_id) { + try { + if (tf2_buffer_->_frameExists(base_frame_) && tf2_buffer_->_frameExists(cloud_frame_id) && + tf2_buffer_->canTransform(base_frame_, cloud_frame_id, tf2::TimePointZero)) { + auto tf = tf2_buffer_->lookupTransform(base_frame_, cloud_frame_id, tf2::TimePointZero); + return tf2::transformToSophus(tf); + } + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return {}; + } + // Should never reach this point, but if it does, spit the Identity + RCLCPP_ERROR(this->get_logger(), "OdometryServer::BaseLinkToCloudTf failed to find tf"); + return {}; +} + void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { - const auto points = PointCloud2ToEigen(msg); + // Extract the points from the message in the desirded coordinate frame. + using Vector3dVector = std::vector; + const auto [points, cloud_frame_id] = [&]() -> std::tuple { + // Extract point cloud and frame id from message + auto points = PointCloud2ToEigen(msg); + auto cloud_frame_id = msg->header.frame_id; + + if (base_frame_.empty() || base_frame_ == cloud_frame_id) { + // Point cloud not modified, therefore it still expressed in its original frame_id + return {points, cloud_frame_id}; + } + + // We need to express the input point cloud seen from the base coordinate frame + const auto base2cloud = BaseLinkToCloudTf(cloud_frame_id); + std::transform(points.cbegin(), points.cend(), points.begin(), + [&](const auto &point) { return base2cloud * point; }); + + // Now the point cloud is in the base_frame_id, not the original one + return {points, base_frame_}; + }(); + + // Extract timestamps from the message for deskewing the cloud const auto timestamps = [&]() -> std::vector { if (!config_.deskew) return {}; return GetTimestamps(msg); @@ -133,58 +146,43 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps); // PublishPose - const auto pose = odometry_.poses().back(); + const Sophus::SE3d pose = odometry_.poses().back(); + + // Header for point clouds and stuff seen from the cloud_frame_id (base_frame/original_frame) + std_msgs::msg::Header cloud_header; + cloud_header.stamp = msg->header.stamp; + cloud_header.frame_id = cloud_frame_id; - // 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; + odom_header.stamp = msg->header.stamp; + 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 = cloud_frame_id; + 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(); - 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))); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header))); + map_publisher_->publish(std::move(EigenToPointCloud2(odometry_.LocalMap(), odom_header))); } } // namespace kiss_icp_ros diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 919a2c2e..68d972ac 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -26,7 +26,9 @@ #include "kiss_icp/pipeline/KissICP.hpp" // ROS 2 +#include #include +#include #include #include @@ -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 BaseLinkToCloudTf(const std::string &pointcloud_frame_id); +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_; @@ -73,7 +77,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..38af6ca3 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; diff --git a/ros/rviz/kiss_icp_ros2.rviz b/ros/rviz/kiss_icp_ros2.rviz index 134ff35e..bc0f1329 100644 --- a/ros/rviz/kiss_icp_ros2.rviz +++ b/ros/rviz/kiss_icp_ros2.rviz @@ -57,7 +57,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_map + Value: /kiss/local_map Use Fixed Frame: true Use rainbow: true Value: true @@ -91,7 +91,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /frame + Value: /kiss/frame Use Fixed Frame: true Use rainbow: true Value: true @@ -107,7 +107,7 @@ Visualization Manager: Length: 1 Name: pointcloud_frame Radius: 0.10000000149011612 - Reference Frame: base_link + Reference Frame: base_footprint Value: true - Alpha: 1 Buffer Length: 1 @@ -135,7 +135,46 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /trajectory + Value: /kiss/trajectory + Value: true + - 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: Reliable + Value: /kiss/odometry Value: true Enabled: true Global Options: From e9d67840c26e71defff9be57c86752029d8e415d Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 13 Oct 2023 16:43:54 +0200 Subject: [PATCH 02/19] Transform the pose instead of the pointcloud --- ros/ros2/OdometryServer.cpp | 80 +++++++++++++++---------------------- ros/ros2/OdometryServer.hpp | 4 +- ros/ros2/Utils.hpp | 11 ++++- ros/rviz/kiss_icp_ros2.rviz | 36 +---------------- 4 files changed, 45 insertions(+), 86 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 8db7b3e4..8b0d6a30 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -83,8 +83,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) // Initialize publishers rclcpp::QoS qos((rclcpp::SystemDefaultsQoS())); odom_publisher_ = create_publisher("/kiss/odometry", qos); - frame_publisher_ = create_publisher("/kiss/frame", qos); - kpoints_publisher_ = create_publisher("/kiss/keypoints", qos); map_publisher_ = create_publisher("/kiss/local_map", qos); traj_publisher_ = create_publisher("/kiss/trajectory", qos); path_msg_.header.frame_id = odom_frame_; @@ -98,71 +96,53 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); } -Sophus::SE3d OdometryServer::BaseLinkToCloudTf(const std::string &cloud_frame_id) { - try { - if (tf2_buffer_->_frameExists(base_frame_) && tf2_buffer_->_frameExists(cloud_frame_id) && - tf2_buffer_->canTransform(base_frame_, cloud_frame_id, tf2::TimePointZero)) { - auto tf = tf2_buffer_->lookupTransform(base_frame_, cloud_frame_id, tf2::TimePointZero); +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()); } - } catch (tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - return {}; } - // Should never reach this point, but if it does, spit the Identity - RCLCPP_ERROR(this->get_logger(), "OdometryServer::BaseLinkToCloudTf failed to find tf"); + 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) { - // Extract the points from the message in the desirded coordinate frame. - using Vector3dVector = std::vector; - const auto [points, cloud_frame_id] = [&]() -> std::tuple { - // Extract point cloud and frame id from message - auto points = PointCloud2ToEigen(msg); - auto cloud_frame_id = msg->header.frame_id; - - if (base_frame_.empty() || base_frame_ == cloud_frame_id) { - // Point cloud not modified, therefore it still expressed in its original frame_id - return {points, cloud_frame_id}; - } - - // We need to express the input point cloud seen from the base coordinate frame - const auto base2cloud = BaseLinkToCloudTf(cloud_frame_id); - std::transform(points.cbegin(), points.cend(), points.begin(), - [&](const auto &point) { return base2cloud * point; }); - - // Now the point cloud is in the base_frame_id, not the original one - return {points, base_frame_}; - }(); - - // Extract timestamps from the message for deskewing the cloud + 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 Sophus::SE3d pose = odometry_.poses().back(); + // Compute the pose using KISS, ego-centric to the LiDAR + const Sophus::SE3d kiss_pose = odometry_.poses().back(); - // Header for point clouds and stuff seen from the cloud_frame_id (base_frame/original_frame) - std_msgs::msg::Header cloud_header; - cloud_header.stamp = msg->header.stamp; - cloud_header.frame_id = cloud_frame_id; + // 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; + }(); // Header for point clouds and stuff seen from desired odom_frame - std_msgs::msg::Header odom_header; - odom_header.stamp = msg->header.stamp; + 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 = odom_header; - transform_msg.child_frame_id = cloud_frame_id; + transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_; transform_msg.transform = tf2::sophusToTransform(pose); tf_broadcaster_->sendTransform(transform_msg); } @@ -180,9 +160,15 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha odom_msg->pose.pose = pose_msg.pose; odom_publisher_->publish(std::move(odom_msg)); - // Publish KISS-ICP internal data, just for debugging - frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header))); - kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header))); - map_publisher_->publish(std::move(EigenToPointCloud2(odometry_.LocalMap(), odom_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 diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 68d972ac..372a91b6 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -48,7 +48,7 @@ class OdometryServer : public rclcpp::Node { void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); /// If user ask, report the pose in the given child_frame - Sophus::SE3d BaseLinkToCloudTf(const std::string &pointcloud_frame_id); + Sophus::SE3d CloudToBaseTf(const std::string &pointcloud_frame_id) const; private: /// Tools for broadcasting TFs. @@ -63,8 +63,6 @@ class OdometryServer : public rclcpp::Node { /// Data publishers. rclcpp::Publisher::SharedPtr odom_publisher_; - rclcpp::Publisher::SharedPtr frame_publisher_; - rclcpp::Publisher::SharedPtr kpoints_publisher_; rclcpp::Publisher::SharedPtr map_publisher_; /// Path publisher diff --git a/ros/ros2/Utils.hpp b/ros/ros2/Utils.hpp index 38af6ca3..0ae32975 100644 --- a/ros/ros2/Utils.hpp +++ b/ros/ros2/Utils.hpp @@ -217,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) { @@ -225,5 +235,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector Date: Fri, 13 Oct 2023 16:47:12 +0200 Subject: [PATCH 03/19] remove include --- ros/ros2/OdometryServer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 8b0d6a30..ecf2a71b 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include From 8ea67c6f0946bb6183d26a47dc752369e19815c3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 11:31:31 +0100 Subject: [PATCH 04/19] Remove default base_frame parameter value This will make KISS-ICP work ego-centric as default --- ros/launch/odometry.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 519229fb..d30bb537 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): DeclareLaunchArgument("bagfile", default_value=""), DeclareLaunchArgument("visualize", default_value="true"), DeclareLaunchArgument("odom_frame", default_value="odom"), - DeclareLaunchArgument("base_frame", default_value="base_footprint"), + DeclareLaunchArgument("base_frame", default_value=""), DeclareLaunchArgument("publish_odom_tf", default_value="false"), # KISS-ICP parameters DeclareLaunchArgument("deskew", default_value="false"), From 73a98d35230d90061b16aec65e6377139fa52d11 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 12:23:47 +0100 Subject: [PATCH 05/19] Remove unused variable --- ros/ros2/OdometryServer.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 372a91b6..95de3711 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -56,7 +56,6 @@ class OdometryServer : public rclcpp::Node { std::unique_ptr tf2_buffer_; std::unique_ptr tf2_listener_; bool publish_odom_tf_; - bool publish_alias_tf_; /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; From 79e5a3f194b28e6429360e7c7a8c3668f76e5cb0 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 12:27:39 +0100 Subject: [PATCH 06/19] Do not reuse pose_msg.pose --- ros/ros2/OdometryServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index ecf2a71b..6c60814b 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -156,7 +156,7 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha // publish odometry msg auto odom_msg = std::make_unique(); odom_msg->header = odom_header; - odom_msg->pose.pose = pose_msg.pose; + odom_msg->pose.pose = tf2::sophusToPose(pose); odom_publisher_->publish(std::move(odom_msg)); // Publish KISS-ICP internal map, just for debugging From a8e52290ddb6ab202909e2ab110181e3eff35c20 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 12:30:35 +0100 Subject: [PATCH 07/19] I'm not ever sure why we did that in first instance --- ros/ros2/OdometryServer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 6c60814b..d3b0cada 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -154,9 +154,9 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha traj_publisher_->publish(path_msg_); // publish odometry msg - auto odom_msg = std::make_unique(); - odom_msg->header = odom_header; - odom_msg->pose.pose = tf2::sophusToPose(pose); + nav_msgs::msg::Odometry odom_msg; + odom_msg.header = odom_header; + odom_msg.pose.pose = tf2::sophusToPose(pose); odom_publisher_->publish(std::move(odom_msg)); // Publish KISS-ICP internal map, just for debugging From 1ff44c3802d88380ce63d7c9a637976f582dc264 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 12:34:07 +0100 Subject: [PATCH 08/19] Be more explicit about the stamps and frame_id of the headers --- ros/ros2/OdometryServer.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index d3b0cada..ad79448f 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -133,14 +133,11 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha return cloud2base.inverse() * kiss_pose * cloud2base; }(); - // 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 + // Broadcast the tf --- if (publish_odom_tf_) { geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.header = odom_header; + transform_msg.header.stamp = msg->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); tf_broadcaster_->sendTransform(transform_msg); @@ -148,26 +145,32 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha // publish trajectory msg geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header = odom_header; + pose_msg.header.stamp = msg->header.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 nav_msgs::msg::Odometry odom_msg; - odom_msg.header = odom_header; + odom_msg.header.stamp = msg->header.stamp; + odom_msg.header.frame_id = odom_frame_; odom_msg.pose.pose = tf2::sophusToPose(pose); odom_publisher_->publish(std::move(odom_msg)); // Publish KISS-ICP internal map, just for debugging if (map_publisher_->get_subscription_count() > 0) { const auto kiss_map = odometry_.LocalMap(); + std_msgs::msg::Header header; + header.stamp = msg->header.stamp; + header.frame_id = odom_frame_; if (egocentric_estimation) { - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, header))); } else { const auto T = CloudToBaseTf(cloud_frame_id).inverse(); - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, T, odom_header))); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, T, header))); } } } + } // namespace kiss_icp_ros From 70d972d9f04bb8105e3eb27e30542606db99897b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 12:53:22 +0100 Subject: [PATCH 09/19] Extract publishing mechanism to a new function (#246) --- ros/ros2/OdometryServer.cpp | 18 +++++++++++++----- ros/ros2/OdometryServer.hpp | 5 +++++ 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index ad79448f..94d412ab 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -133,19 +133,26 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha return cloud2base.inverse() * kiss_pose * cloud2base; }(); + // Spit the current estimated pose to ROS msgs + Publish(pose, msg->header.stamp, cloud_frame_id); +} + +void OdometryServer::Publish(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 = egocentric_estimation ? cloud_frame_id : base_frame_; + 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.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); @@ -153,16 +160,17 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha // publish odometry msg nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = msg->header.stamp; + 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)); // Publish KISS-ICP internal map, just for debugging if (map_publisher_->get_subscription_count() > 0) { + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); const auto kiss_map = odometry_.LocalMap(); std_msgs::msg::Header header; - header.stamp = msg->header.stamp; + header.stamp = stamp; header.frame_id = odom_frame_; if (egocentric_estimation) { map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, header))); diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 95de3711..35dc28fc 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -34,6 +34,7 @@ #include #include #include +#include namespace kiss_icp_ros { @@ -46,6 +47,10 @@ class OdometryServer : public rclcpp::Node { private: /// Register new frame void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + /// Stream the estimated pose to ROS + void Publish(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); /// If user ask, report the pose in the given child_frame Sophus::SE3d CloudToBaseTf(const std::string &pointcloud_frame_id) const; From 505ff8fbd48bfea5c53281b4c91694e6225a50f1 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 13:54:37 +0100 Subject: [PATCH 10/19] 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. --- ros/ros2/OdometryServer.cpp | 17 +++++++++-------- ros/ros2/OdometryServer.hpp | 5 +++-- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 94d412ab..627c19af 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -95,13 +95,14 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); } -Sophus::SE3d OdometryServer::CloudToBaseTf(const std::string &cloud_frame_id) const { +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) 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)) { + 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(cloud_frame_id, base_frame_, tf2::TimePointZero); + 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()); @@ -129,7 +130,7 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha // 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); + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); return cloud2base.inverse() * kiss_pose * cloud2base; }(); @@ -175,8 +176,8 @@ void OdometryServer::Publish(const Sophus::SE3d &pose, if (egocentric_estimation) { map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, header))); } else { - const auto T = CloudToBaseTf(cloud_frame_id).inverse(); - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, T, header))); + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, header))); } } } diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 35dc28fc..97743e1e 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -52,8 +52,9 @@ class OdometryServer : public rclcpp::Node { const rclcpp::Time &stamp, const std::string &cloud_frame_id); - /// If user ask, report the pose in the given child_frame - Sophus::SE3d CloudToBaseTf(const std::string &pointcloud_frame_id) const; + /// 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. From 5e17edc3a39ab90ae90a240cbc11a93b07403d4b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 15:34:08 +0100 Subject: [PATCH 11/19] Bring back debugging clouds to ros driver --- ros/launch/odometry.launch.py | 3 ++- ros/ros2/OdometryServer.cpp | 38 ++++++++++++++++++++++++++++------- ros/ros2/OdometryServer.hpp | 11 ++++++++++ 3 files changed, 44 insertions(+), 8 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index d30bb537..470f2e07 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -43,7 +43,7 @@ def generate_launch_description(): DeclareLaunchArgument("visualize", default_value="true"), DeclareLaunchArgument("odom_frame", default_value="odom"), DeclareLaunchArgument("base_frame", default_value=""), - DeclareLaunchArgument("publish_odom_tf", default_value="false"), + DeclareLaunchArgument("publish_odom_tf", default_value="true"), # KISS-ICP parameters DeclareLaunchArgument("deskew", default_value="false"), DeclareLaunchArgument("max_range", default_value="100.0"), @@ -68,6 +68,7 @@ def generate_launch_description(): "initial_threshold": 2.0, "min_motion_th": 0.1, "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), + "visualize": LaunchConfiguration("visualize"), } ], ), diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 627c19af..5b6be656 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -58,6 +58,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); 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); @@ -82,6 +83,8 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) // Initialize publishers rclcpp::QoS qos((rclcpp::SystemDefaultsQoS())); odom_publisher_ = create_publisher("/kiss/odometry", qos); + frame_publisher_ = create_publisher("/kiss/frame", qos); + kpoints_publisher_ = create_publisher("/kiss/keypoints", qos); map_publisher_ = create_publisher("/kiss/local_map", qos); traj_publisher_ = create_publisher("/kiss/trajectory", qos); path_msg_.header.frame_id = odom_frame_; @@ -108,7 +111,7 @@ Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, RCLCPP_WARN(this->get_logger(), "%s", ex.what()); } } - RCLCPP_ERROR(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str()); + RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str()); return {}; } @@ -131,11 +134,15 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha const auto pose = [&]() -> Sophus::SE3d { if (egocentric_estimation) return kiss_pose; const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); - return cloud2base.inverse() * kiss_pose * cloud2base; + return cloud2base * kiss_pose * cloud2base.inverse(); }(); // Spit the current estimated pose to ROS msgs Publish(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); + } } void OdometryServer::Publish(const Sophus::SE3d &pose, @@ -165,14 +172,27 @@ void OdometryServer::Publish(const Sophus::SE3d &pose, 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 Vector3dVector frame, + const Vector3dVector keypoints, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + if (!publish_debug_clouds_) return; + if (!publish_odom_tf_) { + RCLCPP_WARN_ONCE(get_logger(), + "Using rviz as debugging visualizer withouth publishing to the tf tree is " + "not supported. Please check the Python client"); + } + + // Publish KISS-ICP internal map, and input clouds. Just for debugging + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = odom_frame_; - // Publish KISS-ICP internal map, just for debugging - if (map_publisher_->get_subscription_count() > 0) { + if (map_publisher_->get_subscription_count() < 0) { const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); const auto kiss_map = odometry_.LocalMap(); - std_msgs::msg::Header header; - header.stamp = stamp; - header.frame_id = odom_frame_; if (egocentric_estimation) { map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, header))); } else { @@ -180,6 +200,10 @@ void OdometryServer::Publish(const Sophus::SE3d &pose, map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, header))); } } + + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, header))); } } // namespace kiss_icp_ros diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 97743e1e..79e4b205 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -47,11 +47,19 @@ class OdometryServer : public rclcpp::Node { private: /// Register new frame void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + /// Stream the estimated pose to ROS void Publish(const Sophus::SE3d &pose, const rclcpp::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 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; @@ -62,12 +70,15 @@ class OdometryServer : public rclcpp::Node { std::unique_ptr tf2_buffer_; std::unique_ptr tf2_listener_; bool publish_odom_tf_; + bool publish_debug_clouds_; /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; /// Data publishers. rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr frame_publisher_; + rclcpp::Publisher::SharedPtr kpoints_publisher_; rclcpp::Publisher::SharedPtr map_publisher_; /// Path publisher From 2cfcab5430df6f56a0ed79d9110a9112e299027b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 15:40:54 +0100 Subject: [PATCH 12/19] re-arange logic on when and when not to publish clouds --- ros/ros2/OdometryServer.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 5b6be656..276f3ac1 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -58,7 +58,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); 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); @@ -72,6 +71,16 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) } // clang-format on + // Conditioanlly add debugging information + const bool visualize = declare_parameter("visualize", publish_debug_clouds_); + publish_debug_clouds_ = visualize && publish_odom_tf_; + if (visualize && !publish_odom_tf_) { + RCLCPP_ERROR( + get_logger(), + "Using rviz as debugging visualizer for KISS-ICP withouth publishing to the tf tree is " + "not supported. Please check the Python client"); + } + // Construct the main KISS-ICP odometry node odometry_ = kiss_icp::pipeline::KissICP(config_); @@ -83,9 +92,12 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) // Initialize publishers rclcpp::QoS qos((rclcpp::SystemDefaultsQoS())); odom_publisher_ = create_publisher("/kiss/odometry", qos); - frame_publisher_ = create_publisher("/kiss/frame", qos); - kpoints_publisher_ = create_publisher("/kiss/keypoints", qos); - map_publisher_ = create_publisher("/kiss/local_map", qos); + 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); + } traj_publisher_ = create_publisher("/kiss/trajectory", qos); path_msg_.header.frame_id = odom_frame_; @@ -179,11 +191,6 @@ void OdometryServer::PublishClouds(const Vector3dVector frame, const rclcpp::Time &stamp, const std::string &cloud_frame_id) { if (!publish_debug_clouds_) return; - if (!publish_odom_tf_) { - RCLCPP_WARN_ONCE(get_logger(), - "Using rviz as debugging visualizer withouth publishing to the tf tree is " - "not supported. Please check the Python client"); - } // Publish KISS-ICP internal map, and input clouds. Just for debugging std_msgs::msg::Header header; From ee94752af1bd3850216e75f6e512c195d4f437d5 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 15:42:30 +0100 Subject: [PATCH 13/19] fix lofic --- ros/ros2/OdometryServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 276f3ac1..55ff6196 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -197,7 +197,7 @@ void OdometryServer::PublishClouds(const Vector3dVector frame, header.stamp = stamp; header.frame_id = odom_frame_; - if (map_publisher_->get_subscription_count() < 0) { + if (map_publisher_->get_subscription_count() > 0) { const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); const auto kiss_map = odometry_.LocalMap(); if (egocentric_estimation) { From 22b7f923fe491eaede051404557a5fbc2a44dbd9 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 15:42:56 +0100 Subject: [PATCH 14/19] Update rviz2 --- ros/rviz/kiss_icp_ros2.rviz | 285 ++++++++++++++++++++++-------------- 1 file changed, 177 insertions(+), 108 deletions(-) diff --git a/ros/rviz/kiss_icp_ros2.rviz b/ros/rviz/kiss_icp_ros2.rviz index e9378216..e2949cc4 100644 --- a/ros/rviz/kiss_icp_ros2.rviz +++ b/ros/rviz/kiss_icp_ros2.rviz @@ -23,44 +23,187 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: local_map + SyncSource: frame_deskew Visualization Manager: Class: "" Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 17.483436584472656 - Min Value: -1.2371044158935547 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: frame_deskew + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /kiss/frame + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 61; 229; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: kiss_keypoints + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 1 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /kiss/keypoints + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 98.31531524658203 + Min Value: 0.6587954163551331 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: local_map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /kiss/local_map + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: local_map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /kiss/local_map - Use Fixed Frame: true - Use rainbow: true - Value: 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: Reliable + Value: /kiss/odometry + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 53; 132; 228 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /kiss/trajectory + Value: true + Enabled: true + Name: odometry - Class: rviz_default_plugins/Axes Enabled: true Length: 1 @@ -68,80 +211,6 @@ Visualization Manager: Radius: 0.10000000149011612 Reference Frame: odom Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 1 - Name: pointcloud_frame - Radius: 0.10000000149011612 - Reference Frame: base_footprint - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 53; 132; 228 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Billboards - Line Width: 0.20000000298023224 - Name: trajectory - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /kiss/trajectory - Value: true - - 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: Reliable - Value: /kiss/odometry - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 From b45fde366938e61c3deb5fd059c56e0a33ef2f6b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 19 Oct 2023 16:30:11 +0100 Subject: [PATCH 15/19] Loosing my mind already with this debugging info --- ros/ros2/OdometryServer.cpp | 70 ++++++++++++++++++------------------- ros/ros2/OdometryServer.hpp | 6 ++-- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 55ff6196..a9bc02a0 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -58,6 +58,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); 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); @@ -71,16 +72,6 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) } // clang-format on - // Conditioanlly add debugging information - const bool visualize = declare_parameter("visualize", publish_debug_clouds_); - publish_debug_clouds_ = visualize && publish_odom_tf_; - if (visualize && !publish_odom_tf_) { - RCLCPP_ERROR( - get_logger(), - "Using rviz as debugging visualizer for KISS-ICP withouth publishing to the tf tree is " - "not supported. Please check the Python client"); - } - // Construct the main KISS-ICP odometry node odometry_ = kiss_icp::pipeline::KissICP(config_); @@ -92,14 +83,14 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) // Initialize publishers 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); } - traj_publisher_ = create_publisher("/kiss/trajectory", qos); - path_msg_.header.frame_id = odom_frame_; // Initialize the transform broadcaster tf_broadcaster_ = std::make_unique(*this); @@ -150,16 +141,16 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha }(); // Spit the current estimated pose to ROS msgs - Publish(pose, msg->header.stamp, cloud_frame_id); + 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); } } -void OdometryServer::Publish(const Sophus::SE3d &pose, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id) { +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; @@ -190,27 +181,36 @@ void OdometryServer::PublishClouds(const Vector3dVector frame, const Vector3dVector keypoints, const rclcpp::Time &stamp, const std::string &cloud_frame_id) { - if (!publish_debug_clouds_) return; - - // Publish KISS-ICP internal map, and input clouds. Just for debugging - std_msgs::msg::Header header; - header.stamp = stamp; - header.frame_id = odom_frame_; - - if (map_publisher_->get_subscription_count() > 0) { - const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); - const auto kiss_map = odometry_.LocalMap(); - if (egocentric_estimation) { - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, header))); - } else { - const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, header))); - } + std_msgs::msg::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::msg::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; + + 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, header))); - kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, header))); -} + 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 79e4b205..6e6a9865 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -49,9 +49,9 @@ class OdometryServer : public rclcpp::Node { void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); /// Stream the estimated pose to ROS - void Publish(const Sophus::SE3d &pose, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id); + 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) using Vector3dVector = kiss_icp::pipeline::KissICP::Vector3dVector; From 8a4618942ca96377bfdbcec6e12d0404aad136b2 Mon Sep 17 00:00:00 2001 From: Tim Player Date: Fri, 20 Oct 2023 05:02:42 -0700 Subject: [PATCH 16/19] 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 Date: Fri, 20 Oct 2023 13:44:21 +0100 Subject: [PATCH 17/19] Fix style --- ros/ros1/OdometryServer.cpp | 5 ++--- ros/ros1/OdometryServer.hpp | 3 ++- ros/ros1/Utils.hpp | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp index 2c99dea2..1ce6a495 100644 --- a/ros/ros1/OdometryServer.cpp +++ b/ros/ros1/OdometryServer.cpp @@ -105,8 +105,8 @@ Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, 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()); + 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 {}; } @@ -170,7 +170,6 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, 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, diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp index 8b585154..1dae12b1 100644 --- a/ros/ros1/OdometryServer.hpp +++ b/ros/ros1/OdometryServer.hpp @@ -32,6 +32,7 @@ #include #include #include + #include namespace kiss_icp_ros { @@ -60,7 +61,7 @@ class OdometryServer { /// 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_; diff --git a/ros/ros1/Utils.hpp b/ros/ros1/Utils.hpp index d34ddd03..ed4d833f 100644 --- a/ros/ros1/Utils.hpp +++ b/ros/ros1/Utils.hpp @@ -38,7 +38,6 @@ #include #include - namespace tf2 { inline geometry_msgs::Transform sophusToTransform(const Sophus::SE3d &T) { From e7be78cfdfc8ba0255ef186b6dda285b05a2c538 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 20 Oct 2023 13:45:17 +0100 Subject: [PATCH 18/19] Remove sophus from build system Fixing now the CI is a big pain --- ros/CMakeLists.txt | 7 ++----- ros/package.xml | 1 - 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 19a6c21a..81eacf70 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -52,8 +52,7 @@ if("$ENV{ROS_VERSION}" STREQUAL "1") rosbag std_msgs tf2 - tf2_ros - Sophus) + tf2_ros) catkin_package() # ROS 1 node @@ -73,7 +72,6 @@ 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) @@ -88,8 +86,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") nav_msgs sensor_msgs geometry_msgs - tf2_ros - Sophus) + tf2_ros) rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node) diff --git a/ros/package.xml b/ros/package.xml index 1cc74cac..0a444965 100644 --- a/ros/package.xml +++ b/ros/package.xml @@ -51,7 +51,6 @@ sensor_msgs tf2 tf2_ros - sophus catkin From 7137e883b3130d2452a3f9bed5f8b93c3c96b62b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Fri, 20 Oct 2023 13:49:21 +0100 Subject: [PATCH 19/19] Remove unnecessary alias --- ros/ros1/OdometryServer.cpp | 4 ++-- ros/ros1/OdometryServer.hpp | 5 ++--- ros/ros2/OdometryServer.cpp | 4 ++-- ros/ros2/OdometryServer.hpp | 5 ++--- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp index 1ce6a495..3cf3c480 100644 --- a/ros/ros1/OdometryServer.cpp +++ b/ros/ros1/OdometryServer.cpp @@ -172,8 +172,8 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, odom_publisher_.publish(odom_msg); } -void OdometryServer::PublishClouds(const Vector3dVector frame, - const Vector3dVector keypoints, +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; diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp index 1dae12b1..3786844f 100644 --- a/ros/ros1/OdometryServer.hpp +++ b/ros/ros1/OdometryServer.hpp @@ -52,9 +52,8 @@ class OdometryServer { 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, + void PublishClouds(const std::vector frame, + const std::vector keypoints, const ros::Time &stamp, const std::string &cloud_frame_id); diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index a9bc02a0..dfd8b1d3 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -177,8 +177,8 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, odom_publisher_->publish(std::move(odom_msg)); } -void OdometryServer::PublishClouds(const Vector3dVector frame, - const Vector3dVector keypoints, +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; diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 6e6a9865..13726d3a 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -54,9 +54,8 @@ class OdometryServer : public rclcpp::Node { 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, + void PublishClouds(const std::vector frame, + const std::vector keypoints, const rclcpp::Time &stamp, const std::string &cloud_frame_id);