ros2 / ros1_bridge

ROS 2 package that provides bidirectional communication between ROS 1 and ROS 2
Apache License 2.0
452 stars 288 forks source link

Parameter bridge requires the same package name for ros mapping rules to work #327

Open adrianbattiston opened 3 years ago

adrianbattiston commented 3 years ago

Bug report

Required Info:

Steps to reproduce issue

When creating a custom msg type with a template mapping rule, the parameter bridge (as opposed to the dynamic bridge) only works if the package name in ROS2 is the same as in ROS1. This behaviour doesn't occur for the dynamic bridge.

The specific steps to reproduce the issue below map a move base topic that already exists in ROS1 but not in ROS2. It uses the husky sim, (Melodic, install instructions here), and ROS 2 Foxy. For the CMakeLists.txt and package.xml below, only the lines that are changed are shown.

Folder structure for custom message

robot_msgs
|-->msgs
|     |-->MoveBaseActionResult.msg
|-->CMakeLists.txt
|-->mapping_rules.yaml
|-->package.xml

MoveBaseActionResult.msg

std_msgs/Header header

actionlib_msgs/GoalStatus status

mapping_rules.yaml

-
  ros1_package_name: 'move_base_msgs'
  ros1_message_name: 'MoveBaseActionResult'
  ros2_package_name: 'robot_msgs'
  ros2_message_name: 'MoveBaseActionResult'

CMakeLists.txt

...
project(
  robot_msgs
  VERSION 0.1
  DESCRIPTION ""
)
...
...
find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(actionlib_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MoveBaseActionResult.msg"
  DEPENDENCIES builtin_interfaces std_msgs actionlib_msgs
)

install(
  FILES mapping_rules.yaml
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

package.xml

...
 <name>robot_msgs</name>
...
...
  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>builtin_interfaces</depend>
  <depend>std_msgs</depend>
  <depend>actionlib_msgs</depend>

  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
    <ros1_bridge mapping_rules="mapping_rules.yaml"/>
  </export>
</package>

Build the robot_msgs package and ros1_bridge as per https://github.com/ros2/ros1_bridge/blob/master/doc/index.rst.

Then in 2 separate terminals launch the husky sim with move_base:

roslaunch husky_gazebo husky_empty_world.launch
roslaunch husky_navigation move_base_mapless_demo.launch 

In another terminal set the parameters for the parameter server:

rosparam set /topics "[{topic: '/move_base/result', type: 'move_base_msgs/msg/MoveBaseActionResult'}, 
                       {topic: '/imu/data', type: 'sensor_msgs/msg/Imu'}]"

Launch the parameter bridge in ros2

ros2 run ros1_bridge parameter_bridge 

The custom topic fails, but the IMU topic works, giving the following error (note that the error shows the ROS 2 type as 'move_base_msgs/msg/MoveBaseActionResult' - when it should be robot_msgs/msg/MoveBaseActionResult):

Trying to create bidirectional bridge for topic '/move_base/result' with ROS 2 type 'move_base_msgs/msg/MoveBaseActionResult'
[INFO] [1630381343.254272251] [ros_bridge]: create bidirectional bridge for topic /move_base/result
failed to create bidirectional bridge for topic '/move_base/result' with ROS 2 type 'move_base_msgs/msg/MoveBaseActionResult': No template specialization for the pair
Trying to create bidirectional bridge for topic '/imu/data' with ROS 2 type 'sensor_msgs/msg/Imu'
[INFO] [1630381343.254968007] [ros_bridge]: create bidirectional bridge for topic /imu/data

However, running the dynamic bridge succeeds at bridging the custom topic (using the correct ROS 2 type 'robot_msgs/msg/MoveBaseActionResult' shown below):

ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
...
created 1to2 bridge for topic '/move_base/local_costmap/footprint' with ROS 1 type 'geometry_msgs/PolygonStamped' and ROS 2 type 'geometry_msgs/msg/PolygonStamped'
created 1to2 bridge for topic '/move_base/result' with ROS 1 type 'move_base_msgs/MoveBaseActionResult' and ROS 2 type 'robot_msgs/msg/MoveBaseActionResult'
created 1to2 bridge for topic '/move_base/status' with ROS 1 type 'actionlib_msgs/GoalStatusArray' and ROS 2 type 'actionlib_msgs/msg/GoalStatusArray'
...

As a further test, the parameter bridge also works if you completely change the message package so it has the same name as the ROS1 package (aka, rename everything robot_msgs to move_base_msgs)

Expected behavior

I think you should be able to name ros 1 and 2 packages differently for the parameter bridge.

Actual behavior

parameter bridge only maps custom topics if their packages are named the same thing.

Additional information

Let me know if anything's missing or there'd have been a better way to present the bug above!