前言
機器人在巡檢過程中需要沿著固定路線執(zhí)行任務(wù)，因此可以先把機器人的移動軌跡錄制并保存下來，之后讀取軌跡，方便后續(xù)操作。
一、準備
1.坐標系
巡檢導(dǎo)航過程中，機器人需要確定好坐標系，以便進行定位與導(dǎo)航，在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
二、實現(xiàn)過程
1.軌跡保存
思路：使用/amcl_pose話題獲取機器人當前的位置信息，用nav_msgs::Path格式的一個變量Path進行保存，控制機器人的運動，當機器人運動距離超過某一數(shù)值時，將當前位置pose加入該變量，直到機器人走完預(yù)設(shè)的路徑。之后遍歷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);
//遍歷存儲路徑容器，將路徑四元數(shù)轉(zhuǎn)為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();
}
/*
* 當前位置回調(diào)，將間距超過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;
}
運行上述程序后，控制小車運動，當走完所需的路徑后，需單獨發(fā)送一個話題，從而啟動路徑保存
rostopic pub /start_end_record std_msgs/String "data: 'end'"
2.軌跡讀取并顯示
思路：讀取CSV文本并分割，將路徑點發(fā)布出去。
這里也發(fā)布了路徑起點和終點位置。文章來源：http://www.zghlxwxcb.cn/news/detail-757138.html
#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轉(zhuǎn)換成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;
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中添加話題名稱，結(jié)果如圖：
文章來源地址http://www.zghlxwxcb.cn/news/detail-757138.html
到了這里，關(guān)于讀取機器人移動軌跡并在RVIZ界面中顯示的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
