Get Modelstate Information for Turlebot

edit

Hello Guys,

I run ros Kinetic with gazebo 7.0. My project is a turtlebot who should drive to several objects and try to push them away after a try he should drive to the middle of the room again and drive to the next object. I tried Odometry for get information about the position of the robot but i need accurated position so i need model state.

So i think it should have a higher accuracy when i use modelstates.

I got my odometry information with following piece of code:

def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll,pitch,theta) = euler_from_quaternion ([rot_q.x,rot_q.y,rot_q.z,rot_q.w])
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/odom",Odometry,newOdom)

My question is: It is correct to make the same with the gazebo_msgs/ModelState I updated my code to :

def newModel(msg):
    global x
    global y
    global theta

    x = msg.pose[1].pose.position.x
    y = msg.pose.pose[1].position.y

    rot_q = msg.pose[1].pose.orientation
    (roll,pitch,theta) = euler_from_quaternion ([rot_q.x,rot_q.y,rot_q.z,rot_q.w])
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/gazebo/ModelStates",ModelState,newModel)

I looked in publishing message and mobile base is at position 2 so it should be index 1. But all i got as x and y value is 0 even if i move the robot. Any further help?

With the subscriber note above i got no error but not the correct values.

I even tried out :

sub = rospy.Subscriber("/gazebo/model_states",ModelState,newModel)

If u use this i got following error:

[ERROR] [1430752035.526287969, 184.707000000]: Client [/listener] wants topic /gazebo/model_states to have datatype/md5sum [gazebo_msgs/ModelState/9330fd35f2fcd82d457e54bd54e10593], but our version has [gazebo_msgs/ModelStates/48c080191eb15c41858319b4d8a609c2]. Dropping connection.

This is also solved at Modelstate error