从零入门激光SLAM（七）——ROS常用组件

桦树无泪

已于 2023-09-14 17:13:07 修改

阅读量2k

点赞数 8

分类专栏：从零入门激光SLAM 文章标签： python 开发语言

于 2023-04-09 13:16:31 首次发布

本文链接：https://blog.csdn.net/HUASHUDEYANJING/article/details/130040440

版权

从零入门激光SLAM 专栏收录该内容

16 篇文章 255 订阅

订阅专栏

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。

在学习SLAM过程中，会用到数据包，转换地图等操作，此外坐标系的变换也尤为重要，下面将介绍这些ROS组件，将为后续的学习打下基础。

1.rosbag

rosbag 既可以指命令行中数据包相关命令，也可以指 c++/python 的 rosbag 库。这里的 rosbag 是指前者。rosbag 主要用于记录、回放、分析 rostopic 中的数据。它可以将指定 rostopic 中的数据记录到 .bag 后缀的数据包中，便于对其中的数据进行离线分析和处理。

rosbag play 
rosbag play *.bag——正常播放bag
rosbag play --pause bagFile01.bag——等待按空格键播放
rosbag play bagFile01.bag --clock——加上时间戳
rosbag play -r 2 bagFile01.bag——2倍速播放
rosbag play -l bagFile01.bag——循环播放
rosbag play bagFile01.bag –topics——指定topic播放
rosbag play --wait-for-subscribers bagFile01.bag——等待接受者播放

rosbag record
rosbag record -a——记录所有topic
rosbag record /topic1 /topic2 /topic3——记录选定topic
rosbag record -O mybag.bag /my_topic——这将在当前目录下创建一个名为mybag.bag的文件，并录制/my_topic这个topic的信息。

rosbag info BagFileName.bag——显示包的信息
compress&decompress
rosbag compress *.bag——压缩
rosbag decompress *.bag——解压
rosbag check——确认数据采集的有效性和完整性
rosbag reindex——当rosbbag索引损坏不完整时候，重新索引一个rosbag，优化读取性能
rosbag fix——修复文件损坏不完整的rosbag

2.TF

2.1简介

TF(TransForm)，就是坐标转换，包括了位置和姿态两个方面的变换。注意区分坐标转换和坐标系转换。ROS中机器人模型包含大量的部件，每一个部件统称之为link（比如手部、头部、某个关节、某个连杆）,每一个link上面对应着一个frame（坐标系）, 用frame表示该部件的坐标系，frame和link是绑定在一起的。

TF坐标变换如何实现

1.广播TF变换

2.监听TF变换

2.2TF与frame_id

我们在使用RVIZ时，会发现Global Option里面有一个Fixed Frame，当随意修改过后，rviz里面的topic将不会显示。那这个其实就是frame_id，可认为它是一个全局总坐标系或是一个topic组的名称，比如汽车上装激光雷达，有world坐标系，car坐标系，lidar坐标系，如果你设定这组topic的frame_id均为world，那它也是一个全局坐标系，包含了car，lidar等子坐标系。TF中默认将frame_id当作父坐标，也可以说是一个topic组的名字，其他坐标为子坐标。只有一个frame_id下的TF坐标才能构成TF树，在rviz里同时显示。不同的frame_id可认为是不同的topic组，下图来看是一个以camera_link为父坐标系的组，和一个以world为坐标系的组。总的来说，frame_id的作用有两个，1是一个topic组的ROS标识，2是可作为一个全局坐标系，包含各topic的子坐标系。

1.以TransformStamped消息类型的数组显示所有父子frame的位姿转换关系。

rostopic echo /tf

2.生成TF树的PDF图

rosrun rqt_tf_tree rqt_tf_tree

如果遇到以下报错

(base) cxl@cxl-Lenovo-ThinkBook-16p-Gen-2:~$ rosrun tf view_frames

Listening to /tf for 5.0 seconds

Done Listening

b'dot - graphviz version 2.43.0 (0)\n'

Traceback (most recent call last):

File "/opt/ros/noetic/lib/tf/view_frames", line 120, in <module>

generate(dot_graph)

File "/opt/ros/noetic/lib/tf/view_frames", line 89, in generate

html=html.decode('utf-8')

UnboundLocalError: local variable 'html' referenced before assignment

cd /opt/ros/noetic/lib/tf/view_frames
sudo gedit view_frames
添加
vstr=vstr.decode('utf-8')

3.查看TF树

rosrun rqt_tf_tree rqt_tf_tree

4.显示两个坐标系之间的变换关系

rosrun tf tf_echo[source_frame][target_frame]

5.将topic发布在静态TF树上

x y z 代表link2相对与link1的位置变换。qx qy qz qw表示link2相对与link1的位姿变换的四元数。既以link1为参考坐标系得出link2的坐标为x,y,z和角度信息qx qy qz qw。tf_tree为 link1->link2角度信息为沿着link2的轴做调整。参数100表示每隔100毫秒发布一次

四元数

<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z qx qy qz qw link1 link2 100" />
</launch>

x,y,z,yaw,pitch,roll

<launch>
 <node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z yaw pitch roll link1 link2 100" />
</launch>
例如：将两个激光雷达一起发布到静态坐标系Pandar上。
<node name="tf_right_lidar" pkg="tf" type="static_transform_publisher" args="-0.0794421  -0.6    -0.272642   -3.14885 -3.127 -2.37181 pandar rs_lidar_right 20"/>
 
<node name="tf_left_lidar" pkg="tf" type="static_transform_publisher" args="-0.0309343  0.6 -0.272642  3.10685 -3.127  2.37681 pandar rs_lidar_left  20"/>

6.将多个同类型的多个topic，转到一个frame_id发布

void Callback(const sensor_msgs::PointCloud2::ConstPtr &pandar_msg,const sensor_msgs::PointCloud2::ConstPtr &left_msg, const sensor_msgs::PointCloud2::ConstPtr &right_msg)
    {
        resetParameters();
        pcl::fromROSMsg(*pandar_msg, *pandar_local_laser);
        static tf::StampedTransform trans_left_lidar_in_base, trans_right_lidar_in_base;
	    static bool left_transform_get=false, right_transform_get=false;
	    
	    pcl::fromROSMsg(*left_msg, *left_local_laser);
	    bool ok1 = tf_listener_.waitForTransform(pandar_msg->header.frame_id, left_msg->header.frame_id, ros::Time(0), ros::Duration(2.0));
	    tf_listener_.lookupTransform(pandar_msg->header.frame_id, left_msg->header.frame_id, ros::Time(0), trans_left_lidar_in_base);
	    if(ok1)
	    	left_transform_get = true;
	    pcl_ros::transformPointCloud(*left_local_laser, *left_cloud, trans_left_lidar_in_base);

	    
	    pcl::fromROSMsg(*right_msg, *right_local_laser);
	    bool ok2 = tf_listener_.waitForTransform(pandar_msg->header.frame_id, right_msg->header.frame_id, ros::Time(0), ros::Duration(2.0));
	    tf_listener_.lookupTransform(pandar_msg->header.frame_id, right_msg->header.frame_id, ros::Time(0), trans_right_lidar_in_base);
	    if(ok2)
	    	left_transform_get = true;
	    pcl_ros::transformPointCloud(*right_local_laser, *right_cloud, trans_right_lidar_in_base);
	    *left_right_cloud =  *left_cloud + *right_cloud;
	    *fusion_cloud = *left_right_cloud + *pandar_local_laser;
	    //发布融合后的点云
        sensor_msgs::PointCloud2::Ptr fusion_cloud_msg(new sensor_msgs::PointCloud2);
	    pcl::toROSMsg (*fusion_cloud, *fusion_cloud_msg);
	    fusion_cloud_msg->header = pandar_msg->header;
	    pub_fusion_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(fusion_topic_, 10);
	    pub_fusion_cloud_.publish(fusion_cloud_msg);

7.更改TF坐标系并发布

	static tf::TransformBroadcaster br;
	tf::Transform transform2;
	//平移
	transform2.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
	//旋转
	tf::Quaternion q;
	//q.setRPY(0, 0, M_PI_2);
	q.setRPY(0, 0, 0);
	transform2.setRotation(q);
	//发布
	br.sendTransform(tf::StampedTransform(transform2, ros::Time::now(), "word", "ground"));

8.接收TF坐标系

tf::TransformListener listener;
// 发布频率为10Hz
ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      // 查找特定的tf
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    // 根据tf计算速度信息
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
    // 发布速度信息给turtle2                             
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }

3.Map_server

3.1 简介

map_server提供了map_server ROS 节点，它提供了作为ROS 服务的地图数据。由此包中的工具操纵的地图存储在一对文件中。YAML文件描述地图元数据，并命名图像文件。图像文件对占用数据进行编码。该图像描述了相应像素的颜色中的世界的每个单元的占用状态。白色像素是自由的，黑色像素被占据，并且两者之间的像素是未知的。接受彩色和灰度图像，但大多数地图都是灰色的（即使它们可能存储为彩色）。YAML文件中的阈值用于划分三个类别; 阈值在map_server内部完成。map_server可以读取多种格式的地图图像文件，包括PGM（Portable Gray Map）和PNG（Portable Network Graphics）格式。在使用map_server之前，需要将地图文件转换为指定格式。一些常见的用例包括：

用于SLAM系统中的地图构建
用于机器人导航和路径规划
用于机器人的定位和姿态估计

使用map_server的好处是，它可以轻松地将地图集成到ROS系统中，并且提供了一个标准化的接口，使得其他ROS节点可以方便地使用地图信息。

3.2 使用

安装map_server

sudo apt-get install ros-melodic-map-server

使用map_server功能包发布地图：

rosrun map_server map_server /path/to/map.yaml

一个概率栅格地图（Probabilistic Robotics Grid Map，PGM）包含一个yaml文件，如下图所示：

image: 表示地图文件的文件名，这里为"map.pgm"。
resolution: 表示地图每个像素的实际大小，这里为1米。
origin: 表示地图的原点坐标，即地图的左下角。其中，[-156.502350, 0.000000, 0.000000]中第一个数表示地图左下角点的x坐标，第二个数表示y坐标，第三个数表示z坐标。
negate: 表示地图中占据区域的值是否被反转，这里为0表示未反转。
occupied_thresh: 表示将地图上某个栅格定义为障碍物（occupied）的最小概率阈值，超过该阈值的栅格被认为是障碍物。这里为0.65，即概率大于等于0.65的栅格被视为障碍物。
free_thresh: 表示将地图上某个栅格定义为空闲区域（free）的最大概率阈值，低于该阈值的栅格被认为是空闲区域。这里为0.196，即概率小于等于0.196的栅格被视为空闲区域。

发布的话题可在RVIZ中用map进行可视化。

使用map_server功能包保存地图：

rosrun map_server map_saver map:=/projected_map -f map

map:=后面放ros中topic的名字，在octomap生成的栅格地图就叫/projected_map -f 后面接的是保存成的yaml和pgm文件名字与地址。

3.3 小例子

以下是一个自定义的100x100栅格地图发布器，其中占用（occupied）格子表示为100，自由（free）格子表示为0，未知（unknown）格子表示为-1。

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char **argv)
{
  // Initialize ROS node and publisher
  ros::init(argc, argv, "gridmap_publisher");
  ros::NodeHandle nh;
  ros::Publisher gridmap_pub = nh.advertise<nav_msgs::OccupancyGrid>("gridmap", 1);

  // Define gridmap properties
  const int width = 100;
  const int height = 100;
  const double resolution = 0.1;
  nav_msgs::OccupancyGrid gridmap;
  gridmap.header.frame_id = "map";
  gridmap.info.width = width;
  gridmap.info.height = height;
  gridmap.info.resolution = resolution;
  gridmap.data.resize(width * height);

  // Populate gridmap with data
  for (int i = 0; i < height; i++) {
    for (int j = 0; j < width; j++) {
      if (i < height / 2 && j < width / 2) {
        gridmap.data[i * width + j] = 0; // Free space
      } else {
        gridmap.data[i * width + j] = 100; // Occupied space
      }
    }
  }

  // Publish gridmap and spin
  while (ros::ok()) {
    gridmap_pub.publish(gridmap);
    ros::spinOnce();
    ros::Duration(0.1).sleep();
  }

  return 0;
}

在这个例子中，我们订阅了名为gridmap的栅格地图主题，并在回调函数gridmapCallback中将其转换为costmap_2d::Costmap2D，然后定义了起点和终点，并使用A*算法进行路径规划。最后，我们将生成的路径转换为地图坐标系（"map"）并发布到名为path的主题中。请注意，这个代码示例中缺少astar_planner_和tf_listener_对象的定义和初始化，你需要自己实现这些部分以使代码能够正常运行。

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/tf.h>

int main(int argc, char **argv)
{
  // Initialize ROS node and subscribers
  ros::init(argc, argv, "path_planner");
  ros::NodeHandle nh;
  ros::Subscriber gridmap_sub = nh.subscribe("gridmap", 1, gridmapCallback);

  // Initialize path publisher
  ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("path", 1);

  // Initialize transform listener
  tf::TransformListener tf_listener;

  // Spin
  ros::spin();

  return 0;
}

void gridmapCallback(const nav_msgs::OccupancyGrid::ConstPtr& gridmap_msg)
{
  // Convert OccupancyGrid to Costmap2D
  costmap_2d::Costmap2D costmap(gridmap_msg->info.width, gridmap_msg->info.height, gridmap_msg->info.resolution,
    gridmap_msg->info.origin.position.x, gridmap_msg->info.origin.position.y, gridmap_msg->data);

  // Define start and goal poses
  geometry_msgs::PoseStamped start_pose;
  start_pose.header.frame_id = "map";
  start_pose.pose.position.x = 0.0;
  start_pose.pose.position.y = 0.0;
  start_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

  geometry_msgs::PoseStamped goal_pose;
  goal_pose.header.frame_id = "map";
  goal_pose.pose.position.x = 5.0;
  goal_pose.pose.position.y = 5.0;
  goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

  // Perform A* search and generate path
  std::vector<geometry_msgs::PoseStamped> path;
  if (astar_planner_.makePlan(start_pose, goal_pose, path)) {
    // Transform path to map frame and publish
    tf_listener_.waitForTransform("map", path.front().header.frame_id, path.front().header.stamp, ros::Duration(1.0));
    for (auto& pose : path) {
      tf_listener_.transformPose("map", ros::Time(), pose, pose.header.frame_id, pose);
    }
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.poses = path;
    path_pub.publish(path_msg);
  } else {
    ROS_WARN("Path planning failed!");
  }
}

4.Octomap

4.1 简介

1.地图形式不紧凑。

　　点云地图通常规模很大，所以一个pcd文件也会很大。一张640×

480的图像，会产生30万个空间点，需要大量的存储空间。即使经过一些滤波之后，pcd文件也是很大的。而且讨厌之处在于，它的“大”并不是必需的。点云地图提供了很多不必要的细节。对于地毯上的褶皱、阴暗处的影子，我们并不特别关心这些东西。把它们放在地图里是浪费空间。

2.处理重叠的方式不够好。

　　在构建点云时，我们直接按照估计位姿拼在了一起。在位姿存在误差时，会导致地图出现明显的重叠。例如一个电脑屏变成了两个，原本方的边界变成了多边形。对重叠地区的处理方式应该更好一些。

3.难以用于导航

说起地图的用处，第一就是导航啦！有了地图，就可以指挥机器人从A点到B点运动，岂不是很方便的事？但是，给你一张点云地图，是否有些傻眼了呢？我至少得知道哪些地方可通过，哪些地方不可通过，才能完成导航呀！光有点是不够的！

八叉树可以优雅地压缩、更新地图，并且分辨率可调！它以八叉树（octotree，后面会讲）的形式存储地图，相比点云，能够省下大把的空间。

4.2 使用

在ROS中，octomap_server和octomap_map都是用于构建三维地图的软件包。octomap_server软件包提供了一种将输入数据转换为OctoMap格式的方法。它可以将来自激光雷达或深度摄像头等传感器的数据转换为三维点云，并使用这些点云构建OctoMap。octomap_server还可以将OctoMap数据发布为ROS中的OctoMap消息，以便其他ROS节点可以使用它们。octomap_map软件包提供了一种使用OctoMap数据构建环境模型的方法。它可以订阅来自octomap_server的OctoMap消息，并使用该消息构建一个三维地图。该地图可以用于机器人的定位、路径规划和避障等任务。octomap_server用于构建OctoMap数据，而octomap_map用于将OctoMap数据转换为实际可用的地图。

安装

# octomap_server安装
sudo apt-get install ros-melodic-octomap-ros #安装octomap
sudo apt-get install ros- melodic -octomap-msgs
sudo apt-get install ros-melodic-octomap-server
sudo apt-get install ros-melodic-octomap-rviz-plugins

# octomap_map 源码安装
#在安装前要先安装QT5，先在网站下载安装包
https://download.qt.io/archive/qt/5.12/
1.进入安装包路径
cd ~/下载
2.安装包赋权限
sudo chmod +x qt-opensource-linux-x64-5.12.9.run
3.断开无线网，开始安装。
sudo ./qt-opensource-linux-x64-5.12.9.run
4.一路默认，选择安装内容如下图所示即可。

5.安装 g++
sudo apt-get install g++
6.安装 openGL 库
sudo apt-get install mesa-common-dev
7.添加环境路径
gedit ~/.bashrc
export PATH=/opt/Qt5.12.9/Tools/QtCreator/bin:$PATH
# 下载octomap
8.git clone https://github.com/Ocotmap/octomap.git  //如果clone不下来，可以将https换成git
9.mkdir build
cd build
#在编译之前退出conda环境，因为conda环境中有QT，用qmake -v 命令查看当前应用的qt环境
10.编译
cmake ..
make
11.测试
安装完成后，首先打开一个八叉树地图的例子，在终端输入
bin/octovis octomap/share/data/geb079.bt

4.3 octomap_server使用

Pcd转octomap

#下载代码后
mkdir build
cd build
cmake ..
make
cd ..
#生成octmap文件
 ./bin/pcd2octomap ./data/test.pcd ./data/test.bt

octomap转栅格

运用Lego-loam算法在实时运行时候生成三维octomap地图和栅格地图，首先将octomap_server工具包里面的launch文件改为如下所示，主要修改话题和frame_id

<launch>
  <!--启动的节点-->
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"> 

     <!--栅格分辨率-->
    <param name="resolution" value="1" />
    
    <!--世界坐标系话题，一般为map-->
    <param name="frame_id" type="string" value="map" />
 
    <param name="sensor_model/max_range" value="1000.0" />
    <param name="latch" value="true" />
 
     <!--截取的三维点云范围--> 
    <param name="pointcloud_max_z" value="30" />
    <param name="pointcloud_min_z" value="0.2" />
 
    <param name="graound_filter_angle" value="3.14" />
    
     <!--订阅的点云消息名称 PointClout2，即上一步发布出来的点云topic--> 
     <!-- cloud_in是octomap默认的输入话题，做一下映射-->
    <remap from="cloud_in" to="/registered_cloud" />
  </node>
 
</launch>

在运行算法时候启动octomap_server节点，打开rviz可视化地图，其中OccupanyGrid是显示三维概率地图，OccupanyMap是显示二维占据栅格地图。

启动rviz后，点击“add”，分别添加"Map"、“OccupancyGrid"与"OccupancyMap”，并把其话题名依次改为"/projected_map"、"octomap_full"与"octomap_binary"就可以看到建图过程了。

roslaunch octomap_server octomap_server.launch  #启动octomap
rosrun octomap_server octomap_saver map.bt    # 保存地图
rosrun octomap_server octomap_saver -f fullmap.ot   #完整的概率八叉树地图
octovis map.bt    #显示地图

运用map_server功能包，接收octomap_map的话题，保存栅格地图。

rosrun map_server map_saver map:=/projected_map -f a.pgm   #保存栅格地图

自定义octomap

进入octomap/octomap/src文件夹下，会有一个simple_example.cpp文件，这个是官方给的一个示例文件。随后到octomap/bin执行文件夹下运行。

./simple_example //会生成一个simple_tree.bt文件
Octovis simple_tree.bt //即可可视化改octomap

仿照官方的例子，我们可以在octomap/octomap/src文件夹下建立一个test_map.cpp文件

#include <octomap/octomap.h>
#include <octomap/OcTree.h>
 
using namespace std;
using namespace octomap;
 
 
void print_query_info(point3d query, OcTreeNode* node) {
    if (node != NULL) {
    cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
    }
    else 
    cout << "occupancy probability at " << query << ":\t is unknown" << endl;    
}
 
int main(int /*argc*/, char** /*argv*/) {
 
    cout << endl;
    cout << "generating example map" << endl;
 
    OcTree tree (0.1);  // create empty tree with resolution 0.1 [ http://octomap.github.io/octomap/doc/classoctomap_1_1OcTree.html#a08375ca02aff5117437ab2a417c8e062]
 
 
    // insert some measurements of occupied cells
 
    // 四周的墙
    for (int y=0; y<200; y++){
        int x = 0;
        for (int z=-6; z<7; z++){
            point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
            tree.updateNode(endpoint, true);// integrate &apos;occupied&apos; measurement
        }
    }
 
    
    for (int x=0; x<200; x++){
        int y = 200;
        for (int z=-6; z<7; z++){
            point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
            tree.updateNode(endpoint, true);// integrate &apos;occupied&apos; measurement
        }
    }
 
    for (int y=0; y<200; y++){
        int x = 200;
        for (int z=-6; z<7; z++){
            point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
            tree.updateNode(endpoint, true);// integrate &apos;occupied&apos; measurement
        }
    }
 
    for (int x=0; x<200; x++){
        int y = 0;
        for (int z=-6; z<7; z++){
            point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
            tree.updateNode(endpoint, true);// integrate &apos;occupied&apos; measurement
        }
    }
 
    // 中间的厚墙障碍
    for (int x=80; x<120; x++){
        for (int y=0; y<150; y++){
            for (int z=-6; z<7; z++){
                point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
                tree.updateNode(endpoint, true);// integrate &apos;occupied&apos; measurement
        }
    }
    }
 
    cout << endl;
    tree.writeBinary("test_map1.bt");
    cout << "wrote example file test_map1.bt" << endl << endl;
    cout << "now you can use octovis to visualize: ./octovis test_map1.bt"  << endl;
    cout << "Hint: hit &apos;F&apos;-key in viewer to see the freespace" << endl  << endl;  
}

并在当前文件夹下的CMakelists下添加该文件重新编译，在bin文件夹下会出现相test_map执行文件，执行该文件就可以生成cotomap地图。

ADD_EXECUTABLE(test_map test_map.cpp)
TARGET_LINK_LIBRARIES(test_map octomap)

发布接收octomap

发布现成的octomap.bt文件
#include <ros/ros.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
using namespace std;
int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "octomap_publisher");

  // 创建节点句柄
  ros::NodeHandle nh;

  // 加载OctoMap文件
  std::string octomap_file;
  octomap_file = "/home/cxl/workspace/lidar_slam_basic/ch4/octomap_ros/src/bt_publisher/src/test_map1.bt";
ROS_INFO_STREAM("Loading OctoMap file: " << octomap_file);
octomap::OcTree tree(octomap_file);
ROS_INFO_STREAM("OctoMap file loaded.");

// 将OctoMap转换为OctoMap消息类型
octomap_msgs::Octomap octomap_msg;
octomap_msg.header.frame_id = "map"; //一定要指定frame_id才能显示出三维数据
octomap_msgs::binaryMapToMsg(tree, octomap_msg);


// 创建OctoMap发布者
ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap", 1);

// 设置发布频率
ros::Rate loop_rate(1.0);

while (ros::ok())
{
// 发布OctoMap消息
octomap_pub.publish(octomap_msg);

// 等待下一个周期
loop_rate.sleep();
}

  return 0;
}

发布实时创建的octomap地图

#include <ros/ros.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/OcTree.h>
#include <octomap/ColorOcTree.h>
int main(int argc, char** argv) {
  ros::init(argc, argv, "custom_octomap_publisher");
  ros::NodeHandle nh;

  // 创建OctoMap对象并进行配置
  octomap::OcTree* octree = new octomap::OcTree(0.1); // 使用0.1米的分辨率创建OctoMap
  // 设置OctoMap的尺寸为10米（正方形）
  double octomap_size = 10.0;
  octree->setProbHit(0.7);  // 设置命中概率
  octree->setProbMiss(0.4);  // 设置未命中概率
  octree->setClampingThresMin(0.12);  // 设置最小八叉树节点概率值
  octree->setClampingThresMax(0.97);  // 设置最大八叉树节点概率值
  octree->setResolution(0.05);  // 设置分辨率为5厘米

  // 添加节点到OctoMap中
  octomap::point3d min_point(-octomap_size / 2.0, -octomap_size / 2.0, 0.0);  // 设置正方形左下角点
  octomap::point3d max_point(octomap_size / 2.0, octomap_size / 2.0, 0.0);  // 设置正方形右上角点
  octree->setBBXMin(min_point);  // 设置OctoMap的最小范围
  octree->setBBXMax(max_point);  // 设置OctoMap的最大范围
  octomap::point3d origin(0.0, 0.0, 0.0);
  for(double x = min_point.x(); x <= max_point.x(); x += octree->getResolution()) {
    for(double y = min_point.y(); y <= max_point.y(); y += octree->getResolution()) {
      octree->updateNode(octomap::point3d(x, y, 0.0) + origin, true);
    }
  }

  // 将OctoMap转换为ROS消息类型
  octomap_msgs::Octomap octomap_msg;
  octomap_msg.header.frame_id = "map";
  octomap_msgs::binaryMapToMsg(*octree, octomap_msg);

  // 发布OctoMap消息
  ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap", 1, true);
  octomap_pub.publish(octomap_msg);

  // 循环等待ROS消息
  ros::spin();

  delete octree;
  return 0;
}

接收octomap_msgs::Octomap消息用于规划

#include <ros/ros.h>
#include <octomap_msgs/Octomap.h>
#include <octomap/octomap.h>

int main(int argc, char **argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "octomap_planning_node");
  ros::NodeHandle nh;

  // 创建一个订阅Octomap的订阅者
  ros::Subscriber octomap_sub = nh.subscribe<octomap_msgs::Octomap>("/octomap", 1, octomapCallback);

  // 循环等待ROS事件
  ros::spin();

  return 0;
}

void octomapCallback(const octomap_msgs::Octomap::ConstPtr& octomap_msg)
{
  // 将octomap消息转换为octomap数据类型
  octomap::OcTree* octree = dynamic_cast<octomap::OcTree*>(octomap_msgs::fullMsgToMap(*octomap_msg));

  // 检查octomap是否为空
  if (octree == NULL) {
    ROS_ERROR("Octomap is null!");
    return;
  }

  // 执行规划算法
  // ...

  // 释放octomap内存
  delete octree;
}

5.SSH通信

1、连接同局域网

将主机和从机连接相同的网络，在同一局域网内。例如192.168.0.*（前三位相同）网络会自动给每个机器一个IP

2、安装SSH

sudo apt-get install ssh
sudo apt-get install openssh-server
sudo apt-get install openssh-client

3、id_rsa.pub文件传递

ssh-keygen

然后将id_rsa.pub 文件复制到远程计算机

scp ~/.ssh/id_rsa.pub cxl@192.168.0.188:/tmp

4、修改 /etc/hosts文件

加入无人机zhao和b的ip

5、修改~/.bashrc（所有通信机器）

export ROS_HOSTNAME=192.168.0.122 #本机IP
export ROS_MASTER_URI=http://192.168.0.122 #主机IP
所有机器的主机IP都应该一样，变的只是本机IP，所有消息都会传到主机

6、登录SSH进行控制及查看消息

出现这个错误连接不上采用下面命令

ssh -o StrictHostKeyChecking=no 192.168.xx.xx

需要在主机运行ROSCORE，进入到从机后才能启动ros命令。如果想单独用从机运行，则把bashrc文件加的那两行export注释即可。关于消息，只要是在同一局域网下，所有设备上的rostopic共享，在主机运行rosbag，从机也可以也受到topic。

好啦，到目前为止，博主已经介绍了在学习ROS过程中常用的组件，下一节将介绍ROS中的常用msg，也是ROS专题教程的最后一节，这样你就可以随意的接收发布传感器数据了。

桦树无泪

关注

8
点赞
踩
14

收藏

觉得还不错? 一键收藏
打赏
0
评论
从零入门激光SLAM（七）——ROS常用组件

TF(TransForm)，就是坐标转换，包括了位置和姿态两个方面的变换。注意区分坐标转换和坐标系转换。ROS中机器人模型包含大量的部件，每一个部件统称之为link（比如手部、头部、某个关节、某个连杆）,每一个link上面对应着一个frame（坐标系）, 用frame表示该部件的坐标系，frame和link是绑定在一起的。TF坐标变换如何实现1.广播TF变换2.监听TF变换2.2TF与frame_id。
复制链接

扫一扫