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容器，还有函数应用、指针.....结合实际的应用，感觉更加可以加深理解。