|
文章目录
前言一、准备1.坐标系2.ros下的路径消息格式
二、实现过程1.轨迹保存2.轨迹读取并显示
前言
机器人在巡检过程中需要沿着固定路线执行任务，因此可以先把机器人的移动轨迹录制并保存下来，之后读取轨迹，方便后续操作。
一、准备
1.坐标系
巡检导航过程中，机器人需要确定好坐标系，以便进行定位与导航，在gazebo仿真下可以选择world坐标系，在实际使用中通常使用的是map坐标系，这里以map坐标系为例进行介绍。
2.ros下的路径消息格式
rosmsg show nav_msgs/Path
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
二、实现过程
1.轨迹保存
思路：使用/amcl_pose话题获取机器人当前的位置信息，用nav_msgs::Path格式的一个变量Path进行保存，控制机器人的运动，当机器人运动距离超过某一数值时，将当前位置pose加入该变量，直到机器人走完预设的路径。之后遍历Path中路径点保存输出CSV文本即可。
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
ros::Subscriber robot_pose_sub_;
ros::Subscriber save_path_sub_ ;
nav_msgs::Path curr_trajectory_;
/*
* 计算两点间距离
*/
double calculateDistanceBetweenPose(const geometry_msgs::PoseStamped& pose1,const geometry_msgs::PoseStamped& pose2)
{
double d_x = pose2.pose.position.x - pose1.pose.position.x;
double d_y = pose2.pose.position.y - pose1.pose.position.y;
return sqrt(d_x* d_x + d_y * d_y);
}
/*
* 保存路径
*/
void savePathToFile(string filename)
{
ofstream File;
//保存文本地址
string filePathName;
filePathName = "/home/name/path/"+ filename +".csv";
File.open(filePathName.c_str(),ios::out|ios::trunc);
//遍历存储路径容器，将路径四元数转为yaw角，写入文本
for(int i=0;i |