ROS多点导航

 

?

1 2	cd ~/catkin_ws/src catkin_create_pkg myrobot rospy geometry_msgs sensor_msgs

　　

?

1 2	cd ~/catkin_ws/src/myrobot/src vim patrol.py

　　

 

?

1 2 3 4 5 6 7 8 9 10 11 12

#!/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)] ]

　　

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13

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

　　

 

?

1 2 3 4 5 6 7 8 9 10 11 12

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()

　　

?

1	rostopic echo amcl_pose -n 1

　　

?

1	rosrun myrobot patrol.py

　　

 

 

?

1	chmod 777 patrol.py

　　

 

 

参考：

https://www.guyuehome.com/10949