search

ros2 / rclcpp

rclcpp (ROS Client Library for C++)
Apache License 2.0
513 stars 412 forks source link

Type Adapter cannot convert one RosMsg to another RosMsg #2446

Open allbluelai opened 4 months ago

allbluelai commented 4 months ago

Bug report

Required Info:

Steps to reproduce issue

# TestMsg.msg
uint32 id
uint64 time
// rosAdapter.hpp
#ifndef ROS_ADAPTER_HPP
#define ROS_ADAPTER_HPP

#include <string>
#include <iostream>
#include "rclcpp/type_adapter.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/byte_multi_array.hpp"
#include "std_msgs/msg/header.hpp"
#include "tutorial_interfaces/msg/test_msg.hpp"

#define ROS_TYPE
#ifdef ROS_TYPE
// fail
using MsgT = tutorial_interfaces::msg::TestMsg;
#else
// work fine
using MsgT = std::string;
#endif // ROS_TYPE

template <>
struct rclcpp::TypeAdapter<MsgT, std_msgs::msg::ByteMultiArray>
{
    using is_specialized = std::true_type;
    using custom_type = MsgT;
    using ros_message_type = std_msgs::msg::ByteMultiArray;

    static void
    convert_to_ros_message(
        const custom_type &source,
        ros_message_type &destination)
    {
        std::cout << "convert_to_ros_message!!!" << std::endl;
#ifdef ROS_TYPE
        destination.data.resize(12);
        destination.data[0] = source.id;
        destination.data[1] = source.time;
#else
        destination.data.resize(source.size());
        memcpy(destination.data.data(), source.data(), source.size());
#endif
    }

    static void
    convert_to_custom(
        const ros_message_type &source,
        custom_type &destination)
    {
        std::cout << "convert_to_custom!!!" << std::endl;
#ifdef ROS_TYPE
        std::cout << "source size = " << source.data.size() << std::endl;
        destination.id = source.data[0];
        std::cout << "destination.id = " << destination.id << std::endl;
        destination.time = source.data[4];
#else
        destination.resize(source.data.size());
        memcpy(destination.data(), source.data.data(), source.data.size());
#endif
    }
};

RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(MsgT, std_msgs::msg::ByteMultiArray);

#endif // ROS_ADAPTER_HPP
// pub.cpp
#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rosAdapter.hpp"

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
      : Node("minimal_publisher"), count_(0)
  {
    auto timer_callback =
        [this]() -> void
    {
#ifdef ROS_TYPE
      MsgT msg;
      msg.id = this->count_++;
      msg.time = 0x12345678;
#else
      MsgT msg = "my name!";
#endif
      this->publisher_->publish(msg);
    };

    publisher_ = this->create_publisher<MsgT>("topic", 10);
    timer_ = create_wall_timer(30ms, timer_callback);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<MsgT>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}
// sub.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include "rosAdapter.hpp"

class MinimalSubscriber : public rclcpp::Node
{
public:
  MinimalSubscriber()
      : Node("minimal_subscriber")
  {
    auto callback = [this](const MsgT &msg)
    {
#ifdef ROS_TYPE
      std::cout << "msg.id = " << msg.id << std::endl;
      std::cout << "msg.time = " << msg.time << std::endl;
#else
      std::cout << "msg = " << msg << std::endl;
#endif
    };

    subscription_ = this->create_subscription<MsgT>("topic", 10, callback);
  }

private:
  rclcpp::Subscription<MsgT>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

Expected behavior

The subscriber (sub.cpp) can output the message.

Actual behavior

A segmentation fault occurs because the subscriber receives zero data when calling the convert_to_custom function in TypeAdapter.

$ ros2 run topics sub
convert_to_custom!!!
source size = 0
[ros2run]: Segmentation fault

From the above-presented result, the subscriber enters the convert_to_custom function with a data size of zero. Consequently, the subscriber is unable to switch the ros_message_type to the custom_type (which is another RosMsg Type).

Additional information

It appears that TypeAdapter is unable to convert one RosMsg Type to another RosMsg Type.

clalancette commented 4 months ago

Yeah, this limitation is because the template logic attempts to detect whether the message is of ROSMessageType or of CustomType. In this case, since both are ROSMessageType, it will almost certainly generate the wrong code.

Off-hand, I don't know of a simple way to fix this, but there may be something we can do in the templates to detect the situation and fail to compile, rather than fail at runtime. But I'm not template metaprogramming expert, so I don't exactly know how this would be done.

allbluelai commented 4 months ago

I get more info as follows:

# Msg2.msg
uint32 u32
uint64 u64
// rosAdapter.hpp
#ifndef ROS_ADAPTER_HPP
#define ROS_ADAPTER_HPP

#include <string>
#include <iostream>
#include "rclcpp/type_adapter.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/byte_multi_array.hpp"
#include "std_msgs/msg/header.hpp"
#include "tutorial_interfaces/msg/test_msg.hpp"
#include "tutorial_interfaces/msg/msg2.hpp"

#define ROS_TYPE
#ifdef ROS_TYPE
using MsgT = tutorial_interfaces::msg::TestMsg;
#else
using MsgT = std::string;
#endif // ROS_TYPE

template <>
struct rclcpp::TypeAdapter<MsgT, tutorial_interfaces::msg::Msg2>
{
    using is_specialized = std::true_type;
    using custom_type = MsgT;
    using ros_message_type = tutorial_interfaces::msg::Msg2;

    static void
    convert_to_ros_message(
        const custom_type &source,
        ros_message_type &destination)
    {
        std::cout << "convert_to_ros_message!!!" << std::endl;
#ifdef ROS_TYPE
        destination.u32 = source.id;
        destination.u64 = source.time;
#else
        destination.data.resize(source.size());
        memcpy(destination.data.data(), source.data(), source.size());
#endif
    }

    static void
    convert_to_custom(
        const ros_message_type &source,
        custom_type &destination)
    {
        std::cout << "convert_to_custom!!!" << std::endl;
#ifdef ROS_TYPE
        destination.id = source.u32;
        destination.time = source.u64;
        std::cout << "destination.id = " << destination.id << std::endl;
        std::cout << "destination.time = " << destination.time << std::endl;
#else
        memcpy(destination.data(), source.data.data(), source.data.size());
#endif
    }
};

RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(MsgT, tutorial_interfaces::msg::Msg2);

#endif // ROS_ADAPTER_HPP

In subscriber:

$ ros2 run topics sub
convert_to_custom!!!
destination.id = 51
destination.time = 305419896
msg.id = 51
msg.time = 305419896
convert_to_custom!!!
destination.id = 52
destination.time = 305419896
msg.id = 52
msg.time = 305419896
convert_to_custom!!!
destination.id = 53
destination.time = 305419896
msg.id = 53
msg.time = 305419896
convert_to_custom!!!
destination.id = 54
destination.time = 305419896
msg.id = 54
msg.time = 305419896
...

It works fine from one RosMsg Type (tutorial_interfaces::msg::TestMsg) to another Type (tutorial_interfaces::msg::Msg2).

But, the ros2 topic info get wrong output:

$ ros2 topic info /topic
Type: tutorial_interfaces/msg/TestMsg
Publisher count: 1
Subscription count: 0

The Type field above should be tutorial_interfaces::msg::Msg2.