Provide direct serialization of ROS2 messsage to ROS1 streams

[dbking77]

Short Description

Provide functions to potentially save some CPU when converting ROS messages by allow "direct" serialization of ROS2 message classes to ROS1 streams.

Long Description

Currently, when ros1_bridge bridges a ROS2 topics to a ROS1 topics, it requires three steps

1. Deserialize ROS2 stream into a ROS2 message
2. Use convert_2_to_1 to copy members from ROS2 object to ROS1 message
3. Serialize ROS1 into ROS1 stream.

Using new feature removes a conversion step

1. Deserialize ROS2 stream into a ROS2 message
2. Serialize ROS2 into ROS1 stream.

Here's what the convert_2_to_1 function looks like geometry_msgs::Vector3

template<>
void Factory< ... >::convert_2_to_1(
  const geometry_msgs::msg::Vector3 & ros2_msg,
  geometry_msgs::Vector3 & ros1_msg)
{
  ros1_msg.x = ros2_msg.x;
  ros1_msg.y = ros2_msg.y;
  ros1_msg.z = ros2_msg.z;
}

Here's what the direct serialization function looks like (same internal function is used to implement read, write, and length)

template<>
template<typename STREAM_T, typename ROS2_MSG_T>
void
Factory<..>::msg_2_to_1_stream(
  STREAM_T& stream,
  ROS2_MSG_T& ros2_msg)
{
  stream.next(ros2_msg.x);
  stream.next(ros2_msg.y);
  stream.next(ros2_msg.z);
}

Auto-Generated Code Examples

PoseStamped

template<>
template<typename STREAM_T, typename ROS2_MSG_T>
void
Factory<...>::msg_2_to_1_stream(
  STREAM_T& stream,
  ROS2_MSG_T& ros2_msg)
{
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::msg_2_to_1_stream(stream, ros2_msg.header);

  Factory<
    geometry_msgs::Pose,
    geometry_msgs::msg::Pose
  >::msg_2_to_1_stream(stream, ros2_msg.pose);
}

sensor_msgs::Image

Message Definition

[sensor_msgs/Image]:
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data

Streaming Code

template<>
template<typename STREAM_T, typename ROS2_MSG_T>
void
Factory<...>::msg_2_to_1_stream(
  STREAM_T& stream,
  ROS2_MSG_T& ros2_msg)
{
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::msg_2_to_1_stream(stream, ros2_msg.header);
  stream.next(ros2_msg.height);
  stream.next(ros2_msg.width);
  stream.next(ros2_msg.encoding);
  stream.next(ros2_msg.is_bigendian);
  stream.next(ros2_msg.step);
  streamVectorSize(stream, ros2_msg.data);
  streamPrimitiveVector(stream, ros2_msg.data);
}

sensor_msgs::Imu

Imu message definition

std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

Streaming code

template<>
template<typename STREAM_T, typename ROS2_MSG_T>
void
Factory<
  sensor_msgs::Imu,
  sensor_msgs::msg::Imu
>::msg_2_to_1_stream(STREAM_T& stream,
                     ROS2_MSG_T& ros2_msg)
{
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::msg_2_to_1_stream(stream, ros2_msg.header);

  Factory<
    geometry_msgs::Quaternion,
    geometry_msgs::msg::Quaternion
  >::msg_2_to_1_stream(stream, ros2_msg.orientation);

  streamPrimitiveVector(stream, ros2_msg.orientation_covariance);

  Factory<
    geometry_msgs::Vector3,
    geometry_msgs::msg::Vector3
  >::msg_2_to_1_stream(stream, ros2_msg.angular_velocity);

  streamPrimitiveVector(stream, ros2_msg.angular_velocity_covariance);

  Factory<
    geometry_msgs::Vector3,
    geometry_msgs::msg::Vector3
  >::msg_2_to_1_stream(stream, ros2_msg.linear_acceleration);
  streamPrimitiveVector(stream, ros2_msg.linear_acceleration_covariance);
}

[gbiggs]

Please rebase your PR to incorporate the changes from #377.

[dbking77]

Please rebase your PR to incorporate the changes from #377.

Done

[dbking77]

@gbiggs It looks like some/all the of integration tests were failing because the rosmaster wasn't coming up?

[gbiggs]

The convert_all_in_one_stream function name is no better than the previous function names. You are relying on the types of the function arguments for the correct function to be called. This is fragile and difficult to understand and maintain. Please don't imitate the ROS 1 API naming scheme. Follow the ros1_bridge naming scheme and have explicitly separate functions for converting from ROS 1 to ROS 2 and the reverse.

[dbking77]

The convert_all_in_one_stream function name is no better than the previous function names. You are relying on the types of the function arguments for the correct function to be called. This is fragile and difficult to understand and maintain. Please don't imitate the ROS 1 API naming scheme. Follow the ros1_bridge naming scheme and have explicitly separate functions converting from ROS 1 to ROS 2 and the reverse.

convert_all_in_one_stream is an "internal" function that is used as the implementation the other functions. I think this is a better approach than having 3 versions of the template generation code in interface_factories.cpp.em. I'm my experience the template generation code is nearly impossible to read, change, or debug. If any python code is incorrect, the error message doesn't even provide an line number for the issue.

I'm not trying to emulate the ROS1 naming scheme, I'm am using a strategy from ROS1 to avoid repeating code 3x times.

convert_all_in_one_stream is not meant to be used externally. I can make it private and change the name to internal_stream_translate_helper or something (Please provide suggestions?)

The write and read functions have been changes to exactly match the naming convention of ros1_bridge. The argument ordering have been changed to be "correct".

• convert_2_to_1(ros2_msg, out_stream)
• convert_1_to_2(in_stream, ros2_msg)

[gbiggs]

convert_all_in_one_stream is not meant to be used externally. I can make it private and change the name to internal_stream_translate_helper or something (Please provide suggestions?)

I think that would be better.

[dbking77]

@gbiggs I made the name change, but couldn't make the function private, because a whole bunch of other Factory types need to use it.

[dbking77]

@gbiggs is it possible to trigger a full test run?

[dbking77]

@gbiggs ping

[gbiggs]

CI:

[gbiggs]

Please fix the build failures.

[dbking77]

I was able to reproduce this build failure by compiling with optimization turned on

colcon build --packages-select ros1_bridge --cmake-force-configure --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

It looks like the RelWithDebInfo build causes the internal functions to get inlined inside the generated *.cpp files so there are no symbols for them. I can't define the function specializations in factory.hpp because I can't have a function specialization in a non-specialized class type. Since header files are not generated, its would be difficult add generation of type-specific headers.

My work-around is to just use operator overloading for the different stream types instead of templating and template specialization. The generated code now has 3 functions, instead of just 1 templated function

For example the auto-generated C++ code for PoseStamped now looks like:

template<>
void
Factory<
  geometry_msgs::PoseStamped,
  geometry_msgs::msg::PoseStamped
>::internal_stream_translate_helper(
  ros::serialization::OStream & stream,
  geometry_msgs::msg::PoseStamped const & ros2_msg)
{

  // write non-array field
  // write sub message field
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::internal_stream_translate_helper(stream, ros2_msg.header);

  // write non-array field
  // write sub message field
  Factory<
    geometry_msgs::Pose,
    geometry_msgs::msg::Pose
  >::internal_stream_translate_helper(stream, ros2_msg.pose);
}


template<>
void
Factory<
  geometry_msgs::PoseStamped,
  geometry_msgs::msg::PoseStamped
>::internal_stream_translate_helper(
  ros::serialization::IStream & stream,
  geometry_msgs::msg::PoseStamped  & ros2_msg)
{

  // write non-array field
  // write sub message field
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::internal_stream_translate_helper(stream, ros2_msg.header);

  // write non-array field
  // write sub message field
  Factory<
    geometry_msgs::Pose,
    geometry_msgs::msg::Pose
  >::internal_stream_translate_helper(stream, ros2_msg.pose);
}


template<>
void
Factory<
  geometry_msgs::PoseStamped,
  geometry_msgs::msg::PoseStamped
>::internal_stream_translate_helper(
  ros::serialization::LStream & stream,
  geometry_msgs::msg::PoseStamped const & ros2_msg)
{

  // write non-array field
  // write sub message field
  Factory<
    std_msgs::Header,
    std_msgs::msg::Header
  >::internal_stream_translate_helper(stream, ros2_msg.header);

  // write non-array field
  // write sub message field
  Factory<
    geometry_msgs::Pose,
    geometry_msgs::msg::Pose
  >::internal_stream_translate_helper(stream, ros2_msg.pose);
}

I'm now divided on whether I should have internal_stream_translate_helper based on overloading, or I should just have 3 different code generations routines for the different convert and length functions. I'll prototype this out to see if it seems better or worse than this approach.

@gbiggs Would it be possible to trigger a CI build on the farm just to see if this makes it through?

[gbiggs]

Here you go:

[dbking77]

@gbiggs Looks like stuff compiled, but most/all live tests are failing because they can't access the rosmaster ?

   11: [ros1_talker_ros2_listener_across_dynamic_bridge__dynamic_bridge-2] [ERROR] [1669537320.951368727]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...
  11: [ros1_talker_ros2_listener_across_dynamic_bridge__talker-3] [ERROR] [1669537321.152250959]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...

[gbiggs]

That seems more likely to be a problem with CI rather than this change. Let's try again.

[methylDragon]

It looks like just linting issues @gbiggs https://ci.ros2.org/job/ci_packaging_linux/536/testReport/

[methylDragon]

@dbking77 could you fix those linting issues? :>

[dbking77]

@gbiggs Fixed linting issues

[dbking77]

@gbiggs Can you re-trigger? apt-get update failed

Invoking "sudo apt-get update"
Error: The process '/usr/bin/sudo' failed with exit code 100

[gbiggs]

[dbking77]

@gbiggs Looks like build/test is failing because of missing release file for Jammy debians. It could just be a problem with http://packages.ros.org/ros/ubuntu mirror, but maybe is a problem in general?

   E: The repository 'http://packages.ros.org/ros/ubuntu jammy Release' does not have a Release file.
  W: http://packages.ros.org/ros2/ubuntu/dists/jammy/InRelease: Key is stored in legacy trusted.gpg keyring (/etc/apt/trusted.gpg), see the DEPRECATION section in apt-key(8) for details.

[gbiggs]

Where are you seeing that output?

[dbking77]

THe CI / ros1_bridge (pull_request) : https://github.com/ros2/ros1_bridge/actions/runs/3682873863/jobs/6230902918

[gbiggs]

That automatic check hasn't been re-run; the manual CI run I did above passes so we're happy anyway.

[dbking77]

Is there anything else I need to do for this to get merged?

[gbiggs]

CI passed, despite the automatic check failing, so merging this.