diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index cac21008..d76df67f 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -47,6 +47,7 @@ if("$ENV{ROS_VERSION}" STREQUAL "1") COMPONENTS geometry_msgs nav_msgs sensor_msgs + geometry_msgs roscpp rosbag std_msgs @@ -84,6 +85,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2") rclcpp_components nav_msgs sensor_msgs + geometry_msgs tf2_ros) rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node) diff --git a/ros/launch/odometry.launch b/ros/launch/odometry.launch index 68bdc37a..4dc9a26b 100644 --- a/ros/launch/odometry.launch +++ b/ros/launch/odometry.launch @@ -4,10 +4,9 @@ - + - - + @@ -20,9 +19,9 @@ - + - + diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 948052f9..470f2e07 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -42,9 +42,8 @@ def generate_launch_description(): DeclareLaunchArgument("bagfile", default_value=""), DeclareLaunchArgument("visualize", default_value="true"), DeclareLaunchArgument("odom_frame", default_value="odom"), - DeclareLaunchArgument("child_frame", default_value="base_link"), + DeclareLaunchArgument("base_frame", default_value=""), DeclareLaunchArgument("publish_odom_tf", default_value="true"), - DeclareLaunchArgument("publish_alias_tf", default_value="true"), # KISS-ICP parameters DeclareLaunchArgument("deskew", default_value="false"), DeclareLaunchArgument("max_range", default_value="100.0"), @@ -60,7 +59,7 @@ def generate_launch_description(): parameters=[ { "odom_frame": LaunchConfiguration("odom_frame"), - "child_frame": LaunchConfiguration("child_frame"), + "base_frame": LaunchConfiguration("base_frame"), "max_range": LaunchConfiguration("max_range"), "min_range": LaunchConfiguration("min_range"), "deskew": LaunchConfiguration("deskew"), @@ -69,7 +68,7 @@ def generate_launch_description(): "initial_threshold": 2.0, "min_motion_th": 0.1, "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), - "publish_alias_tf": LaunchConfiguration("publish_alias_tf"), + "visualize": LaunchConfiguration("visualize"), } ], ), diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp index 241f72b7..3cf3c480 100644 --- a/ros/ros1/OdometryServer.cpp +++ b/ros/ros1/OdometryServer.cpp @@ -51,11 +51,11 @@ using utils::GetTimestamps; using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) - : nh_(nh), pnh_(pnh) { - pnh_.param("child_frame", child_frame_, child_frame_); + : nh_(nh), pnh_(pnh), tf2_listener_(tf2_ros::TransformListener(tf2_buffer_)) { + pnh_.param("base_frame", base_frame_, base_frame_); pnh_.param("odom_frame", odom_frame_, odom_frame_); - pnh_.param("publish_alias_tf", publish_alias_tf_, true); - pnh_.param("publish_odom_tf", publish_odom_tf_, true); + pnh_.param("publish_odom_tf", publish_odom_tf_, false); + pnh_.param("visualize", publish_debug_clouds_, publish_debug_clouds_); pnh_.param("max_range", config_.max_range, config_.max_range); pnh_.param("min_range", config_.min_range, config_.min_range); pnh_.param("deskew", config_.deskew, config_.deskew); @@ -76,102 +76,139 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &OdometryServer::RegisterFrame, this); // Initialize publishers - odom_publisher_ = pnh_.advertise("odometry", queue_size_); - frame_publisher_ = pnh_.advertise("frame", queue_size_); - kpoints_publisher_ = pnh_.advertise("keypoints", queue_size_); - map_publisher_ = pnh_.advertise("local_map", queue_size_); - - // Initialize trajectory publisher - path_msg_.header.frame_id = odom_frame_; - traj_publisher_ = pnh_.advertise("trajectory", queue_size_); - - // Broadcast a static transformation that links with identity the specified base link to the - // pointcloud_frame, basically to always be able to visualize the frame in rviz - if (publish_alias_tf_ && child_frame_ != "base_link") { - static tf2_ros::StaticTransformBroadcaster br; - geometry_msgs::TransformStamped alias_transform_msg; - alias_transform_msg.header.stamp = ros::Time::now(); - alias_transform_msg.transform.translation.x = 0.0; - alias_transform_msg.transform.translation.y = 0.0; - alias_transform_msg.transform.translation.z = 0.0; - alias_transform_msg.transform.rotation.x = 0.0; - alias_transform_msg.transform.rotation.y = 0.0; - alias_transform_msg.transform.rotation.z = 0.0; - alias_transform_msg.transform.rotation.w = 1.0; - alias_transform_msg.header.frame_id = child_frame_; - alias_transform_msg.child_frame_id = "base_link"; - br.sendTransform(alias_transform_msg); + odom_publisher_ = pnh_.advertise("/kiss/odometry", queue_size_); + traj_publisher_ = pnh_.advertise("/kiss/trajectory", queue_size_); + if (publish_debug_clouds_) { + frame_publisher_ = pnh_.advertise("/kiss/frame", queue_size_); + kpoints_publisher_ = + pnh_.advertise("/kiss/keypoints", queue_size_); + map_publisher_ = pnh_.advertise("/kiss/local_map", queue_size_); } + // Initialize the transform buffer + tf2_buffer_.setUsingDedicatedThread(true); + path_msg_.header.frame_id = odom_frame_; // publish odometry msg ROS_INFO("KISS-ICP ROS 1 Odometry Node Initialized"); } +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_._frameExists(source_frame) && // + tf2_buffer_._frameExists(target_frame) && // + tf2_buffer_.canTransform(target_frame, source_frame, ros::Time(0), &err_msg)) { + try { + auto tf = tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0)); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + } + } + ROS_WARN("Failed to find tf between %s and %s. Reason=%s", target_frame.c_str(), + source_frame.c_str(), err_msg.c_str()); + return {}; +} + void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; const auto points = PointCloud2ToEigen(msg); const auto timestamps = [&]() -> std::vector { if (!config_.deskew) return {}; return GetTimestamps(msg); }(); + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); // Register frame, main entry point to KISS-ICP pipeline const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps); - // PublishPose - const auto pose = odometry_.poses().back(); + // Compute the pose using KISS, ego-centric to the LiDAR + const Sophus::SE3d kiss_pose = odometry_.poses().back(); + + // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame + const auto pose = [&]() -> Sophus::SE3d { + if (egocentric_estimation) return kiss_pose; + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * kiss_pose * cloud2base.inverse(); + }(); + + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id); + } +} - // Convert from Eigen to ROS types - const Eigen::Vector3d t_current = pose.translation(); - const Eigen::Quaterniond q_current = pose.unit_quaternion(); +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id) { + // Header for point clouds and stuff seen from desired odom_frame // Broadcast the tf if (publish_odom_tf_) { geometry_msgs::TransformStamped transform_msg; - transform_msg.header.stamp = msg->header.stamp; + transform_msg.header.stamp = stamp; transform_msg.header.frame_id = odom_frame_; - transform_msg.child_frame_id = child_frame_; - transform_msg.transform.rotation.x = q_current.x(); - transform_msg.transform.rotation.y = q_current.y(); - transform_msg.transform.rotation.z = q_current.z(); - transform_msg.transform.rotation.w = q_current.w(); - transform_msg.transform.translation.x = t_current.x(); - transform_msg.transform.translation.y = t_current.y(); - transform_msg.transform.translation.z = t_current.z(); + transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.transform = tf2::sophusToTransform(pose); tf_broadcaster_.sendTransform(transform_msg); } // publish trajectory msg geometry_msgs::PoseStamped pose_msg; - pose_msg.pose.orientation.x = q_current.x(); - pose_msg.pose.orientation.y = q_current.y(); - pose_msg.pose.orientation.z = q_current.z(); - pose_msg.pose.orientation.w = q_current.w(); - pose_msg.pose.position.x = t_current.x(); - pose_msg.pose.position.y = t_current.y(); - pose_msg.pose.position.z = t_current.z(); - pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.stamp = stamp; pose_msg.header.frame_id = odom_frame_; + pose_msg.pose = tf2::sophusToPose(pose); path_msg_.poses.push_back(pose_msg); traj_publisher_.publish(path_msg_); // publish odometry msg - auto odom_msg = std::make_unique(); - odom_msg->header = pose_msg.header; - odom_msg->child_frame_id = child_frame_; - odom_msg->pose.pose = pose_msg.pose; - odom_publisher_.publish(*std::move(odom_msg)); - - // Publish KISS-ICP internal data, just for debugging - auto frame_header = msg->header; - frame_header.frame_id = child_frame_; - frame_publisher_.publish(*std::move(EigenToPointCloud2(frame, frame_header))); - kpoints_publisher_.publish(*std::move(EigenToPointCloud2(keypoints, frame_header))); - - // Map is referenced to the odometry_frame - auto local_map_header = msg->header; - local_map_header.frame_id = odom_frame_; - map_publisher_.publish(*std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header))); + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); + odom_publisher_.publish(odom_msg); } + +void OdometryServer::PublishClouds(const std::vector frame, + const std::vector keypoints, + const ros::Time &stamp, + const std::string &cloud_frame_id) { + std_msgs::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto kiss_map = odometry_.LocalMap(); + + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; + + frame_publisher_.publish(*EigenToPointCloud2(frame, cloud_header)); + kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud_header)); + map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header)); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + frame_publisher_.publish(*EigenToPointCloud2(frame, cloud2odom, odom_header)); + kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud2odom, odom_header)); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_.publish(*EigenToPointCloud2(kiss_map, cloud2base, odom_header)); + } else { + map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header)); + } +} + } // namespace kiss_icp_ros int main(int argc, char **argv) { diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp index bcd5ea83..3786844f 100644 --- a/ros/ros1/OdometryServer.hpp +++ b/ros/ros1/OdometryServer.hpp @@ -29,7 +29,11 @@ #include #include #include +#include #include +#include + +#include namespace kiss_icp_ros { @@ -42,6 +46,21 @@ class OdometryServer { /// Register new frame void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg); + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const std::vector frame, + const std::vector keypoints, + const ros::Time &stamp, + const std::string &cloud_frame_id); + + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + /// Ros node stuff ros::NodeHandle nh_; ros::NodeHandle pnh_; @@ -49,19 +68,21 @@ class OdometryServer { /// Tools for broadcasting TFs. tf2_ros::TransformBroadcaster tf_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; bool publish_odom_tf_; - bool publish_alias_tf_; + bool publish_debug_clouds_; /// Data subscribers. ros::Subscriber pointcloud_sub_; /// Data publishers. ros::Publisher odom_publisher_; - ros::Publisher traj_publisher_; - nav_msgs::Path path_msg_; ros::Publisher frame_publisher_; ros::Publisher kpoints_publisher_; ros::Publisher map_publisher_; + ros::Publisher traj_publisher_; + nav_msgs::Path path_msg_; /// KISS-ICP kiss_icp::pipeline::KissICP odometry_; @@ -69,7 +90,7 @@ class OdometryServer { /// Global/map coordinate frame. std::string odom_frame_{"odom"}; - std::string child_frame_{"base_link"}; + std::string base_frame_{}; }; } // namespace kiss_icp_ros diff --git a/ros/ros1/Utils.hpp b/ros/ros1/Utils.hpp index 50944455..ed4d833f 100644 --- a/ros/ros1/Utils.hpp +++ b/ros/ros1/Utils.hpp @@ -27,13 +27,57 @@ #include #include #include +#include #include #include // ROS 1 headers +#include +#include +#include #include #include +namespace tf2 { + +inline geometry_msgs::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::Transform t; + t.translation.x = T.translation().x(); + t.translation.y = T.translation().y(); + t.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.rotation.w = q.w(); + + return t; +} + +inline geometry_msgs::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::Pose t; + t.position.x = T.translation().x(); + t.position.y = T.translation().y(); + t.position.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.orientation.x = q.x(); + t.orientation.y = q.y(); + t.orientation.z = q.z(); + t.orientation.w = q.w(); + + return t; +} + +inline Sophus::SE3d transformToSophus(const geometry_msgs::TransformStamped &transform) { + const auto &t = transform.transform; + return Sophus::SE3d( + Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), + Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); +} +} // namespace tf2 + namespace kiss_icp_ros::utils { using PointCloud2 = sensor_msgs::PointCloud2; using PointField = sensor_msgs::PointField; @@ -171,6 +215,16 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector points_t; + points_t.resize(points.size()); + std::transform(points.cbegin(), points.cend(), points_t.begin(), + [&](const auto &point) { return T * point; }); + return EigenToPointCloud2(points_t, header); +} + inline std::unique_ptr EigenToPointCloud2(const std::vector &points, const std::vector ×tamps, const Header &header) { @@ -179,5 +233,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector #include +#include #include #include @@ -54,10 +55,10 @@ using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) : rclcpp::Node("odometry_node", options) { // clang-format off - child_frame_ = declare_parameter("child_frame", child_frame_); + base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); - publish_alias_tf_ = declare_parameter("publish_alias_tf", publish_alias_tf_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); + publish_debug_clouds_ = declare_parameter("visualize", publish_debug_clouds_); config_.max_range = declare_parameter("max_range", config_.max_range); config_.min_range = declare_parameter("min_range", config_.min_range); config_.deskew = declare_parameter("deskew", config_.deskew); @@ -80,111 +81,136 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) std::bind(&OdometryServer::RegisterFrame, this, std::placeholders::_1)); // Initialize publishers - rclcpp::QoS qos(rclcpp::KeepLast{queue_size_}); - odom_publisher_ = create_publisher("odometry", qos); - frame_publisher_ = create_publisher("frame", qos); - kpoints_publisher_ = create_publisher("keypoints", qos); - map_publisher_ = create_publisher("local_map", qos); + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS())); + odom_publisher_ = create_publisher("/kiss/odometry", qos); + traj_publisher_ = create_publisher("/kiss/trajectory", qos); + path_msg_.header.frame_id = odom_frame_; + if (publish_debug_clouds_) { + frame_publisher_ = create_publisher("/kiss/frame", qos); + kpoints_publisher_ = + create_publisher("/kiss/keypoints", qos); + map_publisher_ = create_publisher("/kiss/local_map", qos); + } // Initialize the transform broadcaster tf_broadcaster_ = std::make_unique(*this); - - // Initialize trajectory publisher - path_msg_.header.frame_id = odom_frame_; - traj_publisher_ = create_publisher("trajectory", qos); - - // Broadcast a static transformation that links with identity the specified base link to the - // pointcloud_frame, basically to always be able to visualize the frame in rviz - if (publish_alias_tf_ && child_frame_ != "base_link") { - rclcpp::PublisherOptionsWithAllocator> options; - options.qos_overriding_options = rclcpp::QosOverridingOptions{ - rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability}; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - - static auto br = std::make_shared( - *this, tf2_ros::StaticBroadcasterQoS(), options); - - geometry_msgs::msg::TransformStamped alias_transform_msg; - alias_transform_msg.header.stamp = this->get_clock()->now(); - alias_transform_msg.transform.translation.x = 0.0; - alias_transform_msg.transform.translation.y = 0.0; - alias_transform_msg.transform.translation.z = 0.0; - alias_transform_msg.transform.rotation.x = 0.0; - alias_transform_msg.transform.rotation.y = 0.0; - alias_transform_msg.transform.rotation.z = 0.0; - alias_transform_msg.transform.rotation.w = 1.0; - alias_transform_msg.header.frame_id = child_frame_; - alias_transform_msg.child_frame_id = "base_link"; - br->sendTransform(alias_transform_msg); - } + tf2_buffer_ = std::make_unique(this->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + tf2_listener_ = std::make_unique(*tf2_buffer_); RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); } +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_->_frameExists(source_frame) && // + tf2_buffer_->_frameExists(target_frame) && // + tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) { + try { + auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + } + } + RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str()); + return {}; +} + void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; const auto points = PointCloud2ToEigen(msg); const auto timestamps = [&]() -> std::vector { if (!config_.deskew) return {}; return GetTimestamps(msg); }(); + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); // Register frame, main entry point to KISS-ICP pipeline const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps); - // PublishPose - const auto pose = odometry_.poses().back(); + // Compute the pose using KISS, ego-centric to the LiDAR + const Sophus::SE3d kiss_pose = odometry_.poses().back(); + + // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame + const auto pose = [&]() -> Sophus::SE3d { + if (egocentric_estimation) return kiss_pose; + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * kiss_pose * cloud2base.inverse(); + }(); - // Convert from Eigen to ROS types - const Eigen::Vector3d t_current = pose.translation(); - const Eigen::Quaterniond q_current = pose.unit_quaternion(); + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id); + } +} - // Broadcast the tf +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + // Broadcast the tf --- if (publish_odom_tf_) { geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.header.stamp = msg->header.stamp; + transform_msg.header.stamp = stamp; transform_msg.header.frame_id = odom_frame_; - transform_msg.child_frame_id = child_frame_; - transform_msg.transform.rotation.x = q_current.x(); - transform_msg.transform.rotation.y = q_current.y(); - transform_msg.transform.rotation.z = q_current.z(); - transform_msg.transform.rotation.w = q_current.w(); - transform_msg.transform.translation.x = t_current.x(); - transform_msg.transform.translation.y = t_current.y(); - transform_msg.transform.translation.z = t_current.z(); + transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.transform = tf2::sophusToTransform(pose); tf_broadcaster_->sendTransform(transform_msg); } // publish trajectory msg geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.pose.orientation.x = q_current.x(); - pose_msg.pose.orientation.y = q_current.y(); - pose_msg.pose.orientation.z = q_current.z(); - pose_msg.pose.orientation.w = q_current.w(); - pose_msg.pose.position.x = t_current.x(); - pose_msg.pose.position.y = t_current.y(); - pose_msg.pose.position.z = t_current.z(); - pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.stamp = stamp; pose_msg.header.frame_id = odom_frame_; + pose_msg.pose = tf2::sophusToPose(pose); path_msg_.poses.push_back(pose_msg); traj_publisher_->publish(path_msg_); // publish odometry msg - auto odom_msg = std::make_unique(); - odom_msg->header = pose_msg.header; - odom_msg->child_frame_id = child_frame_; - odom_msg->pose.pose = pose_msg.pose; + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); odom_publisher_->publish(std::move(odom_msg)); +} + +void OdometryServer::PublishClouds(const std::vector frame, + const std::vector keypoints, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + std_msgs::msg::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto kiss_map = odometry_.LocalMap(); - // Publish KISS-ICP internal data, just for debugging - auto frame_header = msg->header; - frame_header.frame_id = child_frame_; - frame_publisher_->publish(std::move(EigenToPointCloud2(frame, frame_header))); - kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, frame_header))); + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::msg::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; - // Map is referenced to the odometry_frame - auto local_map_header = msg->header; - local_map_header.frame_id = odom_frame_; - map_publisher_->publish(std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header))); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header))); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, odom_header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, odom_header))); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, odom_header))); + } else { + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); + } } } // namespace kiss_icp_ros diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 919a2c2e..13726d3a 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -26,12 +26,15 @@ #include "kiss_icp/pipeline/KissICP.hpp" // ROS 2 +#include #include +#include #include #include #include #include +#include namespace kiss_icp_ros { @@ -45,14 +48,28 @@ class OdometryServer : public rclcpp::Node { /// Register new frame void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); -private: - /// Ros node stuff - size_t queue_size_{1}; + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const std::vector frame, + const std::vector keypoints, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + +private: /// Tools for broadcasting TFs. std::unique_ptr tf_broadcaster_; + std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_listener_; bool publish_odom_tf_; - bool publish_alias_tf_; + bool publish_debug_clouds_; /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; @@ -73,7 +90,7 @@ class OdometryServer : public rclcpp::Node { /// Global/map coordinate frame. std::string odom_frame_{"odom"}; - std::string child_frame_{"base_link"}; + std::string base_frame_{}; }; } // namespace kiss_icp_ros diff --git a/ros/ros2/Utils.hpp b/ros/ros2/Utils.hpp index 36dd685f..0ae32975 100644 --- a/ros/ros2/Utils.hpp +++ b/ros/ros2/Utils.hpp @@ -27,13 +27,57 @@ #include #include #include +#include #include #include // ROS 2 +#include +#include +#include #include #include +namespace tf2 { + +inline geometry_msgs::msg::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::msg::Transform t; + t.translation.x = T.translation().x(); + t.translation.y = T.translation().y(); + t.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.rotation.w = q.w(); + + return t; +} + +inline geometry_msgs::msg::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::msg::Pose t; + t.position.x = T.translation().x(); + t.position.y = T.translation().y(); + t.position.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.orientation.x = q.x(); + t.orientation.y = q.y(); + t.orientation.z = q.z(); + t.orientation.w = q.w(); + + return t; +} + +inline Sophus::SE3d transformToSophus(const geometry_msgs::msg::TransformStamped &transform) { + const auto &t = transform.transform; + return Sophus::SE3d( + Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), + Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); +} +} // namespace tf2 + namespace kiss_icp_ros::utils { using PointCloud2 = sensor_msgs::msg::PointCloud2; @@ -173,6 +217,16 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector points_t; + points_t.resize(points.size()); + std::transform(points.cbegin(), points.cend(), points_t.begin(), + [&](const auto &point) { return T * point; }); + return EigenToPointCloud2(points_t, header); +} + inline std::unique_ptr EigenToPointCloud2(const std::vector &points, const std::vector ×tamps, const Header &header) { @@ -181,5 +235,4 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector