Gazebo | Ignition | Community
Ask Your Question
0

How to ECHO an specific field of the link_states topic?

asked 2015-08-31 12:18:20 -0500

ROS_GAZEBO_USER gravatar image

How can I echo the z position of the labrob::base_footprint link? I would like something like

/gazebo/link_states/pose/labrob::base_footprint/position/x

(but of course this doesn't work)

Here it's explained the LinkStates Message

If I run

rostopic echo /gazebo/link_states/pose

I obtain

name: ['ground_plane::link', 'labrob::base_footprint', 'labrob::back_left_wheel', 'labrob::back_right_wheel', 'labrob::front_left_wheel', 'labrob::front_right_wheel', 'labrob::medium_left_wheel', 'labrob::medium_right_wheel']
pose: 
  - 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  - 
    position: 
      x: 19.734147474
      y: 25.393300817
      z: 0.0383110518968
    orientation: 
      x: -6.13606284887e-05
      y: 6.86919744421e-05
      z: 0.999860588389
      w: -0.016697164527
  - 
    position: 
      x: 19.5695521394
      y: 25.527891231
      z: 0.1182724458
    orientation: 
      x: -0.637152382838
      y: -0.0105863910799
      z: 0.770556968083
      w: -0.0129123315291
  - 
    position: 
      x: 19.578901119
      y: 25.2480472907
      z: 0.118368859468
    orientation: 
      x: 0.216469163719
      y: 0.00354699809064
      z: -0.97614666114
      w: 0.0163161242046
  - 
    position: 
      x: 19.8893736569
      y: 25.5385759122
      z: 0.118310784466
    orientation: 
      x: -0.644692265729
      y: -0.0107127808459
      z: 0.764259826565
      w: -0.0128076670502
  - 
    position: 
      x: 19.8987227742
      y: 25.2587321974
      z: 0.11837054296
    orientation: 
      x: 0.216469164402
      y: 0.00354695690918
      z: -0.976146659959
      w: 0.0163161947405
  - 
    position: 
      x: 19.7278268087
      y: 25.5822062485
      z: 0.118371118222
    orientation: 
      x: 0.216469161475
      y: 0.00354713272186
      z: -0.976146662273
      w: 0.0163160568747
  - 
    position: 
      x: 19.7404481233
      y: 25.2044170258
      z: 0.118336182098
    orientation: 
      x: 0.2172151475
      y: 0.00369544833996
      z: 0.975980931984
      w: -0.0162832356155
twist: 
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 3.29022729574e-06
      y: -5.47510238611e-06
      z: 0.00200377570936
    angular: 
      x: 0.000130190813887
      y: -0.000169448396171
      z: -9.94671504206e-06
  - 
    linear: 
      x: -7.98659884176e-06
      y: -1.47794147006e-05
      z: 0.00200227739128
    angular: 
      x: 0.000127384257108
      y: -0.000166086758399
      z: -9.45221421533e-06
  - 
    linear: 
      x: -3.53599479883e-06
      y: -1.81146807339e-05
      z: 0.00914549857804
    angular: 
      x: 0.000158572383326
      y: -3.16478255242e-05
      z: -2.43400333972e-05
  - 
    linear: 
      x: -1.21134287765e-05
      y: -1.92963202921e-05
      z: 0.00202762801668
    angular: 
      x: 0.000142529614106
      y: -0.000261624018879
      z: -9.62157685716e-06
  - 
    linear: 
      x: -2.7242166671e-05
      y: -1.62340011632e-05
      z: 0.000446377301251
    angular: 
      x: 0.000138476462121
      y: -0.00023012316061
      z: -6.70005553614e-06
  - 
    linear: 
      x: -1.05123889964e-05
      y: -1.67367147569e-05
      z: 0.0018404737071
    angular: 
      x: 0.000145585572561
      y: -8.86791770795e-05
      z: -2.01875535345e-05
  - 
    linear: 
      x: -1.15325504036e-05
      y: -1.54345872713e-05
      z: 0.0021200843129
    angular: 
      x: 0.000136389414048
      y: -0.000182992891938
      z: -8.37950589888e-06

Thanks!!!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-09-01 11:04:22 -0500

ROS_GAZEBO_USER gravatar image

The best way to get the position and more info of a link is using this pluin in your urdf:

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
  <alwaysOn>true</alwaysOn>
  <updateRate>100.0</updateRate>
  <bodyName>base_link</bodyName>
  <topicName>base_pose_ground_truth</topicName>
  <gaussianNoise>0</gaussianNoise>
  <frameName>/map</frameName>
  <xyzOffsets>0 0 0</xyzOffsets>
  <rpyOffsets>0 0 0</rpyOffsets>
</plugin>

image description

(it also works with rostopic echo..... )

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

Stats

Asked: 2015-08-31 12:18:20 -0500

Seen: 2,082 times

Last updated: Sep 01 '15