turtlebot2 多目标点自主导航

任务要求：人为的设置多个点，发送目标点，使得机器人在目标点之间做往返运动

1.目标点的获得

目标点的设置可以是相对坐标（以机器人当前的位置为原点）；绝对坐标（以地图为上的原点为原点）

任务需要就是要个机器人发送地图的绝对坐标，来检测机器人的定位精度，想到以往导航时都是通过Rviz中的2d nav goal 设置目标点

所以可以写一个小例程，将Rviz中的目标点信息读取到文本中，在下次导航的时候，直接读取目标点信息就可以了。

通过网上的信息，2d nav goal 的发布话题是“move_base_simple/goal”，查阅其的消息类型为“geometry_msgs/PoseStamped”

参考资料：https://blog.csdn.net/sinat_25923849/article/details/108056212

position的三个量没什么好说的，平面上前两个就可一了，关于orientation，应该是四元数吧，没怎么深入学习，反复试了几次后发现平面上只有

x,w是数值的，其余的都是0；

读出来信息后就直接利用c++的 fstream 进行文件的写入

代码：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
#define path "/home/zhao/ros_code/pose.dat"

//保存位姿到文件
void Save_pose(vector &p)
{
    ofstream ofs;
    ofs.open(path,ios::out|ios::app);
    ofs< pose;
    pose.push_back(msg->pose.position.x);
    pose.push_back(msg->pose.position.y);
    pose.push_back(msg->pose.orientation.w);
    pose.push_back(msg->pose.orientation.z);

    Save_pose(pose);
    pose.clear();
}


int main(int argc,char **argv)
{
    ros::init(argc,argv,"get_pose");
    ros::NodeHandle h;
    ros::Subscriber sub=h.subscribe("move_base_simple/goal", 1000,Callback);

    ros::spin();
    return 0;
}

2.发送目标点到movebase

参考资料：https://blog.csdn.net/ABC_ORANGE/article/details/104514621  ；https://blog.csdn.net/hanmoge/article/details/113594710 ；https://heyijia.blog.csdn.net/article/details/43565227；

不同于以往的话题通信，目标点的发送采用了动作通信

代码：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
#define path "/home/zhao/ros_code/pose.dat"

//重命名，简化名字
typedef actionlib::SimpleActionClient MoveBaseClient;
vector goal;

void DoShutdown(int sig)
{
	ROS_INFO("shutting down!");
	ros::shutdown(); 
    exit(sig);
}

void Readgoal(vector &p)
{
    ifstream ifs;
    ifs.open(path,ios::in);
    double d;
    while (ifs>>d)
    {
        p.push_back(d);
    }
    int count=p.size();
    cout<<"读取到"<

获取到目标点后，关于两点之间的往复运动。自己写了一个简单的循环，只是为了实现要求，这个循环很简陋

3.总结

首先终于用到了动作通信的实际例子，大体上的流程差不多清楚了。类比以前的话题通信、socket通信，都是有固定的流程的。

关于ros下的等待函数 ros::duration().sleeo(); 、关于ros::spin()和ros::spinonce()的区别和用法，有了一定的了解。

对于一些数据结构的知识也用到了一点，之前学的vector容器，还有函数应用、指针.....结合实际的应用，感觉更加可以加深理解。