ROS机器人编程实践——读书笔记3

本文主要讲解ROS中连续导航：离线方式。（并不是实时规划）。实时要写路径器。

1.txt

2.1 2.2 0.0 0.0 0.0 0.0 1.0
6.5 4.43 0.0 0.0 0.0 0.0 1.0
8.0 3.0 0.0 0.0 0.0 0.0 1.0
9.0 1.0 0.0 0.0 0.0 0.0 1.0
9.0 9.0 0.0 0.0 0.0 0.0 1.0
1.0 9.0 0.0 0.0 0.0 0.0 1.0

position.x，position.y，position.z，orientation.x，orientation.y，orientation.z，orientation.w

ROS在终端启动 $  roslaunch turtlebot_stage turtlebot_in_stage.launch

建立patrol.py

#!/usr/bin/env  python
import rospy
import actionlib
import numpy as np

from move_base_msgs.msg  import MoveBaseAction,MoveBaseGoal

waypoints=[]
f=np.loadtxt('1.txt')
i=0
for i in range(len(f)):	
	point=f[i,:]
	waypoints.append(point)	

def goal_pose(pose):
	goal_pose=MoveBaseGoal()
	goal_pose.target_pose.header.frame_id='map'
	goal_pose.target_pose.pose.position.x=pose[0]
	goal_pose.target_pose.pose.position.y=pose[1]
	goal_pose.target_pose.pose.position.z=pose[2]
	goal_pose.target_pose.pose.orientation.x=pose[3]
	goal_pose.target_pose.pose.orientation.y=pose[4]
	goal_pose.target_pose.pose.orientation.z=pose[5]
	goal_pose.target_pose.pose.orientation.w=pose[6]

	return goal_pose
if __name__ == '__main__':
	rospy.init_node('patrol')

	client=actionlib.SimpleActionClient('move_base',MoveBaseAction)
	client.wait_for_server()

	while True:
		for j in range(len(waypoints)):
			pose=waypoints[j]
			goal=goal_pose(pose)
			client.send_goal(goal)
			client.wait_for_result()

在路径测试时，可以通过相关算法，把路径加载成一个TXT文件。