diff --git a/ros/launch/odometry-composable.launch.py b/ros/launch/odometry-composable.launch.py deleted file mode 100644 index 884cec1b..00000000 --- a/ros/launch/odometry-composable.launch.py +++ /dev/null @@ -1,104 +0,0 @@ -# MIT License -# -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.conditions import IfCondition -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, - PythonExpression, -) -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - current_pkg = FindPackageShare("kiss_icp") - return LaunchDescription( - [ - # ROS 2 parameters - DeclareLaunchArgument("topic", description="sensor_msg/PointCloud2 topic to process"), - DeclareLaunchArgument("bagfile", default_value=""), - DeclareLaunchArgument("visualize", default_value="true"), - DeclareLaunchArgument("odom_frame", default_value="odom"), - DeclareLaunchArgument("base_frame", default_value=""), - DeclareLaunchArgument("publish_odom_tf", default_value="true"), - # KISS-ICP parameters - DeclareLaunchArgument("deskew", default_value="false"), - DeclareLaunchArgument("max_range", default_value="100.0"), - DeclareLaunchArgument("min_range", default_value="5.0"), - # This thing is still not suported: https://github.com/ros2/launch/issues/290#issuecomment-1438476902 - # DeclareLaunchArgument("voxel_size", default_value=None), - ComposableNodeContainer( - name="container_kiss_icp", - namespace="", - package="rclcpp_components", - executable="component_container", - output="screen", - composable_node_descriptions=[ - ComposableNode( - package="kiss_icp", - plugin="kiss_icp_ros::OdometryServer", - name="odometry_node", - extra_arguments=[{"use_intra_process_comms": True}], - remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], - parameters=[ - { - "odom_frame": LaunchConfiguration("odom_frame"), - "base_frame": LaunchConfiguration("base_frame"), - "max_range": LaunchConfiguration("max_range"), - "min_range": LaunchConfiguration("min_range"), - "deskew": LaunchConfiguration("deskew"), - # "voxel_size": LaunchConfiguration("voxel_size"), - "max_points_per_voxel": 20, - "initial_threshold": 2.0, - "min_motion_th": 0.1, - "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), - "visualize": LaunchConfiguration("visualize"), - } - ], - ), - ], - ), - Node( - package="rviz2", - executable="rviz2", - output={"both": "log"}, - arguments=[ - "-d", - PathJoinSubstitution([current_pkg, "rviz", "kiss_icp_ros2.rviz"]), - ], - condition=IfCondition(LaunchConfiguration("visualize")), - ), - # ros2 bag play stil does not support IPC: https://github.com/ros2/rosbag2/issues/902 - ExecuteProcess( - cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")], - output="screen", - condition=IfCondition( - PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"]) - ), - ), - ] - ) diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index 68eb5a87..0ddedb95 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/ros/ros2/OdometryServer.cpp @@ -81,7 +81,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) std::bind(&OdometryServer::RegisterFrame, this, std::placeholders::_1)); // Initialize publishers - rclcpp::QoS qos((rclcpp::SensorDataQoS())); + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); odom_publisher_ = create_publisher("/kiss/odometry", qos); traj_publisher_ = create_publisher("/kiss/trajectory", qos); path_msg_.header.frame_id = odom_frame_; @@ -118,12 +118,12 @@ Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, return {}; } -void OdometryServer::RegisterFrame(sensor_msgs::msg::PointCloud2::UniquePtr msg) { +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 points = PointCloud2ToEigen(msg); const auto timestamps = [&]() -> std::vector { if (!config_.deskew) return {}; - return GetTimestamps(*msg); + return GetTimestamps(msg); }(); const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index f74a668f..418e0d2c 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/ros/ros2/OdometryServer.hpp @@ -46,7 +46,7 @@ class OdometryServer : public rclcpp::Node { private: /// Register new frame - void RegisterFrame(sensor_msgs::msg::PointCloud2::UniquePtr msg); + void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); /// Stream the estimated pose to ROS void PublishOdometry(const Sophus::SE3d &pose, diff --git a/ros/ros2/Utils.hpp b/ros/ros2/Utils.hpp index 2f12ebcd..0ae32975 100644 --- a/ros/ros2/Utils.hpp +++ b/ros/ros2/Utils.hpp @@ -88,9 +88,9 @@ inline std::string FixFrameId(const std::string &frame_id) { return std::regex_replace(frame_id, std::regex("^/"), ""); } -inline auto GetTimestampField(const PointCloud2 &msg) { +inline auto GetTimestampField(const PointCloud2::ConstSharedPtr msg) { PointField timestamp_field; - for (const auto &field : msg.fields) { + for (const auto &field : msg->fields) { if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { timestamp_field = field; } @@ -115,10 +115,11 @@ inline auto NormalizeTimestamps(const std::vector ×tamps) { return timestamps_normalized; } -inline auto ExtractTimestampsFromMsg(const PointCloud2 &msg, const PointField &field) { +inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField &field) { auto extract_timestamps = [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { - const size_t n_points = msg.height * msg.width; + const size_t n_points = msg->height * msg->width; std::vector timestamps; timestamps.reserve(n_points); for (size_t i = 0; i < n_points; ++i, ++it) { @@ -133,11 +134,11 @@ inline auto ExtractTimestampsFromMsg(const PointCloud2 &msg, const PointField &f // According to the type of the timestamp == type, return a PointCloud2ConstIterator using sensor_msgs::PointCloud2ConstIterator; if (timestamp_field.datatype == PointField::UINT32) { - return extract_timestamps(PointCloud2ConstIterator(msg, timestamp_field.name)); + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); } else if (timestamp_field.datatype == PointField::FLOAT32) { - return extract_timestamps(PointCloud2ConstIterator(msg, timestamp_field.name)); + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); } else if (timestamp_field.datatype == PointField::FLOAT64) { - return extract_timestamps(PointCloud2ConstIterator(msg, timestamp_field.name)); + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); } // timestamp type not supported, please open an issue :) @@ -188,7 +189,7 @@ inline void FillPointCloud2Timestamp(const std::vector ×tamps, Poin for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i]; } -inline std::vector GetTimestamps(const PointCloud2 &msg) { +inline std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { auto timestamp_field = GetTimestampField(msg); // Extract timestamps from cloud_msg @@ -197,13 +198,13 @@ inline std::vector GetTimestamps(const PointCloud2 &msg) { return timestamps; } -inline std::vector PointCloud2ToEigen(const PointCloud2 &msg) { +inline std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg) { std::vector points; - points.reserve(msg.height * msg.width); - sensor_msgs::PointCloud2ConstIterator msg_x(msg, "x"); - sensor_msgs::PointCloud2ConstIterator msg_y(msg, "y"); - sensor_msgs::PointCloud2ConstIterator msg_z(msg, "z"); - for (size_t i = 0; i < msg.height * msg.width; ++i, ++msg_x, ++msg_y, ++msg_z) { + points.reserve(msg->height * msg->width); + sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z"); + for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) { points.emplace_back(*msg_x, *msg_y, *msg_z); } return points; @@ -213,7 +214,7 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector EigenToPointCloud2(const std::vector &points, @@ -232,6 +233,6 @@ inline std::unique_ptr EigenToPointCloud2(const std::vector