《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)
{//初始化rosros::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;
}