[Discussion] Allow Multi-robot simulation

[fmessmer]

@ipa-srd @ipa-srd-fg

The goal is to prepare the cob_bringup/cob_bringup_sim structure (as well as everything that comes with it, e.g. robot_description, controller,...) to be able to use multiple robots at the same time.
This might later be extended to a multi-master setup....

Feel free to add more issues and problems faced in order to be able to solve them in accordance with single-robot usecase

@ipa-fmw @ipa-nhg @ipa-mig FYI

[fmessmer]

For now, we see several things that limit/prohibit the usage of the current structure for the multi-robot usecase, e.g.:

In robot_state_publisher the hardcoded remap tags to global / namespace should be removed in favor for including this launch file in a proper namespace. I.e.

    <include file="$(find cob_bringup)/tools/robot_state_publisher.launch">
        <arg name="robot" value="$(arg robot)" />
    </include>

from raw3-2 should be replaced by something like

    <include ns="$(arg robot_id)" "file="$(find cob_bringup)/tools/robot_state_publisher.launch">
        <arg name="robot" value="$(arg robot)" />
    </include>

[fmessmer]

The robot_namespace in the gazebo_ros_control plugins for the respective components should be replaced by something like

  <gazebo>
    <plugin name="ros_control" filename="libhwi_switch_gazebo_ros_control.so">
      <robotNamespace>$(arg robot_id)/base</robotNamespace>
      <filterJointsParam>joint_names</filterJointsParam>
    </plugin>
  </gazebo>

where $(arg robot_id) is a launch file argument that is passed to the urdf during the upload of the robot_description done in upload_robot.
Thus upload_robot should be able to take launch argument

  <arg name="robot_id" default=""/>

(default value is ""!)
which is passed to the urdf like this:

    <param name="robot_description" command="$(find xacro)/xacro.py '$(arg pkg_hardware_config)/$(arg robot)/urdf/$(arg robot).urdf.xacro' robot_id:=$(arg robot_id)" />

[fmessmer]

We need to find a good solution for un-global-namespacing the source_list parameter of the joint_state_publisher

According to this lines, the Subscribers of the joint_state_publisher can also better be namespaced via the launch file than using a global / in the yaml. Thus, I think the / simply can be removed from the yaml and - as described in 1. - using the ns it will be working for the multi-robot case as well.

[ipa-srd-fg]

1. In order to separate multiple robots in the future, I wrapped <group ns="$(arg robot_id)"> around the launch file of the single robot. Therefore we do not have to add an ns tag to every new node. I hope that this structure is not in conflict with the single-robot usecase.
2. I had some errors by launching the files with no argument, therefore we should set the default value to "/" instead of the empty string. Furthermore, I would suggest to add <xacro:arg name="robot_id" default="/"/> to the urdf.
3. By deleting the leading slash, i.e. for the raw3-2 [base/joint_states], makes the namespace relative, at least for the tested raw3-2. By applying the previous changes, it is possible to spawn two robots in Gazebo and control them independently by teleop.
4. Similar to 2., in Launch Laser we should pass through the robot_id in order to add /$(arg robot_id) to both remap targets.
5. By starting the local_costmap_node in Base Collision Observer, the warning
   MessageFilter [target=base_link base_laser_front_link ]: Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.
   and the same for base_laser_front_link pops up regularly. So it seems that the base_link is somehow not able to communicate with those two.

[fmessmer]

I assume you are mainly working within cob_robots, right?
Can you post a link to your development branch? For review....and further discussion...

[fmessmer]

The development branches are:
cob_robots: https://github.com/ipa-srd-fg/cob_robots
cob_simulation: https://github.com/ipa-srd-fg/cob_simulation

The diffs are here:
cob_robots: ipa320/cob_robots@indigo_dev...ipa-srd-fg:indigo_dev
cob_simulation: ipa320/cob_simulation@indigo_dev...ipa-srd-fg:indigo_dev

[fmessmer]

As discussed with @ipa-mig @ipa-srd and @ipa-srd-fg we might need to re-think about the topic names from the laser scanner (might need to happen for other sensors too?)
Other than that the introduction of the 'robot_id' argument seems to work fine.

Still a problem is within costmap node.
Also a "common root" needs to be introduced for the tf trees for the various robots when not using localization.

After current experiments with raw3-2 are satisfactory, changes need to be applied for all robots, too!

[ipa-srd-fg]

Since I am not very familiar with git and have been advised to use feature/... for the naming of the git branches, I adjusted the development branches to the usual naming: cob_common, cob_control, cob_robots, cob_simulation

Currently, I try to apply the changes for all robots. Unfortunately, when there is also a torso in addition to the base (e.g. raw3-1), the related joint_trajectory_controller is unable to find the robot_description on the parameter server. In my opinion, this is a problem of the ros_controllers package (see proposed diffs ros_controllers). In this example the NodeHandle is in the namespace /robot_1/torso and the requested robot_description is in /robot_1/robot_description. Therefore it is not possible to get the robot_description. Eventually, you could confirm this.

There is also a problem with prosilica. By deleting the leading slashes of the topic names, all topic names are then changed by considering the cameraName, i.e. e.g. /stereo/right/image_raw -> /robot_1/head_color_camera_r_sensor/stereo/right/image_raw. I see two solutions:

1. passing the robot_id to the file
2. just deleting the slashes. Since I have no idea, similar to the renaming of the laser scanner, which packages use the information of this camera, it is necessary to adjust those packages, too.

[fmessmer]

That's perfect! It's good to have a meaningful and consistent feature branch name in the related repos.

As to the robot_description param and the ros_controller: I see your changes a bit sceptical as the the joint_trajectory_controller is widely used in the community and I'd rather not touch it. Maybe it then makes sense to actually have those namespaced robot_description parameters rather than just one global /robot_description. Think about simulating raw3-2 and cob4-2 simultaneously! The robot_descriptions are different then...

As to the topic names of the other sensors: We carefully have to look what the names are in the real sensor drivers and then find a consistent solution.

We could discuss these issues next week on Monday...?

[mgruhler]

+1 to having namespaces robot_desctiptions. This would make more sense. However, will we run into problems with e.g. the robot_state_publisher? Or can this be put in a namespace easily?

[ipa-srd-fg]

Sorry for not expressing my ideas in more detail concerning the joint_trajectory_controller. As far as I have understood it correctly, there is no problem when one spawns multiple robots, which all use the same robot_description on the global path /robot_description, since there is a check for robot_description in the root in the code. But when we have namespaced robot_descriptions, e.g. /robot_1/robot_description and /robot_2/robot_description the problem arises. Since the nodehandle nh is in the namespace /robot_1|2/torso it does not find the corresponding parameter, because nh.getParam does not search upwards. Additionally, when one tries to find it in the root, it fails, since it only searches for it in / and does not go down the parameter tree. Even if it goes down, there would be a conflict since there are two robot_description on the parameter server. So one have to use nh.searchParam to find the correct descriptions. Does that make sense?

For discussing the topic issues next week on Monday is suitable for me.

The robot_state_publisher works fine so far with multiple robot_descriptions.

[fmessmer]

@ipa-srd-fg:
If we discuss this on Monday, could you maybe investigate "multi-master" a bit in order for us to discuss whether and how this could be of help for us?
Some links:

• http://wiki.ros.org/sig/Multimaster
• http://wiki.ros.org/multimaster_fkie

@ipa-srd @ipa-mig will you join this meeting?

[mgruhler]

I'd like to. But cannot say yet due to demo preperations

[stefandoerr]

Although I won’t have much time today I would join the discussion at least for the main topics.

Von: Matthias Gruhler [mailto:notifications@github.com]
Gesendet: Freitag, 18. September 2015 13:34
An: ipa320/care-o-bot
Cc: Dörr, Stefan
Betreff: Re: [care-o-bot] [Discussion] Allow Multi-robot simulation (#11)

I'd like to. But cannot say yet due to demo preperations

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/11#issuecomment-141424007.

[ipa-srd-fg]

Previous discussion on robot_description namespace:
ros-controls/ros_controllers#19

[ipa-srd-fg]

As discussed with @ipa-fxm, the work (that we are currently aware of) that still needs to be done in order to allow multi-robot simulation for all robots can be summarized as follows:

• consistent renaming of sensor topics -> no global paths
• resolve tf_prefix for lookupTransform and transformPose (e.g. "base_link" -> tf_listener_.resolve("base_link")
• adjust namespace in command_gui (e.g. warning: parameter /script_server/arm_left/joint_names does not exist on ROS Parameter Server, aborting...)

[mgruhler]

@ipa-fxm, @ipa-srd-fg Just stumbled upon this migration from tf to tf2 which states that tf_prefix will not be supported within tf2.

To quote:

tf2 does not support tf_prefix.
To avoid confusion tf2 asserts that all frame_ids do not start with /.
To make this work tf::resolve will still work to join a tf_prefix and a frame_id,
but it will no longer support escaping a frame_id which starts with '/'.

I'm not sure whether this actually is a problem or not, but may be worth looking into...

[fmessmer]

First PRs have been sent:
However, they should only be merged with all the other multi-robot-related PRs...and after more testing!

• https://github.com/ipa320/cob_common/pull/168
• https://github.com/ipa320/cob_control/pull/55
• [WIP] Allow multi-robot simulation cob_robots#331
• [WIP] Allow multi-robot simulation cob_simulation#92
• https://github.com/ipa320/ipa_navigation_localization/pull/99
• https://github.com/ipa320/ipa_navigation/pull/80

[mgruhler]

True! Other repos that definitely need to be considered:

• ipa320/ipa_navigation_planning (best talk to @ipa-flg about this one)
• ipa320/ipa_navigation_perception
• ipa320/cob_navigation (as @ipa-fxm already noted)

[fmessmer]

The problem with searchParam("robot_description") also occurs when using the rqt_joint_trajectory_controller (see here)

And I think we will find it at some more places...

[floweisshardt]

this is a rqt graph from the current cob4-2 setup. Can be loaded in rqt again (from file). https://gist.github.com/ipa-fmw/323da2de461da730999f

[fmessmer]

With the result from the discussion during the ROSCon 2015 Birds-of-a-Feather session the following is agreed:

• multi-robot support by adding (robot_id) namespacing support and tf_prefix'ing is almost unachievable due to problems faced regarding tf::resolve and robot_description
• it is also the conclusion and the experience from other ROS users that have tried it (e.g. Clearpath)

Therefore, a better approach would be:

• keep each robot in a single-robot setup, i.e. the robot itself is not aware of other robots around
• implement additional filtering/processing/managing nodes that deal with the data exchange and inter-robot communication

First trials with multi-master implementations seem promising and support this approach.
Thus, this needs to be investigated further.

Regarding the current set of PullRequests:
We should review those and only apply the changes that make sense for a single-robot setup (e.g. cob_gazebo_worlds/world.launch, removal of hardcoded global namespacing, laser scanner topic naming,...), but remove everything related to robot_id and tf_prefix and tf::resolve.

[fmessmer]

I created a new public repository for temporary experiments regarding multi-robot/mutli-master support: https://github.com/ipa320/cob_multi_robots
@ipa-srd @ipa-srd-fg

[ipa-srd-fg]

Thanks

[fmessmer]

Just for the record: a similar discussion about Multi-Robot namespacing and tf_prefix'ing in gazebo ros-simulation/gazebo_ros_pkgs#333