|
前言
ROS系统内置了许多建图以及导航节点，在导航时我们通过拖拽箭头的方式实现目的地的设置，那么能不能通过程序写入一系列导航点，让小车依次沿着这些点前行呢？
基础知识
ROS中使用action传递导航目的点，可以通过该博文看看action的基础知识。在这个博文里面已经实现了给定若干个坐标点，小车依次走过这些点。 本文在该基础上加上了走直线功能，举例说明。若想要小车依次走过三个坐标点a b c，按照上面的博客，a到b的小车路径是不可控的，b到c的同样如此，本文将实现小车导航到a点后，沿直线ab走到b，再沿直线bc走到c。
代码
#include
#include
#include
#include
#include
#include
#include
using namespace std;
#define step_line 0.3 //单位m,每隔0.3m设置一个导航点
typedef actionlib::SimpleActionClient MoveBaseClient;
typedef geometry_msgs::PointStamped Point;
move_base_msgs::MoveBaseGoal goal[100];//导航点数组
//函数输入为线段的起始点和终点，以及前面已设置的导航点个数，输出为加上该直线当前已设置的导航点个数
int common_line(move_base_msgs::MoveBaseGoal origin, move_base_msgs::MoveBaseGoal end, int begin){
int num = ceil(sqrt(pow(origin.target_pose.pose.position.x-end.target_pose.pose.position.x,2)+pow(origin.target_pose.pose.position.y-end.target_pose.pose.position.y,2))/step_line);//通过线段长度计算所需要的导航点个数，实际个数为num加一
goal[begin] = origin;
goal[num+begin] = end;
int i = begin + 1;
double yaw;
geometry_msgs::Quaternion q;
double x1 = origin.target_pose.pose.position.x;
double y1 = origin.target_pose.pose.position.y;
double x2 = end.target_pose.pose.position.x;
double y2 = end.target_pose.pose.position.y;//提取起始点和终点的横纵坐标
//根据线段在二维坐标轴中的具体位置，计算线段上的点的坐标以及导航需要的朝向，间距为0.3m，并将这些坐标放入goal数组
if(x1==x2){
if(y1>y2){
yaw = -M_PI/2;
q = tf::createQuaternionMsgFromRollPitchYaw(0,0,yaw);
goal[begin].target_pose.pose.orientation = q;
goal[num+begin].target_pose.pose.orientation = q;
for(; i |