ros2 / ros1_bridge

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

Fields selection #354

Closed amifsud closed 2 years ago

amifsud commented 2 years ago

Bug report

Required Info:

Steps to reproduce issue

Trying to map the following messages in ros1:

Example.msg :

float64 a
float64 b
float64 c
float64 d

Example1.msg :

float64 a
geometry_msgs/Vector3 v

to those ones in ros2:

Example2.msg :

float64 w
float64 x
float64 y
float64 z

Example3.msg :

float64 w
geometry_msgs/Vector3 u

With the mapping rules :

-
    ros1_package_name: 'ros1_msgs'
    ros1_message_name: 'Example1'
    ros2_package_name: 'ros2_msgs'
    ros2_message_name: 'Example2'
        fields_1_to_2:
            a:   'w'
            v.x: 'x'
            v.y: 'y'
            v.z: 'z'

Or the following ones :

-
    ros1_package_name: 'ros1_msgs'
    ros1_message_name: 'Example'
    ros2_package_name: 'ros2_msgs'
    ros2_message_name: 'Example3'
        fields_1_to_2:
            a: 'w'
            b: 'u.x'
            c: 'u.y'
            d: 'u.z'

In ros1 workspace (~/ros1_ws):

source /opt/ros/noetic/setup.bash
catkin_make install

source ~/ros1_ws/install/setup.bash
rosmsg show ros1_msgs/Example
rosmsg show ros1_msgs/Example1

In ros2 workspace (~/ros2_ws):

source /opt/ros/foxy/setup.bash
colcon build --build-base foxy_build --install-base foxy_install

source ~/ros2_ws/foxy_install/setup.bash
ros2 interface show ros2_msgs/msg/Example2
ros2 interface show ros2_msgs/msg/Example3

In ros1_bridge workspace (~/bridge_ws):

source ~/ros1_ws/install/setup.bash
source ~/ros2_ws/foxy_install/setup.bash
colcon build --build-base foxy_build --install-base foxy_install --cmake-force-configure

source ~/bridge_ws/foxy_install/setup.bash
ros2 run ros1_bridge dynamic_bridge --print-pairs | grep Example

Expected behavior

In ros1 workspace (~/ros1_ws):

source /opt/ros/noetic/setup.bash
catkin_make install

source ~/ros1_ws/install/setup.bash
rosmsg show ros1_msgs/Example

Result :

float64 a
float64 b
float64 c
float64 d
rosmsg show ros1_msgs/Example1

Result :

float64 a
geometry_msgs/Vector3 v

In ros2 workspace (~/ros2_ws):

source /opt/ros/foxy/setup.bash
colcon build --build-base foxy_build --install-base foxy_install

source ~/ros2_ws/foxy_install/setup.bash
ros2 interface show ros2_msgs/msg/Example2

Result :

float64 w
float64 x
float64 y
float64 z
ros2 interface show ros2_msgs/msg/Example3

Result :

float64 w
geometry_msgs/Vector3 u

In ros1_bridge workspace (~/bridge_ws):

source ~/ros1_ws/install/setup.bash
source ~/ros2_ws/foxy_install/setup.bash
colcon build --build-base foxy_build --install-base foxy_install --cmake-force-configure

Result : No error or warning for both mapping rules

source ~/bridge_ws/foxy_install/setup.bash
ros2 run ros1_bridge dynamic_bridge --print-pairs | grep Example

Result : for both mapping rules :

- 'icars2_msgs/msg/Example2' (ROS 2) <=> 'icars_msgs/Example1' (ROS 1)

Actual behavior

Expected behavior until in ros1_bridge workspace.

In ros1_bridge workspace (~/bridge_ws):

source ~/ros1_ws/install/setup.bash
source ~/ros2_ws/foxy_install/setup.bash
colcon build --build-base foxy_build --install-base foxy_install --cmake-force-configure

Result: Expected behavior for the first mapping rules. For the second mapping rule (field selection to the right)

Starting >>> ros1_bridge
--- stderr: ros1_bridge                              
Traceback (most recent call last):
  File "bin/ros1_bridge_generate_factories", line 39, in <module>
    sys.exit(main())
  File "bin/ros1_bridge_generate_factories", line 29, in main
    return generate_cpp(
  File "~/bridge_ws/src/ros1_bridge/ros1_bridge/__init__.py", line 70, in generate_cpp
    data = generate_messages(rospack)
  File "~/bridge_ws/src/ros1_bridge/ros1_bridge/__init__.py", line 187, in generate_messages
    mapping = determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx)
  File "~/bridge_ws/src/ros1_bridge/ros1_bridge/__init__.py", line 747, in determine_field_mapping
    get_ros2_selected_fields(ros2_field_selection, ros2_spec, msg_idx)
  File "~/bridge_ws/src/ros1_bridge/ros1_bridge/__init__.py", line 694, in get_ros2_selected_fields
    parent_ros2_spec = load_ros2_message(msg_idx.ros2_get_from_field(current_field))
  File "~/bridge_ws/src/ros1_bridge/ros1_bridge/__init__.py", line 966, in ros2_get_from_field
    return self._ros2_idx[(field.type.pkg_name, field.type.type)]
AttributeError: 'NamespacedType' object has no attribute 'pkg_name'
make[2]: *** [CMakeFiles/ros1_bridge.dir/build.make:68: generated/get_factory.cpp] Error 1
make[1]: *** [CMakeFiles/Makefile2:259: CMakeFiles/ros1_bridge.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< ros1_bridge [19.9s, exited with code 2]

Summary: 0 packages finished [20.0s]
  1 package failed: ros1_bridge
  1 package had stderr output: ros1_bridge
source ~/bridge_ws/foxy_install/setup.bash
ros2 run ros1_bridge dynamic_bridge --print-pairs | grep Example

Result: Expected behavior for the first mapping rules. For the second :

Package 'ros1_bridge' not found

Additional information

I tried to use field_2_to_1 to take field selection always to the left, but I got another problem (see this issue).

gbiggs commented 2 years ago

Can you please provide a copy of your source to reproduce the problem?

amifsud commented 2 years ago

Here is a repo with the different workspaces (ros1_bridge as submodule)

Thanks for your reply Alexis

gbiggs commented 2 years ago

@amifsud I've managed to reproduce your problem, but only under certain conditions.

If I run the commands you have given in sequence in a single terminal, I can reproduce your behaviour. However, if I run them in three separate terminals, one for each of the three workspaces, then the bridge builds correctly and behaves correctly.

Please try running the commands in three terminals like this:

In terminal 1:

$ source /opt/ros/noetic/setup.bash
$ catkin_make install
$ source ~/ros1_ws/install/setup.bash
$ rosmsg show ros1_msgs/Example
$ rosmsg show ros1_msgs/Example1

In terminal 2:

$ source /opt/ros/foxy/setup.bash
$ colcon build --build-base foxy_build --install-base foxy_install
$ source ~/ros2_ws/foxy_install/setup.bash
$ ros2 interface show ros2_msgs/msg/Example2
$ ros2 interface show ros2_msgs/msg/Example3

In terminal 3:

$ source ~/ros1_ws/install/setup.bash
$ source ~/ros2_ws/foxy_install/setup.bash
$ colcon build --build-base foxy_build --install-base foxy_install --cmake-force-configure
$ source ~/bridge_ws/foxy_install/setup.bash
$ ros2 run ros1_bridge dynamic_bridge --print-pairs | grep Example

ROS 1 and ROS 2 workspaces are not meant to be overlayed on each other (the ros1_bridge is a special case that uses a lot of careful engineering to work), and complex overlaying often causes problems. I think that is what is causing the problem you are seeing.

amifsud commented 2 years ago

Hello,

I didn't mention in the first post that it was actually in different terminals, sorry for that. I tested again today, I got the same issue. @quarkytale : did you get the same issue as you said in #353 using also different terminals ?

Thanks, Alexis

quarkytale commented 2 years ago

I actually could reproduce even in three separate terminals. Which branch of ros1_bridge are you using @gbiggs if building from source, or just binaries? I'm using this dockerfile and building foxy branch of ros1_bridge from source.

quarkytale commented 2 years ago

@amifsud Opened a PR for this issue too. Please test it (using quarkytale/parsing_ros2_msg_fields branch) with your project if you get a chance!