ROS多点导航
cd ~/catkin_ws/src catkin_create_pkg myrobot rospy geometry_msgs sensor_msgs
cd ~/catkin_ws/src/myrobot/src vim patrol.py
#!/usr/bin/env python import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal waypoints = [ # <1> [(1.522, 0.444, 0.0), (0.0, 0.0, -0.519, 0.85)], [(-2.0432, -0.439, 0.0), (0.0, 0.0, -0.559, 0.82902)] ]
def goal_pose(pose): # <2> goal_pose = MoveBaseGoal() goal_pose.target_pose.header.frame_id = 'map' goal_pose.target_pose.pose.position.x = pose[0][0] goal_pose.target_pose.pose.position.y = pose[0][1] goal_pose.target_pose.pose.position.z = pose[0][2] goal_pose.target_pose.pose.orientation.x = pose[1][0] goal_pose.target_pose.pose.orientation.y = pose[1][1] goal_pose.target_pose.pose.orientation.z = pose[1][2] goal_pose.target_pose.pose.orientation.w = pose[1][3] return goal_pose
if __name__ == '__main__': rospy.init_node('patrol') client = actionlib.SimpleActionClient('move_base', MoveBaseAction) # <3> client.wait_for_server() while True: for pose in waypoints: # <4> print("goal:x=%f y=%f"%(pose[0][0],pose[0][1])) goal = goal_pose(pose) client.send_goal(goal) client.wait_for_result()
rostopic echo amcl_pose -n 1
rosrun myrobot patrol.py
chmod 777 patrol.py
参考:
https://www.guyuehome.com/10949
作者:kay 出处:https://www.cnblogs.com/kay2018/ 本文版权归作者和博客园共有,欢迎转载,但未经作者同意必须保留此段声明,且在文章页面明显位置给出原文连接,否则保留追究法律责任的权利。 如果文中有什么错误,欢迎指出。以免更多的人被误导。 |