ros-perception / image_common

Common code for working with images in ROS
http://www.ros.org/wiki/image_common
124 stars 219 forks source link

Added type adaptation to Image Publisher raw transport #301

Open AiVerisimilitude opened 5 months ago

AiVerisimilitude commented 5 months ago

As the title suggests, we added type adaptation to the raw transport. To achieve this, there are a couple of modifications that were made:

Plugins don't mesh well with the idea of type-adaptation without knowing the type beforehand. So here the design decision was to collapse the raw publish plugin into an implicit part of the image_tranport publisher.

Since the publisher now needs to be templated for supporting different type-adapted types, a new higher level publisher is needed which is named PublisherBase with an alias created for backwards compatibility using Publisher = PublisherBase<sensor_msg::mgs::Image>;

Usage:

Type adaptation is only supported on the raw transport. Here is an example of a type for usage in type-adaptation:

template<>
struct rclcpp::TypeAdapter<
  std::string,
  sensor_msgs::msg::Image
>
{
  using is_specialized = std::true_type;
  using custom_type = std::string;
  using ros_message_type = sensor_msgs::msg::Image;

  static
  void
  convert_to_ros_message(
    const custom_type & source,
    ros_message_type & destination)
  {
    destination.encoding = source;
  }

  static
  void
  convert_to_custom(
    const ros_message_type & source,
    custom_type & destination)
  {
    destination = source.encoding;
  }
};

And an alias for easier to read code:

using AdaptedType = rclcpp::TypeAdapter<std::string, sensor_msgs::msg::Image>;

We can create a type-adapted publisher with:

std::shared_ptr<PublisherBase<AdaptedType>> publisher;

publisher = image_transport::create_type_adapted_publisher<AdaptedType>(node_.get(), "camera/image");

And publish the type-adapted message with:

publisher->publish(std::make_unique<std::string>("A string"));

A subscriber can be used the normal way without type-adaptation:

void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
{
//...
}

image_transport::Subscriber subscriber 
     = image_transport::create_subscription(node_.get(), "camera/image", imageCallback, "raw");

Or with type-adaptation:


void typeAdaptedCallback(std::unique_ptr<std::string> msg)
{
// ...
}

rclcpp::Subscription<AdaptedType>::SharedPtr subscriber;
// Assumes raw transport
subcriber 
    =  image_transport::create_type_adapted_subscription<AdaptedType>(node_.get(), "camera/image", typeAdaptedCallback);

As final notes, this PR takes a backwards compatible approach so that it does not break existing code, but I don't necessarily like the structure. For example, I don't like having 2 create functions, I would rather have one, but that would break existing implementations as we would need to return a shared pointer (in line with usage of regular publishers). However, I do find it a good starting point for discussion on how to move forward.

roncapat commented 5 months ago

@AiVerisimilitude basically in this way can we have publishers and subscribers that supports both intra-process/type-adaptation as well as inter-process/compression-plugins depending on the situation, without recompilation?

And, what happens with intra-process enabled, but no type-adapter? Will zero-copy work?

AiVerisimilitude commented 5 months ago

@roncapat Current subscribers are not really touched, I only added a new way to get one that is type adapted which is based on rclcpp::Subscriber directly. And yes, you are correct that you can have a plublisher that supports both intra-process/type-adapted as well as inter-process/compression without recompilation. Compression plugins remain unaffected by this change but there is the step of conversion if you have compression plugins. This is also something I want to think about the current behavior of rclcpp::publisher since if we have both a inter-process raw subscriber (or intra process disabled) and a compression subscriber, if we publish a type-adapted type, serialization of the type adapted type will happen 2x, once in rclcpp::Publisher#316 rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg); and again in our publisherBase rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *shared_msg) (since the compression plugins require the image message).

As for zero copy, I believe it does but it depends on the result of this call in rclcpp::Publisher#278 auto unique_msg = this->duplicate_ros_message_as_unique_ptr(msg); part of publish(const T & msg). If this does not trigger a copy when given a sensor_msgs::msg::Image::ConstSharedPtr then yes, it supports zero copy, otherwise no >_<

roncapat commented 5 months ago

Thank you for the feedback! Much appreciated.