前言
机器人在巡检过程中需要沿着固定路线执行任务，因此可以先把机器人的移动轨迹录制并保存下来，之后读取轨迹，方便后续操作。
一、准备
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 <fstream>
#include <tf/tf.h>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
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<curr_trajectory_.poses.size();i++)
{
float x = curr_trajectory_.poses[i].pose.position.x;
float y = curr_trajectory_.poses[i].pose.position.y;
tf::Quaternion quat;
tf::quaternionMsgToTF(curr_trajectory_.poses[i].pose.orientation,quat);
double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
float th = yaw;
File<<x<<","<<y<<","<<th<<endl;
}
File.close();
}
/*
* 当前位置回调，将间距超过4cm的路径点加入容器
*/
void robotPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & curr_pose)
{
geometry_msgs::PoseStamped current_pose;
current_pose.header.frame_id = "map";
current_pose.header.stamp = ros::Time::now();
current_pose.pose.position = curr_pose->pose.pose.position;
current_pose.pose.orientation = curr_pose->pose.pose.orientation;
if(curr_trajectory_.poses.empty())
{
curr_trajectory_.poses.push_back(current_pose);
return ;
}
int poses_num = curr_trajectory_.poses.size();
double dis = calculateDistanceBetweenPose(curr_trajectory_.poses[poses_num - 1],current_pose);
if(dis > 0.04)
curr_trajectory_.poses.push_back(current_pose);
}
/*
* 接收路径保存指令，开始保存路径点
*/
void savePathCallback(const std_msgs::String::ConstPtr& msg)
{
string str_msgs = msg->data.c_str();
if(str_msgs.compare("end") == 0)
{
if(!curr_trajectory_.poses.empty())
{
string file_path = "aaa";
savePathToFile(file_path.c_str());
curr_trajectory_.poses.clear();
cout<<"end1!"<<endl;
}
cout<<"end2!"<<endl;
}
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"save_path_node");
ros::NodeHandle nh_;
robot_pose_sub_ = nh_.subscribe("/amcl_pose",1,robotPoseCallback);
save_path_sub_ = nh_.subscribe("/start_end_record", 1,savePathCallback);
curr_trajectory_.header.frame_id = "map";
curr_trajectory_.header.stamp = ros::Time::now();
ros::spin();
return 0;
}
运行上述程序后，控制小车运动，当走完所需的路径后，需单独发送一个话题，从而启动路径保存
rostopic pub /start_end_record std_msgs/String "data: 'end'"
2.轨迹读取并显示
思路：读取CSV文本并分割，将路径点发布出去。
这里也发布了路径起点和终点位置。
#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <tf/tf.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <istream>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
#include <tf/tf.h>
using namespace std;
/**
* 字符串分割
*/
std::vector<std::string> split(const std::string &str, const std::string &pattern) {
char *strc = new char[strlen(str.c_str()) + 1];
strcpy(strc, str.c_str()); // string转换成C-string
std::vector<std::string> res;
char *temp = strtok(strc, pattern.c_str());
while (temp != NULL) {
res.push_back(std::string(temp));
temp = strtok(NULL, pattern.c_str());
}
delete[] strc;
return res;
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"path_pub");
ros::NodeHandle n;
ros::Rate r(10);
ros::Publisher path_pub = n.advertise<nav_msgs::Path>("/path_pub",10);
ros::Publisher path_end_pub = n.advertise<geometry_msgs::PointStamped>("/path_end_point",10);
ros::Publisher path_start_pub = n.advertise<geometry_msgs::PointStamped>("/path_start_point",10);
nav_msgs::Path nav_path;
nav_path.header.frame_id= "/map";
nav_path.header.stamp = ros::Time::now();
geometry_msgs::PoseStamped path_pose;
//读取CSV文件
std::ifstream csv_data("path/aaa.csv",std::ios::in);
if(!csv_data)
{
std::cout<<"open .csv failed"<<std::endl;
ROS_ERROR(" .csv doesn't exisit ");
std::exit(1);
}
geometry_msgs::Quaternion quat;
std::string line;
int line_num = 0;
std::vector<std::string> strbuf;
while(ros::ok())
{
while(std::getline(csv_data,line))
{
// std::cout<<line<<std::endl;
path_pose.header.frame_id = "/map";
path_pose.header.stamp =ros::Time::now();
path_pose.header.seq = 0;
line_num++;
strbuf = split(line, ",");
path_pose.pose.position.x = atof(strbuf[0].c_str());
path_pose.pose.position.y = atof(strbuf[1].c_str());
path_pose.pose.position.z = 0.0;
/*
float yaw = atof(strbuf[2].c_str());
quat = tf::createQuaternionMsgFromYaw(yaw);
path_pose.pose.orientation.x = quat.x;
path_pose.pose.orientation.y = quat.y;
path_pose.pose.orientation.z = quat.z;
path_pose.pose.orientation.w = quat.w;
*/
path_pose.pose.orientation.z = std::sin(atof(strbuf[1].c_str())/2.0);
path_pose.pose.orientation.w = std::cos(atof(strbuf[1].c_str())/2.0);
nav_path.poses.push_back(path_pose);
}
path_pub.publish(nav_path);
ros::Duration(1).sleep();
path_end_pub.publish(nav_path.poses[nav_path.poses.size()-1]);
// std::cout<<"----2-----"<<std::endl;
path_start_pub.publish(nav_path.poses[0]);
ros::spinOnce();
r.sleep();
}
return 0;
}
在rviz中添加话题名称，结果如图：
803
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
