ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

UWSim multi robots with sensors

asked 2015-03-22 12:31:57 -0500

raluse gravatar image

Hi,

I am having issues setting up a scene with 2 robots, each equipped with an IMU. My scene XML looks like:

...
<vehicle>
    <name>AZOR</name>
    <file>data/scenes/g500ARM5.urdf</file>
    <position>
      <x> 5</x>
      <y> 7 </y>
      <z> -3 </z>
    </position>
    <orientation>
      <r>0</r>
      <p>0 </p>
      <y>0 </y>
    </orientation>
    <imu>
      <name>IMU_AZOR</name>
      <relativeTo>base_link</relativeTo>
      <position>
        <x>0</x>
        <y>0</y>
        <z>0</z>
      </position>  
      <orientation>
        <r>0</r>
        <p>0</p>
        <y>0</y>
      </orientation>
      <std>0.00000001</std>
    </imu>
  </vehicle>

  <vehicle>
    <name>TOM</name>
    <file>data/scenes/g500ARM5.urdf</file>
    <position>
      <x> 10</x>
      <y> 8 </y>
      <z> -3 </z>
    </position>
    <orientation>
      <r>0</r>
      <p>0 </p>
      <y>0 </y>
    </orientation>
    <imu>
      <name>IMU_TOM</name>
      <relativeTo>base_link</relativeTo>
      <position>
        <x>0</x>
        <y>0</y>
        <z>0</z>
      </position>  
      <orientation>
        <r>0</r>
        <p>0</p>
        <y>0</y>
      </orientation>
      <std>0.00000001</std>
    </imu>
  </vehicle>

  <rosInterfaces>
    <ROSOdomToPAT>
      <topic> /dataNavigator_AZOR </topic>
      <vehicleName> AZOR </vehicleName>
    </ROSOdomToPAT>
    <ROSOdomToPAT>
      <topic> /dataNavigator_TOM </topic>
      <vehicleName> TOM </vehicleName>
    </ROSOdomToPAT>
    <PATToROSOdom>
      <topic> /uwsim/odom_AZOR </topic>
      <vehicleName> AZOR </vehicleName>
    </PATToROSOdom>
    <PATToROSOdom>
      <topic> /uwsim/odom_TOM </topic>
      <vehicleName> TOM </vehicleName>
    </PATToROSOdom>
    <WorldToROSTF>
      <rootName> world </rootName>
      <enableObjects> 0 </enableObjects>
      <rate>10</rate>
    </WorldToROSTF>
    <ImuToROSImu>
      <name>IMU_AZOR</name>
      <topic>g500/IMU_AZOR</topic>
      <rate>20</rate>
    </ImuToROSImu>
    <ImuToROSImu>
      <name>IMU_TOM</name>
      <topic>g500/IMU_TOM</topic>
      <rate>20</rate>
    </ImuToROSImu>
  </rosInterfaces>

When I try to run this scene, UWSim gives me Segmentation Fault. However, If I remove the second IMU sensor, the simulation is working OK. How can I have two vehicles with 2 IMU sensors? Thanks

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-03-26 05:34:27 -0500

raluse gravatar image

The problem comes from the WorldToROSTF element:

<WorldToROSTF>
  <rootName> world </rootName>
  <enableObjects> 0 </enableObjects>
  <rate>10</rate>
</WorldToROSTF>

After this is removed, the simulation works fine.

edit flag offensive delete link more

Comments

True, there was a bug on TF publishing for some sensors. I've just updated git main branch, thanks for the catch and sorry for the late response!.

Javier Perez gravatar image Javier Perez  ( 2015-03-31 09:58:31 -0500 )edit

Do you know if your changes have made it to version 1.4? Im sitting here with the exact same issue.

ROS: Jade UWSim 1.4 Ubuntu 14.04.4

Ragnar Moberg gravatar image Ragnar Moberg  ( 2016-02-29 06:46:33 -0500 )edit

Actually, it was fixed, but at some point reintroduced, Should be fixed again for indigo source, I'll merge changes to jade source right now.

Javier Perez gravatar image Javier Perez  ( 2016-03-01 04:51:43 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2015-03-22 12:31:57 -0500

Seen: 333 times

Last updated: Mar 26 '15