Using modelstates and twist to move model
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