Ask Your Question
1

How do I publish gazebo position of a robot model on odometry topic.

asked 2015-12-06 01:21:42 -0600

cybodroid gravatar image

updated 2018-06-11 17:25:35 -0600

Hello, I have a robot model that runs using teleop key, and I want to publish its position and velocity on odometry topic. How should I achieve this?

Thanks.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
5

answered 2017-10-09 03:56:25 -0600

You can use the gazebo_ros_p3d plugin in your robot's URDF to directly publish (ground truth) odometry for your robot. Here's a usage example: tracker_chassis.gazebo.xacro.xml

For your use case (publishing directly to odom) it would probably look like this:

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
  <alwaysOn>true</alwaysOn>
  <updateRate>50.0</updateRate>
  <bodyName>base_link</bodyName>
  <topicName>odom</topicName>
  <gaussianNoise>0.01</gaussianNoise>
  <frameName>world</frameName>
  <xyzOffsets>0 0 0</xyzOffsets>
  <rpyOffsets>0 0 0</rpyOffsets>
</plugin>
edit flag offensive delete link more
1

answered 2017-07-01 20:16:05 -0600

R. Tellez gravatar image

updated 2017-10-09 03:29:01 -0600

You can do that by doing two options:

  1. You compute the odometry based on the movement of the robot and odometry equations
  2. You get the odometry directly asking Gazebo how much your robot moved

Once you have that data, you create a topic with Odometry msg type and publish the data obtained from the call.

Since you are not providing any information about your robot, we cannot explain here option 1. Let's explain option 2

In order to get how much the robot moved you should call the /gazebo/get_model_state service provided by Gazebo. That service will provide you with the current position of the robot in the world. In order to call it, do the following:

rosservice call /gazebo/get_model_state "model_name: 'the_name_of_your_robot' relative_entity_name: ' ' "

Just calling that service you will get the current position of the robot in Gazebo. However, you must do that call inside a node to get the position data and publish it later in an odometry topic. You can do all that with the following code:

#! /usr/bin/env python
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from gazebo_msgs.srv import GetModelState, GetModelStateRequest

rospy.init_node('odom_pub')

odom_pub=rospy.Publisher ('/my_odom', odometry)

rospy.wait_for_service ('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelstate)

odom=Odometry()
header = Header()
header.frame_id='/odom'

model = GetModelStateRequest()
model.model_name='the_name_of_your_robot'

r = rospy.Rate(2)

while not rospy.is_shutdown():
    result = get_model_srv(model)

    odom.pose.pose = result.pose
    odom.twist.twist = result.twist

    header.stamp = rospy.Time.now()
    odom.header = header

    odom_pub.publish (odom)

    r.sleep()

That's it! That Python code will do exactly that: get the robot position from the simulator, and then publish it in an odometry topic.

Here you can find a video ( https://youtu.be/I_5leJK8vhQ ) that shows how to do all the explained above with a full example for a kobuki robot.

edit flag offensive delete link more

Comments

1

This answer would be greatly improved by adding the instructions given in the video. Links can and do change/go down, and if this were to happen then this accepted answer would no longer useful.

jayess gravatar imagejayess ( 2017-07-26 00:13:26 -0600 )edit

Answer edited and corrected as requested. Cheers

R. Tellez gravatar imageR. Tellez ( 2017-10-09 03:30:10 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2015-12-06 01:21:42 -0600

Seen: 4,897 times

Last updated: Jun 11 '18