How to use GetModelState service from Gazebo in python

edit

Hi @FirmYn ,

as you said,

In documentation GetModelState needs two arguments : model_name and relative_entity_name

the model_name is the name of a model that exists on the scene and the relative_entity_name is a link that is part of the model.

If in your scene you have a model named mobile_base with a link named wheel_left_link and another model named brick_box_3x1x3 with a link named chassis, for example, the following code would show you the right position and orientation:

#! /usr/bin/env python

from gazebo_msgs.srv import GetModelState
import rospy

class Block:
    def __init__(self, name, relative_entity_name):
        self._name = name
        self._relative_entity_name = relative_entity_name

class Tutorial:

    _blockListDict = {
        'block_a': Block('mobile_base', 'wheel_left_link'),
        'block_b': Block('brick_box_3x1x3', 'chassis'),

    }

    def show_gazebo_models(self):
        try:
            model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
            for block in self._blockListDict.itervalues():
                blockName = str(block._name)
                resp_coordinates = model_coordinates(blockName, block._relative_entity_name)
                print '\n'
                print 'Status.success = ', resp_coordinates.success
                print(blockName)
                print("Cube " + str(block._name))
                print("Valeur de X : " + str(resp_coordinates.pose.position.x))
                print("Quaternion X : " + str(resp_coordinates.pose.orientation.x))

        except rospy.ServiceException as e:
            rospy.loginfo("Get Model State service call failed:  {0}".format(e))


if __name__ == '__main__':
    tuto = Tutorial()
    tuto.show_gazebo_models()

Your code is not working because you are trying to get the model state of a link named model of the objects block_a and block_b.

You have to make sure on your scene there are objects with the name you are trying to retrieve (block_a and block_b), and make sure the model has a link with the name you are passing as second argument to the model_coordinates function (in the example code).

To help you on this, you can call the following three services:

1. /gazebo/get_world_properties - to get the name of the objects on the scene

2. /gazebo/get_model_properties to see the name of the links (body_names) of a specific model on the scene

3. /gazebo/get_model_state to do what you asked on this question.

If you still didn't understand very well, I have created a video ( https://youtu.be/WqK2IY5_9OQ ) that will help you understand it.

Hope it helps.