前言
在了解了ROS地图消息包的数据结构后(链接在上),本文将编写一个节点,发布地图消息包,看看在RViz中显示是什么效果。
一、准备
1、为了简单起见,发布一个两行四列的地图
2、为了便于观测,只对地图第一行进行赋值(注意,第一行是从下往上数),第二行保持空白。
3、根据ROS定义的栅格占据值,有障碍物占据的栅格值为100,没有障碍物的为0,未知状态的为-1.
4、进行如下赋值,则0和1号格有障碍物,2号格没有,3号格未知。
二、实现步骤
三、操作
1、输入cd catkin_ws1/src/ 进入工作空间的src目录
2、输入catkin_create_pkg map_pkg rospy roscpp nav_msgs创建软件包map_pkg
3、输入code .打开VScode
4、在map_pkg软件包的src目录下创建节点代码文件map_pub_node.cpp
5、在map_pub_node.cpp节点文件中,写入如下代码
cpp
#include <ros/ros.h>// 引入ROS和导航消息
#include <nav_msgs/OccupancyGrid.h>
int main(int argc, char *argv[])
{
ros::init(argc,argv,"map_pub_node");// 初始化ROS节点
ros::NodeHandle n;
ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map",10);// 创建一个名为/map的OccupancyGrid类型的话题发布者
ros::Rate r(1);// 设置发布频率为1Hz
while (ros::ok())
{
nav_msgs::OccupancyGrid msg;// 创建一个OccupancyGrid类型的消息
// 设置消息的帧ID和时间戳
msg.header.frame_id = "map";
msg.header.stamp = ros::Time::now();
// 设置地图的原点坐标和分辨率
msg.info.origin.position.x = 0;
msg.info.origin.position.y = 0;
msg.info.resolution = 1.0;
// 设置地图的宽度和高度
msg.info.width = 4;
msg.info.height = 2;
// 设置地图的数据,这里假设地图大小为4x2,并赋予一些示例数据
msg.data.resize(4*2);
msg.data[0] = 100;
msg.data[1] = 100;
msg.data[2] = 0;
msg.data[3] = -1;
// 发布地图消息
pub.publish(msg);
// 按照预定的频率休眠
r.sleep();
}
return 0;
}
6、在CMakeLists.txt文件末尾,添加如下编译规则
cpp
add_executable(map_pub_node src/map_pub_node.cpp)
target_link_libraries(map_pub_node
${catkin_LIBRARIES}
)
7、进入工作空间,输入source ./devel/setup.bash设置ROS的环境变量,以便在当前的终端窗口中正确运行ROS软件包。
8、输入catkin_make进行编译
9、编译成功
10、接下来运行节点,首先重新打开一个终端,启动roscore
11、再打开一个终端,进入工作空间,先输入source ./devel/setup.bash设置环境变量,再输入rosrun map_pkg map_pub_node运行节点。
12、再打开一个终端,输入rviz运行rviz
13、在rviz中添加一个坐标轴标识
14、这个标识的位置,便是世界坐标系的原点
15、点击这个,添加地图显示
16、将地图消息的话题名称设置为实验节点发布的/map
17、可以看到地图显示出来了,跟地图规划时进行对比,发现是一样的,说明地图创建成功了。