Nacho/sync ros wrappers (#188)

* Inline all util functions

* Merge implementations (wip)

* ROS 2 instead of ROS2

* Sacrufice missuse of pointers for similar wrappers

* Convert everything to pointers

* Propagate use of pointers

I hate this

* Consisten with ROS 1

ros/CMakeLists.txt

@@ -41,7 +41,7 @@ else()
 endif()
 
 if("$ENV{ROS_VERSION}" STREQUAL "1")
-  message(STATUS "KISS-ICP-ROS1 wrapper will be compiled")
+  message(STATUS "KISS-ICP ROS 1 wrapper will be compiled")
   find_package(
     catkin REQUIRED
     COMPONENTS geometry_msgs
@@ -54,14 +54,14 @@ if("$ENV{ROS_VERSION}" STREQUAL "1")
                tf2_ros)
   catkin_package()
 
-  # ROS1 node
+  # ROS 1 node
   add_executable(odometry_node ros1/OdometryServer.cpp)
   target_include_directories(odometry_node PUBLIC include ${catkin_INCLUDE_DIRS})
   target_link_libraries(odometry_node kiss_icp::pipeline ${catkin_LIBRARIES})
   install(TARGETS odometry_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
   install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 elseif("$ENV{ROS_VERSION}" STREQUAL "2")
-  message(STATUS "KISS-ICP-ROS2 wrapper will be compiled")
+  message(STATUS "KISS-ICP ROS 2 wrapper will be compiled")
 
   find_package(ament_cmake REQUIRED)
   find_package(nav_msgs REQUIRED)
@@ -71,7 +71,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
   find_package(sensor_msgs REQUIRED)
   find_package(tf2_ros REQUIRED)
 
-  # ROS2 node
+  # ROS 2 node
   add_library(odometry_component SHARED ros2/OdometryServer.cpp)
   target_include_directories(odometry_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
   target_link_libraries(odometry_component kiss_icp::pipeline)

ros/README.md

@@ -5,7 +5,7 @@ work in progress and we are very happy to receive any contribution from the comu
 
 https://user-images.githubusercontent.com/21349875/214578180-b1d2431c-8fff-440e-aa6e-99a1d85989b5.mp4
 
-## ROS2
+## ROS 2
 
 ### How to build
 
@@ -37,7 +37,7 @@ and then,
 ros2 bag play <path>*.bag
 ```
 
-## ROS1
+## ROS 1
 
 ### How to build
 

ros/launch/odometry.launch.py

@@ -37,7 +37,7 @@ def generate_launch_description():
     current_pkg = FindPackageShare("kiss_icp")
     return LaunchDescription(
         [
-            # ROS2 parameters
+            # ROS 2 parameters
             DeclareLaunchArgument("topic", description="sensor_msg/PointCloud2 topic to process"),
             DeclareLaunchArgument("bagfile", default_value=""),
             DeclareLaunchArgument("visualize", default_value="true"),

ros/package.xml

@@ -33,11 +33,11 @@
   <!-- Common ROS 1 / ROS 2 dependencies -->
   <build_depend>ros_environment</build_depend>
 
-  <!-- ROS1 dependencies -->
+  <!-- ROS 1 dependencies -->
   <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
   <depend condition="$ROS_VERSION == 1">roscpp</depend>
 
-  <!-- ROS2 dependencies -->
+  <!-- ROS 2 dependencies -->
   <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
   <depend condition="$ROS_VERSION == 2">rcutils</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp</depend>

ros/ros1/OdometryServer.cpp

@@ -21,6 +21,8 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 #include <Eigen/Core>
+#include <memory>
+#include <utility>
 #include <vector>
 
 // KISS-ICP-ROS
@@ -30,7 +32,7 @@
 // KISS-ICP
 #include "kiss_icp/pipeline/KissICP.hpp"
 
-// ROS
+// ROS 1 headers
 #include "geometry_msgs/PoseStamped.h"
 #include "geometry_msgs/TransformStamped.h"
 #include "nav_msgs/Odometry.h"
@@ -44,6 +46,10 @@
 
 namespace kiss_icp_ros {
 
+using utils::EigenToPointCloud2;
+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_);
@@ -64,14 +70,14 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle
     odometry_ = kiss_icp::pipeline::KissICP(config_);
 
     // Initialize subscribers
-    pointcloud_sub_ = nh_.subscribe<const sensor_msgs::PointCloud2 &>(
-        "pointcloud_topic", queue_size_, &OdometryServer::RegisterFrame, this);
+    pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_topic", queue_size_,
+                                                              &OdometryServer::RegisterFrame, this);
 
     // Initialize publishers
     odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("odometry", queue_size_);
     frame_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("frame", queue_size_);
     kpoints_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("keypoints", queue_size_);
-    local_map_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("local_map", queue_size_);
+    map_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("local_map", queue_size_);
 
     // Initialize trajectory publisher
     path_msg_.header.frame_id = odom_frame_;
@@ -99,11 +105,11 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle
     ROS_INFO("KISS-ICP ROS 1 Odometry Node Initialized");
 }
 
-void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2 &msg) {
-    const auto points = utils::PointCloud2ToEigen(msg);
+void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+    const auto points = PointCloud2ToEigen(msg);
     const auto timestamps = [&]() -> std::vector<double> {
         if (!config_.deskew) return {};
-        return utils::GetTimestamps(msg);
+        return GetTimestamps(msg);
     }();
 
     // Register frame, main entry point to KISS-ICP pipeline
@@ -118,7 +124,7 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2 &msg) {
 
     // Broadcast the tf
     geometry_msgs::TransformStamped transform_msg;
-    transform_msg.header.stamp = msg.header.stamp;
+    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();
@@ -130,37 +136,37 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2 &msg) {
     transform_msg.transform.translation.z = t_current.z();
     tf_broadcaster_.sendTransform(transform_msg);
 
-    // publish odometry msg
-    nav_msgs::Odometry odom_msg;
-    odom_msg.header.stamp = msg.header.stamp;
-    odom_msg.header.frame_id = odom_frame_;
-    odom_msg.child_frame_id = child_frame_;
-    odom_msg.pose.pose.orientation.x = q_current.x();
-    odom_msg.pose.pose.orientation.y = q_current.y();
-    odom_msg.pose.pose.orientation.z = q_current.z();
-    odom_msg.pose.pose.orientation.w = q_current.w();
-    odom_msg.pose.pose.position.x = t_current.x();
-    odom_msg.pose.pose.position.y = t_current.y();
-    odom_msg.pose.pose.position.z = t_current.z();
-    odom_publisher_.publish(odom_msg);
-
     // publish trajectory msg
     geometry_msgs::PoseStamped pose_msg;
-    pose_msg.pose = odom_msg.pose.pose;
-    pose_msg.header = odom_msg.header;
+    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_;
     path_msg_.poses.push_back(pose_msg);
     traj_publisher_.publish(path_msg_);
 
+    // publish odometry msg
+    auto odom_msg = std::make_unique<nav_msgs::Odometry>();
+    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
-    std_msgs::Header frame_header = msg.header;
+    auto frame_header = msg->header;
     frame_header.frame_id = child_frame_;
-    frame_publisher_.publish(utils::EigenToPointCloud2(frame, frame_header));
-    kpoints_publisher_.publish(utils::EigenToPointCloud2(keypoints, frame_header));
+    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
-    std_msgs::Header local_map_header = msg.header;
+    auto local_map_header = msg->header;
     local_map_header.frame_id = odom_frame_;
-    local_map_publisher_.publish(utils::EigenToPointCloud2(odometry_.LocalMap(), local_map_header));
+    map_publisher_.publish(*std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header)));
 }
 
 }  // namespace kiss_icp_ros

ros/ros1/OdometryServer.hpp

@@ -40,7 +40,7 @@ class OdometryServer {
 
 private:
     /// Register new frame
-    void RegisterFrame(const sensor_msgs::PointCloud2 &msg);
+    void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg);
 
     /// Ros node stuff
     ros::NodeHandle nh_;
@@ -59,7 +59,7 @@ class OdometryServer {
     nav_msgs::Path path_msg_;
     ros::Publisher frame_publisher_;
     ros::Publisher kpoints_publisher_;
-    ros::Publisher local_map_publisher_;
+    ros::Publisher map_publisher_;
 
     /// KISS-ICP
     kiss_icp::pipeline::KissICP odometry_;

ros/ros1/Utils.hpp

@@ -25,6 +25,7 @@
 #include <Eigen/Core>
 #include <algorithm>
 #include <cstddef>
+#include <memory>
 #include <regex>
 #include <string>
 #include <vector>
@@ -37,13 +38,13 @@ using PointCloud2 = sensor_msgs::PointCloud2;
 using PointField = sensor_msgs::PointField;
 using Header = std_msgs::Header;
 
-std::string FixFrameId(const std::string &frame_id) {
+inline std::string FixFrameId(const std::string &frame_id) {
     return std::regex_replace(frame_id, std::regex("^/"), "");
 }
 
-auto GetTimestampField(const PointCloud2 &msg) {
+inline auto GetTimestampField(const PointCloud2::ConstPtr 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;
         }
@@ -55,7 +56,7 @@ auto GetTimestampField(const PointCloud2 &msg) {
 }
 
 // Normalize timestamps from 0.0 to 1.0
-auto NormalizeTimestamps(const std::vector<double> &timestamps) {
+inline auto NormalizeTimestamps(const std::vector<double> &timestamps) {
     const double max_timestamp = *std::max_element(timestamps.cbegin(), timestamps.cend());
     // check if already normalized
     if (max_timestamp < 1.0) return timestamps;
@@ -65,15 +66,15 @@ auto NormalizeTimestamps(const std::vector<double> &timestamps) {
     return timestamps_normalized;
 }
 
-auto ExtractTimestampsFromMsg(const PointCloud2 &msg, const PointField &field) {
+inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstPtr msg, const PointField &field) {
     // Extract timestamps from cloud_msg
-    const size_t n_points = msg.height * msg.width;
+    const size_t n_points = msg->height * msg->width;
     std::vector<double> timestamps;
     timestamps.reserve(n_points);
 
     // Option 1: Timestamps are unsigned integers -> epoch time.
     if (field.name == "t" || field.name == "timestamp") {
-        sensor_msgs::PointCloud2ConstIterator<uint32_t> msg_t(msg, field.name);
+        sensor_msgs::PointCloud2ConstIterator<uint32_t> msg_t(*msg, field.name);
         for (size_t i = 0; i < n_points; ++i, ++msg_t) {
             timestamps.emplace_back(static_cast<double>(*msg_t));
         }
@@ -83,39 +84,41 @@ auto ExtractTimestampsFromMsg(const PointCloud2 &msg, const PointField &field) {
 
     // Option 2: Timestamps are floating point values between 0.0 and 1.0
     // field.name == "timestamp"
-    sensor_msgs::PointCloud2ConstIterator<double> msg_t(msg, field.name);
+    sensor_msgs::PointCloud2ConstIterator<double> msg_t(*msg, field.name);
     for (size_t i = 0; i < n_points; ++i, ++msg_t) {
         timestamps.emplace_back(*msg_t);
     }
     return timestamps;
 }
 
-auto CreatePointCloud2Msg(const size_t n_points, const Header &header, bool timestamp = false) {
-    PointCloud2 cloud_msg;
-    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
-    cloud_msg.header = header;
-    cloud_msg.header.frame_id = FixFrameId(cloud_msg.header.frame_id);
-    cloud_msg.fields.clear();
+inline std::unique_ptr<PointCloud2> CreatePointCloud2Msg(const size_t n_points,
+                                                         const Header &header,
+                                                         bool timestamp = false) {
+    auto cloud_msg = std::make_unique<PointCloud2>();
+    sensor_msgs::PointCloud2Modifier modifier(*cloud_msg);
+    cloud_msg->header = header;
+    cloud_msg->header.frame_id = FixFrameId(cloud_msg->header.frame_id);
+    cloud_msg->fields.clear();
     int offset = 0;
-    offset = addPointField(cloud_msg, "x", 1, PointField::FLOAT32, offset);
-    offset = addPointField(cloud_msg, "y", 1, PointField::FLOAT32, offset);
-    offset = addPointField(cloud_msg, "z", 1, PointField::FLOAT32, offset);
+    offset = addPointField(*cloud_msg, "x", 1, PointField::FLOAT32, offset);
+    offset = addPointField(*cloud_msg, "y", 1, PointField::FLOAT32, offset);
+    offset = addPointField(*cloud_msg, "z", 1, PointField::FLOAT32, offset);
     offset += sizeOfPointField(PointField::FLOAT32);
     if (timestamp) {
         // asuming timestamp on a velodyne fashion for now (between 0.0 and 1.0)
-        offset = addPointField(cloud_msg, "time", 1, PointField::FLOAT64, offset);
+        offset = addPointField(*cloud_msg, "time", 1, PointField::FLOAT64, offset);
         offset += sizeOfPointField(PointField::FLOAT64);
     }
 
     // Resize the point cloud accordingly
-    cloud_msg.point_step = offset;
-    cloud_msg.row_step = cloud_msg.width * cloud_msg.point_step;
-    cloud_msg.data.resize(cloud_msg.height * cloud_msg.row_step);
+    cloud_msg->point_step = offset;
+    cloud_msg->row_step = cloud_msg->width * cloud_msg->point_step;
+    cloud_msg->data.resize(cloud_msg->height * cloud_msg->row_step);
     modifier.resize(n_points);
     return cloud_msg;
 }
 
-void FillPointCloud2XYZ(const std::vector<Eigen::Vector3d> &points, PointCloud2 &msg) {
+inline void FillPointCloud2XYZ(const std::vector<Eigen::Vector3d> &points, PointCloud2 &msg) {
     sensor_msgs::PointCloud2Iterator<float> msg_x(msg, "x");
     sensor_msgs::PointCloud2Iterator<float> msg_y(msg, "y");
     sensor_msgs::PointCloud2Iterator<float> msg_z(msg, "z");
@@ -127,12 +130,12 @@ void FillPointCloud2XYZ(const std::vector<Eigen::Vector3d> &points, PointCloud2
     }
 }
 
-void FillPointCloud2Timestamp(const std::vector<double> &timestamps, PointCloud2 &msg) {
+inline void FillPointCloud2Timestamp(const std::vector<double> &timestamps, PointCloud2 &msg) {
     sensor_msgs::PointCloud2Iterator<double> msg_t(msg, "time");
     for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i];
 }
 
-std::vector<double> GetTimestamps(const PointCloud2 &msg) {
+inline std::vector<double> GetTimestamps(const PointCloud2::ConstPtr msg) {
     auto timestamp_field = GetTimestampField(msg);
 
     // Extract timestamps from cloud_msg
@@ -141,30 +144,31 @@ std::vector<double> GetTimestamps(const PointCloud2 &msg) {
     return timestamps;
 }
 
-std::vector<Eigen::Vector3d> PointCloud2ToEigen(const PointCloud2 &msg) {
+inline std::vector<Eigen::Vector3d> PointCloud2ToEigen(const PointCloud2::ConstPtr msg) {
     std::vector<Eigen::Vector3d> points;
-    points.reserve(msg.height * msg.width);
-    sensor_msgs::PointCloud2ConstIterator<float> msg_x(msg, "x");
-    sensor_msgs::PointCloud2ConstIterator<float> msg_y(msg, "y");
-    sensor_msgs::PointCloud2ConstIterator<float> 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<float> msg_x(*msg, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> msg_y(*msg, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> 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;
 }
 
-PointCloud2 EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points, const Header &header) {
-    PointCloud2 msg = CreatePointCloud2Msg(points.size(), header);
-    FillPointCloud2XYZ(points, msg);
+inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
+                                                       const Header &header) {
+    auto msg = CreatePointCloud2Msg(points.size(), header);
+    FillPointCloud2XYZ(points, *msg);
     return msg;
 }
 
-PointCloud2 EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
-                               const std::vector<double> &timestamps,
-                               const Header &header) {
-    PointCloud2 msg = CreatePointCloud2Msg(points.size(), header, true);
-    FillPointCloud2XYZ(points, msg);
-    FillPointCloud2Timestamp(timestamps, msg);
+inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
+                                                       const std::vector<double> &timestamps,
+                                                       const Header &header) {
+    auto msg = CreatePointCloud2Msg(points.size(), header, true);
+    FillPointCloud2XYZ(points, *msg);
+    FillPointCloud2Timestamp(timestamps, *msg);
     return msg;
 }