I am trying to run my script to send pose to my move_base from msgs. But it runs the code and waits without throwing any error. My tracker is leg running is running and providing me a People_tracked msg of
PeopleArray(Person[] people)
Person (geometry_msgs/Pose pose uint32 id).
Can't figure out the problem as there is no error.
class move_base_goal:
def __init__(self):
self.goal_pose = PersonArray()
self.new_person = Person()
self.goal_pose.people.append(self.new_person)
if not self.goal_pose.people:
return move_base_goal()
rospy.init_node('movebase_goal_py')
rospy.Subscriber("people_tracked", PersonArray, self.callback )
def callback(self,msg):
self.goal_pose.people[0].pose.position.x = msg.people[0].pose.position.x
self.goal_pose.people[0].pose.position.y = msg.people[0].pose.position.y
def movebase_goal(msg):
move = actionlib.SimpleActionClient('move_base',MoveBaseAction)
move.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = self.goal_pose.people[0].pose.x
goal.target_pose.pose.position.y = self.goal_pose.people[0].pose.y
goal.target_pose.pose.orientation.w = 1.0
move.send_goal(goal)
if __name__ == '__main__':
x = move_base_goal()
rospy.spin()
↧