Using modelstates and twist to move model

edit

Hello, im trying to move my human model (person_standing) to a goal XY-coordinate. But im having problems with getting its starting position using a modelStates subscriber. My code teleports the model to (0 , 0) in XY world coordinates, so it seems that the code is not running the function newModel for some reason. Any tips?

import rospy
from gazebo_msgs.msg import ModelState, ModelStates
from math import atan2
from tf.transformations import euler_from_quaternion

##Initializing XY THETA
x = 0.0
y = 0.0
theta = 0.0

##Getting XY-coordinates for model
def newModel(msg):
    global x
    global y
    global theta
    x = msg.pose[2].position.x
    y = msg.pose[2].position.y
    rot_q = msg.pose[2].orientation
    (roll,pitch,theta) = euler_from_quaternion ([rot_q.x,rot_q.y,rot_q.z,rot_q.w])

def pose_publisher():
    sub = rospy.Subscriber("/gazebo/model_states",ModelStates,newModel)
    pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10)
    pose_msg = ModelState()
    pose_msg.model_name = 'person_standing'
    rate = rospy.Rate(5)

    ##Goal position
    goalx = 6
    goaly = 15

    while not rospy.is_shutdown():
            inc_x = goalx - x
            inc_y = goaly - y
            angle_to_goal = atan2 (inc_y,inc_x)

            ##If model not facing goal, rotate until it faces it
            if abs(angle_to_goal - theta) > 0.1:
                pose_msg.twist.linear.x = 0.0
                pose_msg.twist.angular.z = 0.3
            else: ##Go to that goal
                pose_msg.twist.linear.x = 0.5
                pose_msg.twist.angular.z = 0.0

            pub.publish(pose_msg)
            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('pose_publisher')
    try:
        pose_publisher()
    except rospy.ROSInterruptException:
        pass