《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，选择合适的话题和坐标系就可以显示路径了。