Nacho/re enable ipc

[nachovizzo]

Fix IPC for OdometryServer in ROS 2 wrapper

Old Description

After #163 was merged, #171 was also added. But we missed the fact that the subscription must be done with unique_ptr if zero-copy intra-process communication should be enabled.

From the ROS 2 Intra-Process-Communication documentation:

This shows that the address of the message being received is the same as the one that was published and that it is not a copy. This is because we’re publishing and subscribing with std::unique_ptrs which allows ownership of a message to be moved around the system safely. You can also publish and subscribe with const & and std::shared_ptr, but zero-copy will not occur in that case.

After debating and testing with @roncapat I realized that I was panicking with no reason about unique_ptr Nevertheless, I've changed a bit (QoS) that was not working for me when composing the node and also relaxed a bit the rviz topic subscriptions

Open questions

• Do we want to keep 2 .launch.py files, one for "plain nodes" + "composable nodes", or just use the old launch file and let the users compose "their own" nodes? A: No, it doesn't make sense to have 2 launch files for me
• Does it make sense to have a launch file launching a single-composable node? Since rosbag is still not supporting IPC (Composable Player and Recorder nodes ros2/rosbag2#902) this seems like a silly idea. A: Yes it is a silly idea.

[nachovizzo]

@roncapat I know you might be quite busy :) But if you don't mind taking a look into this I'd be delighted, you are the authority in terms of IPC for me :P

Thanks :)

[roncapat]

@roncapat I know you might be quite busy :) But if you don't mind taking a look into this I'd be delighted, you are the authority in terms of IPC for me :P

Thanks :)

Hi Ignacio, from my experience using const ConstSharedPtr is totally fine for IPC! All my code is based on this pattern - publish std::move(UniquePtr) & subscribe const ConstSharedPtr.

Basically with the constness you are guaranteeing the IPC manager that you will not edit that memory region, but read only, so you can share among multiple subscribers with no issues IIRC.

[nachovizzo]

@roncapat I know you might be quite busy :) But if you don't mind taking a look into this I'd be delighted, you are the authority in terms of IPC for me :P
Thanks :)

Hi Ignacio, from my experience using const ConstSharedPtr is totally fine for IPC! All my code is based on this pattern - publish std::move(UniquePtr) & subscribe const ConstSharedPtr.

Wow! Thanks for the fast reply!

So I guess it's again me just being paranoid. Then this doesn't change anything right? As long as the "data producers" use unique_ptr?

But then how does it work with the shared_ptr? Does it still eat the same piece of memory on the heap?

[roncapat]

Yeah basically this promotion happens when using the pattern I described:
https://github.com/ros2/rclcpp/blob/fff009a75100f2afd8ef1c3863620bf5ebe67708/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp#L204-L206

[nachovizzo]

Yeah basically this promotion happens when using the pattern I described: https://github.com/ros2/rclcpp/blob/fff009a75100f2afd8ef1c3863620bf5ebe67708/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp#L204-L206

Oh yeah! thanks for pointing to the implementation! I was doing a bit of "testing" anyway, so posting it here in case anyone is also curious.

namespace kiss_icp_ros {
class Dummy : public rclcpp::Node {
public:
    /// Dummy constructor
    Dummy() = delete;

    explicit Dummy(const rclcpp::NodeOptions &options) : rclcpp::Node("dummy_node", options) {
        pointcloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
            "pointcloud_topic", rclcpp::SensorDataQoS(),
            std::bind(&Dummy::CallBack, this, std::placeholders::_1));
        pointcloud_pub_ =
            create_publisher<sensor_msgs::msg::PointCloud2>("/kiss/input", rclcpp::SensorDataQoS());
        RCLCPP_INFO(this->get_logger(), "DUMMY node initialized");
    }

    /// Register new frame
    void CallBack(sensor_msgs::msg::PointCloud2::UniquePtr msg) {
        printf("Published message with stamp: %d, and address: 0x%" PRIXPTR "\n",
               msg->header.stamp.sec, reinterpret_cast<std::uintptr_t>(msg.get()));
        pointcloud_pub_->publish(std::move(msg));
    }

    /// Data subscribers.
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
};
}  // namespace kiss_icp_ros
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(kiss_icp_ros::Dummy)

Then added this on the composable launch file:

   ComposableNode(
        package="kiss_icp",
        plugin="kiss_icp_ros::Dummy",
        name="dummy_node",
        extra_arguments=[{"use_intra_process_comms": True}],
        remappings=[("pointcloud_topic", LaunchConfiguration("topic"))],
    ),

The correspondence print msg on the "consumer" on this composition and the results are just amazing:

[component_container-1] Published message with stamp: 1697626087, and address: 0x5653678DE080
[component_container-1] Received message with stamp: 1697626087, and address: 0x5653678DE080

[nachovizzo]

@roncapat I've updated the PR with just 1 bit. Since I was still getting this error, thanks a lot for looking over !!!

[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to[component_container-1] [ERROR 1698948736.701288673] [container_kiss_icp]: Component constructor threw an exception: intraprocess communication is not allowed with a zero qos history depth value (on_load_node() at ../../rclcpp_components/src/component_manager.cpp:278)

[benemer]

Sorry for the late reply! I was planning to dive more into ROS2 components and IPC when reviewing this, but I did not find the time. However, this PR looks fine from my side!