search

robotology / gazebo-yarp-plugins

Plugins to interface Gazebo with YARP.
33 stars 48 forks source link

Port naming conflict while spawning multiple robots through a gazebo world file #342

Open yeshasvitirupachuri opened 6 years ago

yeshasvitirupachuri commented 6 years ago

As mentioned in this #93 while spawning multiple icub robots into gazebo gazeboYarpPluginsRobotName icubSim inside gazebo_icub_robotname.ini is commented out.

All the ports open correctly while spawning(dragging and dropping) two base icub models inside gazebo

registration name /iCub/head/command:i ip 10.255.54.112 port 10201 type tcp
registration name /iCub/head/rpc:i ip 10.255.54.112 port 10200 type tcp
registration name /iCub/head/state:o ip 10.255.54.112 port 10202 type tcp
registration name /iCub/head/stateExt:o ip 10.255.54.112 port 10203 type tcp
registration name /iCub/inertial ip 10.255.54.112 port 10004 type tcp
registration name /iCub/left_arm/analog:o ip 10.255.54.112 port 10008 type tcp
registration name /iCub/left_arm/analog:o/rpc:i ip 10.255.54.112 port 10007 type tcp
registration name /iCub/left_arm/command:i ip 10.255.54.112 port 10205 type tcp
registration name /iCub/left_arm/rpc:i ip 10.255.54.112 port 10204 type tcp
registration name /iCub/left_arm/state:o ip 10.255.54.112 port 10206 type tcp
registration name /iCub/left_arm/stateExt:o ip 10.255.54.112 port 10207 type tcp
registration name /iCub/left_foot/analog:o ip 10.255.54.112 port 10191 type tcp
registration name /iCub/left_foot/analog:o/rpc:i ip 10.255.54.112 port 10190 type tcp
registration name /iCub/left_leg/analog:o ip 10.255.54.112 port 10189 type tcp
registration name /iCub/left_leg/analog:o/rpc:i ip 10.255.54.112 port 10188 type tcp
registration name /iCub/left_leg/command:i ip 10.255.54.112 port 10213 type tcp
registration name /iCub/left_leg/rpc:i ip 10.255.54.112 port 10212 type tcp
registration name /iCub/left_leg/state:o ip 10.255.54.112 port 10214 type tcp
registration name /iCub/left_leg/stateExt:o ip 10.255.54.112 port 10215 type tcp
registration name /iCub/right_arm/analog:o ip 10.255.54.112 port 10010 type tcp
registration name /iCub/right_arm/analog:o/rpc:i ip 10.255.54.112 port 10009 type tcp
registration name /iCub/right_arm/command:i ip 10.255.54.112 port 10209 type tcp
registration name /iCub/right_arm/rpc:i ip 10.255.54.112 port 10208 type tcp
registration name /iCub/right_arm/state:o ip 10.255.54.112 port 10210 type tcp
registration name /iCub/right_arm/stateExt:o ip 10.255.54.112 port 10211 type tcp
registration name /iCub/right_foot/analog:o ip 10.255.54.112 port 10195 type tcp
registration name /iCub/right_foot/analog:o/rpc:i ip 10.255.54.112 port 10194 type tcp
registration name /iCub/right_leg/analog:o ip 10.255.54.112 port 10193 type tcp
registration name /iCub/right_leg/analog:o/rpc:i ip 10.255.54.112 port 10192 type tcp
registration name /iCub/right_leg/command:i ip 10.255.54.112 port 10217 type tcp
registration name /iCub/right_leg/rpc:i ip 10.255.54.112 port 10216 type tcp
registration name /iCub/right_leg/state:o ip 10.255.54.112 port 10218 type tcp
registration name /iCub/right_leg/stateExt:o ip 10.255.54.112 port 10219 type tcp
registration name /iCub/torso/command:i ip 10.255.54.112 port 10197 type tcp
registration name /iCub/torso/rpc:i ip 10.255.54.112 port 10196 type tcp
registration name /iCub/torso/state:o ip 10.255.54.112 port 10198 type tcp
registration name /iCub/torso/stateExt:o ip 10.255.54.112 port 10199 type tcp
registration name /iCub_0/head/command:i ip 10.255.54.112 port 10238 type tcp
registration name /iCub_0/head/rpc:i ip 10.255.54.112 port 10237 type tcp
registration name /iCub_0/head/state:o ip 10.255.54.112 port 10239 type tcp
registration name /iCub_0/head/stateExt:o ip 10.255.54.112 port 10240 type tcp
registration name /iCub_0/inertial ip 10.255.54.112 port 10220 type tcp
registration name /iCub_0/left_arm/analog:o ip 10.255.54.112 port 10230 type tcp
registration name /iCub_0/left_arm/analog:o/rpc:i ip 10.255.54.112 port 10229 type tcp
registration name /iCub_0/left_arm/command:i ip 10.255.54.112 port 10242 type tcp
registration name /iCub_0/left_arm/rpc:i ip 10.255.54.112 port 10241 type tcp
registration name /iCub_0/left_arm/state:o ip 10.255.54.112 port 10243 type tcp
registration name /iCub_0/left_arm/stateExt:o ip 10.255.54.112 port 10244 type tcp
registration name /iCub_0/left_foot/analog:o ip 10.255.54.112 port 10224 type tcp
registration name /iCub_0/left_foot/analog:o/rpc:i ip 10.255.54.112 port 10223 type tcp
registration name /iCub_0/left_leg/analog:o ip 10.255.54.112 port 10222 type tcp
registration name /iCub_0/left_leg/analog:o/rpc:i ip 10.255.54.112 port 10221 type tcp
registration name /iCub_0/left_leg/command:i ip 10.255.54.112 port 10250 type tcp
registration name /iCub_0/left_leg/rpc:i ip 10.255.54.112 port 10249 type tcp
registration name /iCub_0/left_leg/state:o ip 10.255.54.112 port 10251 type tcp
registration name /iCub_0/left_leg/stateExt:o ip 10.255.54.112 port 10252 type tcp
registration name /iCub_0/right_arm/analog:o ip 10.255.54.112 port 10232 type tcp
registration name /iCub_0/right_arm/analog:o/rpc:i ip 10.255.54.112 port 10231 type tcp
registration name /iCub_0/right_arm/command:i ip 10.255.54.112 port 10246 type tcp
registration name /iCub_0/right_arm/rpc:i ip 10.255.54.112 port 10245 type tcp
registration name /iCub_0/right_arm/state:o ip 10.255.54.112 port 10247 type tcp
registration name /iCub_0/right_arm/stateExt:o ip 10.255.54.112 port 10248 type tcp
registration name /iCub_0/right_foot/analog:o ip 10.255.54.112 port 10228 type tcp
registration name /iCub_0/right_foot/analog:o/rpc:i ip 10.255.54.112 port 10227 type tcp
registration name /iCub_0/right_leg/analog:o ip 10.255.54.112 port 10226 type tcp
registration name /iCub_0/right_leg/analog:o/rpc:i ip 10.255.54.112 port 10225 type tcp
registration name /iCub_0/right_leg/command:i ip 10.255.54.112 port 10254 type tcp
registration name /iCub_0/right_leg/rpc:i ip 10.255.54.112 port 10253 type tcp
registration name /iCub_0/right_leg/state:o ip 10.255.54.112 port 10255 type tcp
registration name /iCub_0/right_leg/stateExt:o ip 10.255.54.112 port 10256 type tcp
registration name /iCub_0/torso/command:i ip 10.255.54.112 port 10234 type tcp
registration name /iCub_0/torso/rpc:i ip 10.255.54.112 port 10233 type tcp
registration name /iCub_0/torso/state:o ip 10.255.54.112 port 10235 type tcp
registration name /iCub_0/torso/stateExt:o ip 10.255.54.112 port 10236 type tcp

But when a gazebo .world file with two icub is used to start gazebo, only the ports related to one robot are started and as the second robot also is trying to start with the same naming it fails to start due to address conflict


registration name /iCub/head/command:i ip 10.255.54.112 port 10201 type tcp
registration name /iCub/head/rpc:i ip 10.255.54.112 port 10200 type tcp
registration name /iCub/head/state:o ip 10.255.54.112 port 10202 type tcp
registration name /iCub/head/stateExt:o ip 10.255.54.112 port 10203 type tcp
registration name /iCub/inertial ip 10.255.54.112 port 10004 type tcp
registration name /iCub/left_arm/analog:o ip 10.255.54.112 port 10008 type tcp
registration name /iCub/left_arm/analog:o/rpc:i ip 10.255.54.112 port 10007 type tcp
registration name /iCub/left_arm/command:i ip 10.255.54.112 port 10205 type tcp
registration name /iCub/left_arm/rpc:i ip 10.255.54.112 port 10204 type tcp
registration name /iCub/left_arm/state:o ip 10.255.54.112 port 10206 type tcp
registration name /iCub/left_arm/stateExt:o ip 10.255.54.112 port 10207 type tcp
registration name /iCub/left_foot/analog:o ip 10.255.54.112 port 10191 type tcp
registration name /iCub/left_foot/analog:o/rpc:i ip 10.255.54.112 port 10190 type tcp
registration name /iCub/left_leg/analog:o ip 10.255.54.112 port 10189 type tcp
registration name /iCub/left_leg/analog:o/rpc:i ip 10.255.54.112 port 10188 type tcp
registration name /iCub/left_leg/command:i ip 10.255.54.112 port 10213 type tcp
registration name /iCub/left_leg/rpc:i ip 10.255.54.112 port 10212 type tcp
registration name /iCub/left_leg/state:o ip 10.255.54.112 port 10214 type tcp
registration name /iCub/left_leg/stateExt:o ip 10.255.54.112 port 10215 type tcp
registration name /iCub/right_arm/analog:o ip 10.255.54.112 port 10010 type tcp
registration name /iCub/right_arm/analog:o/rpc:i ip 10.255.54.112 port 10009 type tcp
registration name /iCub/right_arm/command:i ip 10.255.54.112 port 10209 type tcp
registration name /iCub/right_arm/rpc:i ip 10.255.54.112 port 10208 type tcp
registration name /iCub/right_arm/state:o ip 10.255.54.112 port 10210 type tcp
registration name /iCub/right_arm/stateExt:o ip 10.255.54.112 port 10211 type tcp
registration name /iCub/right_foot/analog:o ip 10.255.54.112 port 10195 type tcp
registration name /iCub/right_foot/analog:o/rpc:i ip 10.255.54.112 port 10194 type tcp
registration name /iCub/right_leg/analog:o ip 10.255.54.112 port 10193 type tcp
registration name /iCub/right_leg/analog:o/rpc:i ip 10.255.54.112 port 10192 type tcp
registration name /iCub/right_leg/command:i ip 10.255.54.112 port 10217 type tcp
registration name /iCub/right_leg/rpc:i ip 10.255.54.112 port 10216 type tcp
registration name /iCub/right_leg/state:o ip 10.255.54.112 port 10218 type tcp
registration name /iCub/right_leg/stateExt:o ip 10.255.54.112 port 10219 type tcp
registration name /iCub/torso/command:i ip 10.255.54.112 port 10197 type tcp
registration name /iCub/torso/rpc:i ip 10.255.54.112 port 10196 type tcp
registration name /iCub/torso/state:o ip 10.255.54.112 port 10198 type tcp
registration name /iCub/torso/stateExt:o ip 10.255.54.112 port 10199 type tcp

@traversaro

traversaro commented 6 years ago

This behavior is what is obtained with Gazebo 7, the behavior could be different with Gazebo 8 or 9.

yeshasvitirupachuri commented 6 years ago

I figured a way to spawn two models at desired pose in gazebo using 

gz model -m iCub -f /<icub-gazebo directory>/icub/icub.sdf --pose-z 1 --pose-Y 3.14

gz model -m iCub_0 -f /<icub-gazebo directory>/icub/icub.sdf --pose-x 0.8 --pose-z 1

The gz help model gives the following output

 gz model [options]

    Change properties of a model, delete a model, or
    spawn a new model. If a name for the world, option -w, is
    not pecified, the first world found on the Gazebo master
    will be used.

Options:
  -h [ --help ]           Print this help message
  -m [ --model-name ] arg Model name.
  -w [ --world-name ] arg World name.
  -d [ --delete ]         Delete a model.
  -f [ --spawn-file ] arg Spawn model from SDF file.
  -s [ --spawn-string ]   Spawn model from SDF string, pass by a pipe.
  -i [ --info ]           Output model state information to the terminal.
  -p [ --pose ]           Output model pose as a space separated 6-tuple: x y z
                          roll pitch yaw.
  -x [ --pose-x ] arg     x value
  -y [ --pose-y ] arg     y value
  -z [ --pose-z ] arg     z value
  -R [ --pose-R ] arg     roll in radians.
  -P [ --pose-P ] arg     pitch in radians.
  -Y [ --pose-Y ] arg     yaw in radians.
yeshasvitirupachuri commented 6 years ago

The behavior is the same with Gazebo 9.0 while launching gazebo with a world containing two icub models.

Also, spawning a model with file name -f is also not working in gazebo 9.0. So, after launching gazebo and inserting two icub models, I can arrange them using:

gz model -m iCub -x 0 -y 0 -z 0.6

gz model -m iCub_0 -x 0 -y 0 -z 0.6 -R 0 -P 0 -Y 3.14
traversaro commented 6 years ago

A possible strategy to deal with this is to find a way to explicitly pass (possibly overriding the one in the .ini) the gazeboYarpPluginsRobotName parameter.

traversaro commented 6 years ago

Anyhow, what are the names of the models when spawned with two files? If they are not distinct, I think that is a Gazebo issue that should be opened in the Gazebo bug tracker.

lrapetti commented 6 years ago

By defining the model name for each iCub model directly in the .world file, it is possible to create a world with multiple iCub models. e.g.

    <!-- iCub 0 -->
   <model name="iCub_0">
      <include>
          <uri>model://icub</uri>
      </include>
   </model>

    <!-- iCub 1 -->
   <model name="iCub_1">
      <include>
          <uri>model://icub</uri>
          <pose>0.6 0.0 0.6 0 0 0</pose>
      </include>
   </model>

In this way the ControlBoard ports are opened with the correct prefix:

$ yarp name list | grep iCub_0
registration name /iCub_0/head/command:i ip 10.255.35.123 port 10022 type tcp
registration name /iCub_0/head/rpc:i ip 10.255.35.123 port 10021 type tcp
registration name /iCub_0/head/state:o ip 10.255.35.123 port 10023 type tcp
registration name /iCub_0/head/stateExt:o ip 10.255.35.123 port 10024 type tcp
registration name /iCub_0/left_arm/command:i ip 10.255.35.123 port 10026 type tcp
registration name /iCub_0/left_arm/rpc:i ip 10.255.35.123 port 10025 type tcp
registration name /iCub_0/left_arm/state:o ip 10.255.35.123 port 10027 type tcp
registration name /iCub_0/left_arm/stateExt:o ip 10.255.35.123 port 10028 type tcp
registration name /iCub_0/left_leg/command:i ip 10.255.35.123 port 10034 type tcp
registration name /iCub_0/left_leg/rpc:i ip 10.255.35.123 port 10033 type tcp
registration name /iCub_0/left_leg/state:o ip 10.255.35.123 port 10035 type tcp
registration name /iCub_0/left_leg/stateExt:o ip 10.255.35.123 port 10036 type tcp
registration name /iCub_0/right_arm/command:i ip 10.255.35.123 port 10030 type tcp
registration name /iCub_0/right_arm/rpc:i ip 10.255.35.123 port 10029 type tcp
registration name /iCub_0/right_arm/state:o ip 10.255.35.123 port 10031 type tcp
registration name /iCub_0/right_arm/stateExt:o ip 10.255.35.123 port 10032 type tcp
registration name /iCub_0/right_leg/command:i ip 10.255.35.123 port 10038 type tcp
registration name /iCub_0/right_leg/rpc:i ip 10.255.35.123 port 10037 type tcp
registration name /iCub_0/right_leg/state:o ip 10.255.35.123 port 10039 type tcp
registration name /iCub_0/right_leg/stateExt:o ip 10.255.35.123 port 10040 type tcp
registration name /iCub_0/torso/command:i ip 10.255.35.123 port 10018 type tcp
registration name /iCub_0/torso/rpc:i ip 10.255.35.123 port 10017 type tcp
registration name /iCub_0/torso/state:o ip 10.255.35.123 port 10019 type tcp
registration name /iCub_0/torso/stateExt:o ip 10.255.35.123 port 10020 type tcp

$ yarp name list | grep iCub_1
registration name /iCub_1/head/command:i ip 10.255.35.123 port 10047 type tcp
registration name /iCub_1/head/rpc:i ip 10.255.35.123 port 10046 type tcp
registration name /iCub_1/head/state:o ip 10.255.35.123 port 10048 type tcp
registration name /iCub_1/head/stateExt:o ip 10.255.35.123 port 10049 type tcp
registration name /iCub_1/left_arm/command:i ip 10.255.35.123 port 10051 type tcp
registration name /iCub_1/left_arm/rpc:i ip 10.255.35.123 port 10050 type tcp
registration name /iCub_1/left_arm/state:o ip 10.255.35.123 port 10052 type tcp
registration name /iCub_1/left_arm/stateExt:o ip 10.255.35.123 port 10053 type tcp
registration name /iCub_1/left_leg/command:i ip 10.255.35.123 port 10059 type tcp
registration name /iCub_1/left_leg/rpc:i ip 10.255.35.123 port 10058 type tcp
registration name /iCub_1/left_leg/state:o ip 10.255.35.123 port 10060 type tcp
registration name /iCub_1/left_leg/stateExt:o ip 10.255.35.123 port 10061 type tcp
registration name /iCub_1/right_arm/command:i ip 10.255.35.123 port 10055 type tcp
registration name /iCub_1/right_arm/rpc:i ip 10.255.35.123 port 10054 type tcp
registration name /iCub_1/right_arm/state:o ip 10.255.35.123 port 10056 type tcp
registration name /iCub_1/right_arm/stateExt:o ip 10.255.35.123 port 10057 type tcp
registration name /iCub_1/right_leg/command:i ip 10.255.35.123 port 10063 type tcp
registration name /iCub_1/right_leg/rpc:i ip 10.255.35.123 port 10062 type tcp
registration name /iCub_1/right_leg/state:o ip 10.255.35.123 port 10064 type tcp
registration name /iCub_1/right_leg/stateExt:o ip 10.255.35.123 port 10065 type tcp
registration name /iCub_1/torso/command:i ip 10.255.35.123 port 10043 type tcp
registration name /iCub_1/torso/rpc:i ip 10.255.35.123 port 10042 type tcp
registration name /iCub_1/torso/state:o ip 10.255.35.123 port 10044 type tcp
registration name /iCub_1/torso/stateExt:o ip 10.255.35.123 port 10045 type tcp
lrapetti commented 6 years ago

Still it is not clear why when the same model is add mulitple times in a world, Gazebo is not using the same automatic name serialization convention (model, model_0, ...) that is used when the same model is dropped multiple times.

yeshasvitirupachuri commented 6 years ago

Also please add the names showed in gazebo for the models

lrapetti commented 6 years ago

This is what I see from the UI (iCub_0, iCub_1) two_icub_gazebo

lrapetti commented 6 years ago

I noticed that with this solution the link names have a "double prefix" due to the fact that we are including a model (with name iCub) inside a model (iCub_0 or iCub_1).

E.g.

yeshasvitirupachuri commented 6 years ago

I noticed that with this solution the link names have a "double prefix" due to the fact that we are including a model (with name iCub) inside a model (iCub_0 or iCub_1).

E.g.

  • iCub_0::iCub::l_hand
  • iCub_1::iCub::l_hand

Aaah that's a bit of a catch! :grinning: but anyway for the moment it works fine to launch two models with a world. So, now we can give less preference to the task of prefix handling and you can focus on implementing the joint positions model plugin.

As discussed f2f linkattacher plugin cannot handle the naming for the robots at the moment, but it is an easy task to extend it for this case.

traversaro commented 6 years ago

This is exactly why in gazebo_yarp_controlboard the joint names actually specify the ending of the joint scoped name, and not the joint name itself, for easily dealing with this case, see https://github.com/robotology/gazebo-yarp-plugins/issues/63 and the related commit https://github.com/robotology/gazebo-yarp-plugins/commit/193214f7863248c52a55391f3ef5a1c758a8ad2c .

traversaro commented 6 years ago

Another related issue: https://github.com/robotology/gazebo-yarp-plugins/issues/325 .

lrapetti commented 6 years ago

I have pushed an example of world with two icub in https://github.com/lrapetti/icub-gazebo-wholebody/tree/two_icubs.

It is working fine, the only thing that still is not straightforward (and that is preventing me to push to the master) is the fact that it is necessary to comment out gazeboYarpPluginsRobotName icubSim inside gazebo_icub_robotname.ini.

Another thing that I think should be defined, is the place where the .ini files of the gazebo-yarp-plugins should be stored. At the moment I have stored it under the corresponding model (models/icub_standup/conf/linkattacher.ini), but I think the correct place would be under the corresponding world (worlds/two_icubs_standup_world/conf/linkattacher.ini) since it contains the name of the model (that can be world dependent). However I am not sure on how to define yarpConfigurationFile uri starting from the world path.

lrapetti commented 6 years ago

Another thing that I think should be defined, is the place where the .ini files of the gazebo-yarp-plugins should be stored. At the moment I have stored it under the corresponding model (models/icub_standup/conf/linkattacher.ini), but I think the correct place would be under the corresponding world (worlds/two_icubs_standup_world/conf/linkattacher.ini) since it contains the name of the model (that can be world dependent). However I am not sure on how to define yarpConfigurationFile uri starting from the world path.

This problem (in this case specific of linkattacher, but that could be applied to any other plugin that defines port name in a separate .ini) could be avoided with the solution propose at https://github.com/robotology/gazebo-yarp-plugins/pull/393#issuecomment-435977500.