《Ubuntu20.04环境下的ROS进阶学习7》
一、使用nav_msgs消息包显示小车轨迹
在我们跑实验的时候通常希望看到小车的轨迹，在ROS1中可以将小车的路径存储在nav_msgs::Path 这种消息类型里，发布出来后使用rviz来显示小车轨迹。
二、了解nav_msgs消息包
那么首先我们要来了解一下nav_msgs这个消息包了。这里可以下载common_interfaces这个功能包，里面有常用的很多种消息包可供选择。
使用 git clone https://github.com/ros2/common_interfaces.git
其中nav_msgs消息包中有四种消息格式分别是：
① GridCells.msg ：一般用于显示网格单元的状态，通常用于显示区域中的障碍和自用空间。
② MapMetaData.msg ：提供关于栅格地图的元数据，通常与①一起用。
③ OccupancyGrid.msg ：表示一个二维栅格地图，用于描述环境的占用状态。
④ Odometry.msg ：提供有关机器人位姿和速度的信息，通常用于定位和导航。
⑤ Path.msg ：表示一条路径，通常用于导航算法中的路径规划和跟踪。
这里我们选择第五种消息包，显示一条路径。
这里如果你直接去查看这些消息格式是比较麻烦的，因为他们通常会递归很久才会找到终止条件，即基例，我们可以使用 rosmsg show nav_msgs/Path.msg 来查看这个消息格式。
通过观察缩进可以看到消息的子父类关系。
三、使用nav_msgs::Path.h操作一把
a、编写程序
/*
本文件由jk编辑，本文件主要目的是通过一个小例程了解nav_msgs的使用
*/
//C++库相关
#include <iostream>
#include <cmath>
//ROS库相关
#include <ros/ros.h>
#include <nav_msgs/Path.h>
using namespace std;
int main(int argc , char ** argv)
{
//初始化ros
ros::init(argc , argv , "nav_path_pub");
ros::NodeHandle node;
//创建一个路径的发布者
ros::Publisher Path_pub_car0 = node.advertise<nav_msgs::Path>("/path_car0" , 10);
//创建路径的消息存储数据
nav_msgs::Path nav_path_msgs_car0;
//设置发布频率
ros::Rate loop_rate(10);
//从传感器获取的位置信息 x , y , z 这里我们不关心z轴信息，则忽视它
double path_x = 0;
double path_y = 0.2;
//从传感器获取的角度
double roll = 0.0;
double pitch = 0.0;
double yaw = 0.0;
cout << "i will go while" << endl;
while(ros::ok())
{
//退出循环条件
if(path_x > 10)
{
cout << "i will break" << endl;
break;
}
//获取路径 , 这里大概率是从您的传感器获得的数据
path_x = path_x + 0.1;
path_y = path_y * (-1);
cout << "path_x = " << path_x << endl;
cout << "path_y = " << path_y << endl;
//获取角度 , 这里大概率是从您的传感器获得的数据
double Quaternion_w = cos(0.5*roll)*cos(0.5*pitch)*cos(0.5*yaw) + sin(0.5*roll)*sin(0.5*pitch)*sin(0.5*yaw);
double Quaternion_x = sin(0.5*roll)*cos(0.5*pitch)*cos(0.5*yaw) - cos(0.5*roll)*sin(0.5*pitch)*sin(0.5*yaw);
double Quaternion_y = cos(0.5*roll)*sin(0.5*pitch)*cos(0.5*yaw) + sin(0.5*roll)*cos(0.5*pitch)*sin(0.5*yaw);
double Quaternion_z = cos(0.5*roll)*cos(0.5*pitch)*sin(0.5*yaw) - sin(0.5*roll)*sin(0.5*pitch)*cos(0.5*yaw);
//这里创建一个临时的变量存储位置和姿态信息
geometry_msgs::PoseStamped this_pose_stamped;
//创建一个存储时间的临时变量，用于给header.stamp赋值 获得当前的时间
ros::Time current_time = ros::Time::now();
//下面是存储要发布的数据注意这里的frame_id 可能会需要修改
nav_path_msgs_car0.header.stamp = current_time;
nav_path_msgs_car0.header.frame_id = "map";
this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "map";
this_pose_stamped.pose.position.x = path_x;
this_pose_stamped.pose.position.y = path_y;
this_pose_stamped.pose.position.z = 0.0;
this_pose_stamped.pose.orientation.w = Quaternion_w;
this_pose_stamped.pose.orientation.x = Quaternion_x;
this_pose_stamped.pose.orientation.y = Quaternion_y;
this_pose_stamped.pose.orientation.z = Quaternion_z;
nav_path_msgs_car0.poses.push_back(this_pose_stamped);
//将上面的数据发布出去
Path_pub_car0.publish(nav_path_msgs_car0);
//设置发布频率
loop_rate.sleep();
}
return 0;
}
b、修改CMakeList.txt 和 package.xml文件
这里就不多展示了，之前的文章都有过详细说明。
c、使用rviz展示效果
这里需要添加Path，选择合适的话题和坐标系就可以显示路径了。
原文地址：https://blog.csdn.net/qq_55958175/article/details/142601960
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！