navgation_test.py 文件源码

def people_msg_cb(self,msg):
        self.people_msg=msg
        if self.people_msg.pos is not None:
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = 'base_link'
            q_angle = quaternion_from_euler(0, 0,0)
            self.q=Quaternion(*q_angle)
            goal.target_pose.pose=(Pose(Point(self.people_msg.pos.x-0.5,self.people_msg.pos.y,self.people_msg.pos.z),self.q))
            self.move_base.send_goal(goal)
            rospy.spin()