Topics Crosslinked When 2 Similar Robots Viewing Each Other's /tf

[KKSTB]

I have made 2 similar robots connected to each other using multimaster. I used topic_tools relay to copy the required topics (e.g. /tf, /tf_static) to another topics (e.g. /robot1/tf, /robot2/tf). To sync these topics, I used master_sync's sync_nodes to sync the relay nodes.

Also to display the robot model, I created a service to let other robot getting the robot description so that it can set it to rosparam.

A sample launch file of the robot:

<?xml version="1.0"?>
<launch>
    <arg name="net_name" default="robot"/>
    <arg name="net_id" default="1"/>
    <node name="$(arg net_name)$(arg net_id)_tf" pkg="topic_tools" type="relay" output="screen" args="/tf /$(arg net_name)$(arg net_id)/tf"/>
    <node name="$(arg net_name)$(arg net_id)_tf_static" pkg="topic_tools" type="relay" output="screen" args="/tf_static /$(arg net_name)$(arg net_id)/tf_static"/>
    <node name="$(arg net_name)$(arg net_id)_robot_description" pkg="my_pkg" type="get_robot_description.py" output="screen" args="-ns=$(arg net_name)$(arg net_id)"/>

    <node name="master_discovery" pkg="fkie_master_discovery" type="master_discovery" output="screen"/>
    <node name="master_sync" pkg="fkie_master_sync" type="master_sync" output="screen">
        <rosparam param="sync_nodes" subst_value="True">['/$(arg net_name)*']</rosparam>
    </node>
</launch>

Then I use RViz to view the robot:
rviz /tf:=/robot2/tf /tf_static:=/robot2/tf_static /robot_description:=/robot2/robot_description

Scenario 1 (work):

1. Robot2 running with the above launch file launched
2. Robot1 not running but with the above launch file launched and used my get_robot_description.py to obtain robot2's robot description. Then use the above RViz command to view robot2's state (correct and I can see robot2's movement)

Scenario 2 (not work):

1. Robot2 running with the above launch file launched
2. Robot1 running with the above launch file launched, and used my get_robot_description.py to obtain both robot's robot description. Then use the above RViz command to view robot2's state (I see robot1's /tf is used instead, and robot2's robot_description is used, so that the visual model is wrong and robot1's movement is shown instead). Use another terminal and RViz command to view robot1's state (I see robot2's /tf is used instead, and robot1's robot_description is used, so that the visual model is wrong and robot2's movement is shown instead)

So it seems the sync topics are kind of crosslinked, while services are ok.

I also tried master_sync's ignore_nodes to ignore all the robot core's nodes but no difference. Is there something that I have missed?

[KKSTB]

I further did an interesting test. Hope this helps:

Scenario 3 (work at step 1. not work at step 2):

1. Robot1 running with the above launch file launched, and used my get_robot_description.py to obtain robot1's robot description. Then use the above RViz command to view robot1's state (correct and I can see robot1's movement)
2. Then launch robot2 with the above launch file launched. I can see RViz suddenly switches to robot2's /tf instead of robot1. Therefore the 3D visual model becomes wrong after robot2 launched.

[KKSTB]

It looks strange, but the problem can be avoided by just making the source relay topic name unique. E.g.

<?xml version="1.0"?>
<launch>
    <arg name="net_name" default="robot"/>
    <arg name="net_id" default="1"/>
    <node name="relay_tf" pkg="topic_tools" type="relay" output="screen" args="/tf /relay$(arg net_id)/tf"/>
    <node name="relay_tf_static" pkg="topic_tools" type="relay" output="screen" args="/tf_static /relay$(arg net_id)/tf_static"/>

    <node name="$(arg net_name)$(arg net_id)_tf" pkg="topic_tools" type="relay" output="screen" args="/relay$(arg net_id)/tf /$(arg net_name)$(arg net_id)/tf"/>
    <node name="$(arg net_name)$(arg net_id)_tf_static" pkg="topic_tools" type="relay" output="screen" args="/relay$(arg net_id)/tf_static /$(arg net_name)$(arg net_id)/tf_static"/>

    <node name="$(arg net_name)$(arg net_id)_robot_description" pkg="my_pkg" type="get_robot_description.py" output="screen" args="-ns=$(arg net_name)$(arg net_id)"/>

    <node name="master_discovery" pkg="fkie_master_discovery" type="master_discovery" output="screen"/>
    <node name="master_sync" pkg="fkie_master_sync" type="master_sync" output="screen">
        <rosparam param="sync_nodes" subst_value="True">['/$(arg net_name)*']</rosparam>
    </node>
</launch>

This trick doesnt work if the intermediate topics /relay$(arg net_id)/tf and /relay$(arg net_id)/tf_static are not unique across ROSMaster, e.g. /relay/tf and /relay/tf_static.

Is there any hope to allow topics of already different names to have the same source topic names?

[atiderko]

Hi, can you use sync_topics instead of sync_nodes to sync only the traget topics (e.g. /robot1/tf, /robot2/tf) and not all topics of the relay node?

[KKSTB]

You are right. That also works!

<?xml version="1.0"?>
<launch>
    <arg name="net_name" default="robot"/>
    <arg name="net_id" default="1"/>
    <node name="$(arg net_name)$(arg net_id)_tf" pkg="topic_tools" type="relay" output="screen" args="/tf /$(arg net_name)$(arg net_id)/tf"/>
    <node name="$(arg net_name)$(arg net_id)_tf_static" pkg="topic_tools" type="relay" output="screen" args="/tf_static /$(arg net_name)$(arg net_id)/tf_static"/>
    <node name="$(arg net_name)$(arg net_id)_robot_description" pkg="my_pkg" type="get_robot_description.py" output="screen" args="-ns=$(arg net_name)$(arg net_id)"/>

    <node name="master_discovery" pkg="fkie_master_discovery" type="master_discovery" output="screen"/>
    <node name="master_sync" pkg="fkie_master_sync" type="master_sync" output="screen">
        <rosparam param="ignore_nodes">['/main*', '/rviz*', '/sound*', '/rqt*']</rosparam>
        <rosparam param="sync_topics" subst_value="True">['/$(arg net_name)*']</rosparam>
    </node>
</launch>

So the cases that I have tested:

1. sync_nodes to sync only the relay nodes --> crosslink of topics happens
2. ignore_nodes to ignore all nodes except the relay nodes, but no sync_topics on the relay topics --> crosslink of topics happens
3. ignore_nodes to ignore all nodes except the relay nodes, in combination with sync_topics of the relay topics --> works and I will take this solution because of performance
4. 1st set of relay nodes to generate intermediate local topics of unique names, 2nd set of relay nodes to use the intermediate topics to generate the wanted sync topics, and sync_nodes the 2nd set relay nodes --> works
5. sync_topics of the relay topics only. No sync_nodes or ignore_nodes --> local nodes shutdown due to new node registered with same name

Btw is there any method to specifically sync only the wanted topics and services and ignore the rest by default, without having to supply a list of ignore_*? Because e.g. older robot with older ignore_* list may attempt to sync with newer robot with new nodes and crash the older robot's program. If there is no ignore_* then such compatibility problem could be addressed (of course I can always keep myself reminded to precede new nodes' name with names already in ignore_* list. but this is less easy to maintain).