本问题是Effective C++的条款20

提高代码效率

我们知道函数的实际参数是实参的一个副本，它是由copy构造函数产生的，但这种传const引用的方式可以提高函数调用效率，它不会涉及副本的构造函数、析构函数和copy构造函数

测试代码如下，对基类定义了copy构造函数，但派生类没有定义copy构造函数，信息中会输出this指针的地址

1
2
3
4
5
6
7
8
9

void useDerived(const Derived& obj)
{
    cout<<" use Derived"<<endl;
}

Derived f;
cout<<"-------------"<<endl;
useDerived(f);
cout<<"*************"<<endl;

运行结果：

1
2
3
4
5
6
7

Base constrct  0x75fd20
Derived construct  0x75fd20
-------------
 use Derived
*************
Derived deconstruct  0x75fd20
Base deconstrct  0x75fd20

如果改用值传递的方式，结果就变成下面这样：

1
2
3
4
5
6
7
8
9
10

Base constrct  0x75fd10
Derived construct  0x75fd10
-------------
Base copy constrct  ---0x75fd40
 use Derived
Derived deconstruct  0x75fd40
Base deconstrct  0x75fd40
*************
Derived deconstruct  0x75fd10
Base deconstrct  0x75fd10

避免对象切割问题( object sliced )

如果上面的函数改成这样：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

void useDerived(Base f)
{
    f.testConst();
    cout<<"value use Derived"<<endl;
}

// testConst声明为虚函数
void Base::testConst() const
{
    cout << "base test const" <<endl;
}
void Derived::testConst() const
{
    cout << "derived test const" <<endl;
}
// 调用
Derived d;
useDerived(d);

结果为：

1
2
3
4
5
6
7
8
9
10

base constructor
derived constructor
# 开始副本
base copy constructor
base test const     // 基类版本
value use Derived
base destructor

derived destructor
base destructor

不过对于内置类型和STL的iterator，函数对象，还是用值传递的方式比较好

根据概率论知识，SLAM工作可以划分为定位和建图两部分，一般是通过观测值$z_{1:t}$和里程计信息$u_{1:t-1}$进行定位得到$x_{1:t}$，然后根据位姿 $x_{1:t}$ 以及观测信息进行建图得到m。

gmapping其实就是优化版的RBPF，先看RBPF的一般过程：

1. 时间t的粒子群 $X^ \left( i \right)_t$ 是 $X^ \left( i \right)_{t-1}$ 从提议分布中采样获得的，提议分布往常是里程计运动模型 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$
2. 粒子权重 $w^ \left( i \right)$ 是目标分布和提议分布的比，用来描述提议分布和目标分布的差别，越接近1说明提议分布越好
3. 重采样：只有部分粒子用于估计目标分布，重采样之后，所有粒子权重相同。 如果能直接从目标分布采样，所有粒子权重都相同，实际上只用1个粒子就行，或者说没有采样的概念了。因此，提议分布越差，粒子群的权重差别越大， $N_{eff}$ (粒子归一化权重的平方和的倒数)就越大。
4. 根据轨迹各个位姿 $X^ \left( i \right)_{1:t}$ 和各个观测 $Z^ \left( i \right)_{1:t}$ 估计地图

• 目标分布： 真实分布，如果考虑机器人的运动，那么机器人在t时刻位置的真实分布就是目标分布，但是这是不能得到的，只是理论上的分布，我们需要只能通过其他分布来得到该分布

• 提议分布: 使用提议分布来得到目标分布的真实情况。一般的，使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性，方差更小，更适合作为提议分布，因此考虑激光和里程计一起作为改进的提议分布

gmappping的优化

优化提议分布

不仅考虑机器人的运动，而且考虑了最近的观测，使得提议分布更接近目标分布，从而减少了所需的粒子数。之前的提议分布是 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$，优化之后是 $P(x{_t}\ |x_{t-1}\ , \ u_{t-1}\ , \ m_{t-1}\ , \ z_t)$

如果只用运动模型做提议分布，粒子权重计算比较简单: $w^ \left( i \right)_t \propto w^ \left( i \right)_{t-1}\ \cdot p(z_t\ | m^ \left( i \right)_{t-1}\ , x^ \left( i \right)_t)$。 但是里程计提供位姿的不确定度比激光大得多，导致置信区间太大，似然又太小，各个粒子的权重差别很大，实际上有意义的只有一小部分，结果就需要很多粒子。

相比之下，雷达观测的置信区间小，似然大，所需粒子就少了很多。所以可以把观测加入到提议分布优化之后，结果粒子权重公式为公式 13

因为观测似然函数的不确定形态，提议分布没有闭式解，所以方法是用采样对提议分布做估计。

减少重采样

RBPF中，粒子要覆盖里程计状态的全部空间，但只有一部分粒子是接近目标分布的，粒子权重的差别大，所以需要频繁重采样来丢弃权重小的粒子，保留权重大的粒子，同时又造成了另一个弊端：粒子退化。

选择性重采样，当 $N_{eff}$ 小于粒子数一半时，说明粒子分布和真实分布差距很大，某些粒子离真实值近，某些远，执行重采样。

GMapping依靠粒子的多样性， 回环时消除累积的误差。起始位置需要选择特征较为丰富的地方， 这样能在回环时提高正确粒子权重。

1. 使用scan-matcher估计似然函数的置信区间
2. 对置信区间采样，对采样点进行评价
3. 对每个粒子计算，计算K个粒子的期望方差，计算时考虑了里程计运动信息，公式15和16

算法复杂度

主要影响因素是计算提议分布，更新地图，计算权重，检测是否需要重采样，重采样。 结合源码，最大的影响参数是particles, 其次是resampleThreshold, map_update_interval, throttle_scans, lskip

$N_{eff}$

1. 机器人经过未知区域时，$N_{eff}$ 下降很慢
2. 机器人经过已知区域时，每个粒子都在各自的地图中保持定位，权重都相差不大
3. 当closing loop时， $N_{eff}$ 会显著下降，有些粒子能保持定位，有些则丢失了定位，权重相差很大，此时执行重采样

缺陷

从论文和源码上看，gmapping算法有以下缺陷:

1. scan matcher 计算的是观测似然函数的置信区间，但函数可能是多峰的，比如closing loop的时候，这就造成了问题，粒子群的权重会有很大波动， $N_{eff}$ 会显著下降，当低于resample_Threshold时，就会执行重采样，然后恢复 $N_{eff}$ 最大值． $N_{eff}$ 之所以大幅下降，就是因为观测似然出现多峰，使得提议分布与目标分布差别太大，导致粒子权重减小．因此不适用于多闭环的环境

2. 遇到长走廊或很空旷的环境，雷达数据没有什么特征，比如测距都是最大值，造成位姿在走廊方向的严重不确定，此时里程计精度就很重要了，有利于粒子群的收敛。如果里程计精度不好，增加粒子数也可以解决这个问题。

3. drawFromMotion函数是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《概率机器人》中提到的速度模型和里程计模型，有很多方面都没有考虑，精度上可能有折扣。

4. 随着场景增大所需的粒子也增加，内存和计算量都会增加，实际中建的地图不到几千平米时就会发生错误

5. 重采样的粒子索引选取用的是轮盘赌算法，有些论文提出了一些更好的算法

6. 机器人的轨迹估计与真实的轨迹有一定差异，轨迹出现较大畸变的地方导致了＂假墙＂．这是粒子多样性降低造成的，需要增加粒子数
   

7. 源码中，用户设置的miniScore参数过大时(超过170)，scan match失败，转而使用里程计进行位姿估计．但这样就更不准了，而且gmapping算法里没有提示

综合来看，Gmapping适合的环境有以下特点: 小场景、closing loop比较少、没有长走廊或很空旷的环境、雷达精度高、机器人主机配置较高。此时相比Cartographer，Gmapping不需要太多的粒子并且没有回环检测，因此计算量小于Cartographer，精度没有差太多。

参考：
各种激光slam: 开源代码的比较
2D激光SLAM算法优劣对比

用于估计的二轮差速里程计模型

二轮差速模型的航迹推演原理图，后方两个驱动轮，前面两个万向轮

• IMU只提供yaw
• 根据航迹推演，两驱车的线速度是两个轮子线速度的平均值，角速度是 差/底盘长度;两个参数作为raw_vel话题发布
• 机器人切线运动模型
  base_controller节点正确读取到底层（比如嵌入式控制板）传回的速度后进行积分，计算出机器人的估计位置和姿态，并将里程计信息和tf变换发布出去。
• 机器人中通过运行底盘控制ROS驱动，来实现读取串口的速度反馈，利用航迹推演算法计算得到里程计并发布到/odom这个主题

之所以说这个是粗略的定位，是因为在实际情况中可能会碰到轮子打滑，地面不平整等因素的干扰，里程计的运动增量带有噪声，对速度积分进行航迹推算得到的里程计累积误差会越来越大。当然上层会通过激光信息来匹配校准。

IMU姿态的协方差矩阵代表了姿态测量的不确定度。

用于规划的速度模型

IMU线加速度噪声积分后根本不能用做线速度

IMU在制造过程中，由于物理因素，导致实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。自校准就是要通过给的补偿值来减小坐标轴的偏差及原始值的固定偏差，也就是所谓的IMU内参标定，是为了获得噪声参数。

如果将IMU安装到机器人或摄像头上后，需要知道IMU与机器人或摄像头的相对位置，这个时候进行的标定就是所谓的IMU外参标定

通过IMU获得的是odom坐标系下的坐标，sensor_msgs/Imu消息：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

# 保存IMU的数据
# 加速度单位 m/s^2 (not in g's), 角速度单位 rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# 如果协方差矩阵元素全是0，那么认为是未知协方差。
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

Header header
geometry_msgs/Quaternion orientation   # IMU的当前姿态，4元数的形式表示
float64[9] orientation_covariance   # 姿态的协方差

geometry_msgs/Vector3 angular_velocity  	 # IMU的3轴角速度
float64[9] angular_velocity_covariance

geometry_msgs/Vector3 linear_acceleration  	 # IMU的3轴加速度
float64[9] linear_acceleration_covariance

滤波工具

ROS官方提供了滤波工具，可以直接安装：sudo apt-get install ros-kinetic-imu-tools，包括两个package和一个rviz插件：

• imu_filter_madgwick: 将IMU设备读取的角速度、加速度和磁力计融合成方位信息

• imu_complementary_filter: 一种基于强制融合的新方法将IMU设备读取的角速度、加速度和磁力计融合成方位信息。

• rviz_imu_plugin：显示sensor_msgs::Imu的rviz插件

两种滤波器都是订阅IMU节点发布的imu/data_raw话题，经过滤波处理后，再发布imu/data话题。但是我用两种滤波器处理IMU数据后，没有发现明显的改善，也许是设备的原因，本来数据浮动就不大

参考：imu_tools

目前导航功能包只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息类型发布的传感器数据。

sensor_msgs/LaserScan和 sensor_msgs/PointCloud跟其他的消息一样，包括tf帧和与时间相关的信息。字段frame_id存储与数据相关联的tf帧信息。以激光扫描为例，它将是激光数据所在帧。

我们使用laser_geometry包将雷达的scan转换为点云，它只有一个C++类LaserProjection，没有ROS API.

有两个函数可以将sensor_msgs/LaserScan转换为sensor_msgs/PointCloud或者sensor_msgs/PointCloud2。 projectLaser()简单快速，不改变数据的frame; transformLaserScanToPointCloud()速度慢但是精度高，使用tf和sensor_msgs/LaserScan的time_increment转换每束激光，对倾斜的雷达或移动的机器人，推荐这种方法。

sensor_msgs/PointCloud还包括一些额外的通道，例如intensities, distances, timestamps, index 或者thew viewpoint
支持将三维空间中的点的数组以及任何保存在一个信道中的相关数据。例如，一条带有intensity信道的 PointCloud 可以保持点云数据中每一个点的强度。

projectLaser

它执行最简单的激光投射，每束激光投射出去的角度根据的是以下公式：

但最后形成的点云消息，坐标系还是laser，不适用于雷达移动或畸变不能忽略的情况。

transformLaserScanToPointCloud

这种方法需要tf变换，由于我们是扫描过程中一直收集数据，选择的target_frame必须是固定的。因为sensor_msgs/LaserScan的时间戳是第一次测量的时间，这个时间还无法转换到target_frame，得等到最后一个雷达的测量时间完成坐标系转换。

代码

依赖包是laser_geometry, pcl_conversions, pcl_ros
修改package.xml文件，添加

1
2

<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

#include "ros/ros.h"
#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "tf/transform_listener.h"
#include <boost/shared_ptr.hpp>

laser_geometry::LaserProjection  projector;
ros::Publisher pub;
boost::shared_ptr<tf::TransformListener> listener_;

void chatterCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    sensor_msgs::PointCloud2 cloud;
    // projector.projectLaser(*msg, cloud);    // 第一种方法

    if(!listener_->waitForTransform(
                "/base_link",
                msg->header.frame_id,
                msg->header.stamp + ros::Duration().fromSec(msg->ranges.size()*msg->time_increment),
                ros::Duration(2.0)) )
    {
        ROS_WARN("no transform");
        return;
    }
    projector.transformLaserScanToPointCloud("/base_link",*msg,
                                             cloud, *listener_);
    pub.publish(cloud);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "laser_toCloud");
    ros::NodeHandle nh;
    listener_ = boost::make_shared<tf::TransformListener>();
    pub = nh.advertise<sensor_msgs::PointCloud2>("cloud",10);
    ros::Subscriber sub = nh.subscribe("/scan", 1000, chatterCallback);   // 先有发布的scan话题

    ros::spin();
    return 0;
}

参考：
激光LeGO-LOAM生成pcd点云地图保存和格式转换
LaserScan转pcl::PointCloud
laser_geometry

PCL包括多个子模块库。最重要的PCL模块库有如下：过滤器Filters、特征Features、关键点Keypoints、配准Registration、Kd树Kd-tree、八叉树Octree、切分Segmentation、Sample Consensus、Surface、Range Image、文件读写I/O、Visualization、通用库Common、Search

PCL中的所有模块和算法都是通过Boost共享指针来传送数据的，因而避免了多次复制系统中已存在的数据的需要

ROS中已经安装了pcl，不必再安装了，而且网上给的那个源一直添加失败，服务器在国外。但是ROS的pcl缺工具包，比如查看点云图片的pcl_viewer。它的安装命令是：sudo apt-get install pcl-tools

优点:

• 可以表达物体的空间轮廓和具体位置

• 点云本身和视角无关，也就是你可以任意旋转，可以从不同角度和方向观察一个点云，而且不同的点云只要在同一个坐标系下就可以直接融合

缺点：

• 点云并不是稠密的表达，一般比较稀疏，放大了看，会看到一个个孤立的点。它的周围是黑的，也就是没有信息。所以在空间很多位置其实没有点云，这部分的信息是缺失的。

• 点云的分辨率和离相机的距离有关。离近了看不清是个什么东西，只能拉的很远才能看个大概。

安装

编译pcl-1.8

1
2
3
4
5
6

mkdir build
cd build
# 配置cmake
cmake -DCMAKE_BUILD_TYPE=None -DCMAKE_INSTALL_PREFIX=/usr \ -DBUILD_GPU=ON-DBUILD_apps=ON -DBUILD_examples=ON \ -DCMAKE_INSTALL_PREFIX=/usr ..
make
sudo make install

cmake配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

cmake_minimum_required(VERSION 2.6)
project(pcl_test)

find_package(PCL 1.7 REQUIRED)
# 可选的工具包
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_test main.cpp)
target_link_libraries (pcl_test ${PCL_LIBRARIES})

PCL中常见的PointT类型

PointXYZ是使用最常见的一个点数据类型，因为他之包含三维XYZ坐标信息，这三个浮点数附加一个浮点数来满足存储对齐，可以通过points[i].data[0]或者points[i].x访问点X的坐标值

1
2
3
4
5
6
7
8
9
10

union
{
	float data[4];
	struct
	{
		float x;
		float y;
		float z;
	};
};

PointXYZRGB也是个union，成员如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

union{
	float data[4];
	struct
	{
		float x;
		float y;
		float z;
	};
};
union{
	struct{
		float rgb;
	};
	float data_c[4];
};

在使用PCL库的时候，经常需要显示点云，可以用下面这段代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

#include <pcl/visualization/cloud_viewer.h>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr cloud(new PointCloudT);

cloud->is_dense = false;
// 打开点云文件，获取的是vertex
if(pcl::io::loadPLYFile("file.ply", *cloud) <0 )
{
	PCL_ERROR("loading cloud failed !");
	return -1;
}
cout<< "点云共有： "<< cloud->size()<<"个点"<<endl;   //文件中vertex的个数

pcl::visualization::CloudViewer viewer ("Viewer");
viewer.showCloud(cloud);

while (!viewer.wasStopped ())
{

}

变换点云

1

pcl::transformPointCloud (*cloud_in, *cloud_out, matrix);

cloud_in为源点云，cloud_out为变换后的点云，注意这里的matrix是4X4的欧拉矩阵，也就是第一行为R,t；第二行为0,1的矩阵

PCLVisualizer类与CloudViewer类

对于CloudViewer类来说，其方法有大致以下几类

1
2
3
4
5
6
7
8
9
10
11
12

void showCloud()

void wasStopped()  // 关闭窗口

void runOnVisualizationThread()
void runOnVisualizationThreadOnce ()

void removeVisualizationCallable()
// 键盘鼠标的响应
boost::signals2::connection registerKeyboardCallback()
boost::signals2::connection registerMouseCallback()
boost::signals2::connection registerPointPickingCallback()

常用代码：

1
2
3
4
5
6

pcl::visualization::CloudViewer viewer("3D Point Cloud Viewer");
// 除了显示什么都不能干
viewer.showCloud(m_cloud);
while(!viewer.wasStopped())
{
}

如果只是简单显示点云，调用CloudViewer就可以了。PCLVisualizer更加强大，可以改变窗口背景，还可以添加球体，设置文本，功能特别丰富

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);

int v1(0);
int v1(1);
// 视窗的x轴最小值，y轴最小值，x轴最大值，y轴最大值
viewer->createViewPort(0.0, 0, 0.5, 1.0, v1); //创建左视窗
viewer->createViewPort(0.5, 0, 1.0, 1.0, v2); //创建右视窗
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
// 代码最后，都会出现这样一个while循环，在spinOnce中为界面刷新保留了时间
while (!viewer->wasStopped())
{
	viewer->spinOnce(100);   // 多长时间调用内部渲染一次
	boost::this_thread::sleep(boost::posix_time::microseconds(100000));  //延时0.1秒
}

addPointCloud函数将点云添加到视窗对象中，并定义一个唯一的字符串作为ID号，利用此字符串保证在其他成员方法中也能标识引用该点云，多次调用addPointCloud()，可以实现多个点云的添加，每调用一次就创建一个新的ID号。如果想更新一个已经显示的点云，可以使用updatePointCloud函数。删除有removePointCloud(ID号)和removeAllPointClouds。 这里使用的是最简单的一个重载，此外可以在第二个参数添加PointCloudColorHandler或PointCloudColorHandler

PCLVisualizer类的键盘事件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

bool next_iteration = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing){
  if(event.getKeySym() == "space" && event.keyDown()){
      next_iteration = true;
  }
}

int main()
{
  pcl::visualization::PCLVisualizer *viewer;
  // 必须要给PCLVisualizer指针赋值实例化对象
  viewer = new pcl::visualization::PCLVisualizer("PCL Windows");
  viewer->registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
  
  while(!viewer->wasStopped())
  {
	viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds (100000));
    if(next_iteration)
    {}
  }
  return 0;
}

其他常用函数

1
2
3
4
5
6
7
8

// 初始化相机参数
initCameraParameters();
setCameraPosition(
        0, 0, 5,                                // camera位置
        0, 0, 4,                                // view向量--相机朝向
        0, 1, 5                                 // up向量
        );

还可以对不同的视窗添加文字和背景颜色

参考:
PCL库简要说明
PCL中可用的PointT类型
PCLVisualizer
PCLVisualizer可视化类

运行博物馆数据集

launch修改如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

<launch>
  <param name="/use_sim_time" value="false" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

如果只运行bag文件是没有tf信息的，其实是在urdf里面：

使用rosbag最好不要加倍播放，很可能出现tf的时间戳问题，可能是lookup时间导致

注意rviz需要有这三项

我的rviz插件的名称和官方不同，保证话题一样就行：

使用雷达online建图

我先拿思岚A2雷达测试，运行雷达的命令是：roslaunch rplidar_ros rplidar.launch，参见ROS中使用激光雷达(RPLIDAR)

复制/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files/revo_lds_.lua文件为no_odom_and_imu.lua，在它基础上修改22和23行:

1
2

tracking_frame = "base_footprint",   --原来是laser
published_frame = "base_footprint",  --原来是laser

1
2
3
4
5
6
7
8
9
10

<launch>
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename no_odom_and_imu.lua"
      output="screen">
  </node>
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

现在跟普通的launch文件不一样了，我们需要的是/home/user/catkin_ws/install_isolated/share/cartographer_ros中的launch和lua，此时还没有，可以直接复制过去或者重新编译：

1
2

cd ~/catkin_ws
catkin_make_isolated --install --use-ninja

现在运行roslaunch cartographer_ros home_map_no_odom.launch，效果如下：

启动后，不移动时，正常的建图日志

1
2
3
4
5
6
7
8
9
10
11
12

Extrapolator is still initializing.
[pose_graph_2d.cc:148] Inserted submap (0, 0).

[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0

[motion_filter.cc:42] Motion filter reduced the number of nodes to 4.2%.
[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0
scan rate: 13.27 Hz 7.54e-02 s +/- 2.37e-03 s (pulsed at 100.01% real time)

cartographer_node是各个节点或话题的枢纽。它订阅话题scan，有时会有imu（惯导数据），然后完成定位与子图的构建，并发布话题 submap_list。 submap_list这个话题被/cartographer_occupancy_grid_node订阅，用于构建子图占用的栅格地图。

保存地图

首先不再接受进一步数据

1

rosservice call /finish_trajectory 0

1
2
3

status:
  code: 0
  message: "Finished trajectory 0."

1
2
3
4
5

Shutdown the subscriber of [scan]
[ map_builder_bridge.cc:152] Finishing trajectory with ID '0'...
[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0

然后序列化保存其当前状态：

1

rosservice call /write_state "{filename: '${MAPS}/home.pbstream'}"

再将pbstream 转换为pgm和yaml

1

rosrun cartographer_ros cartographer_pbstream_to_ros_map  -map_filestem=${MAPS}/home  -pbstream_filename=${MAPS}/home.pbstream -resolution=0.05

1
2

[pbstream_to_ros_map_main.cc:50] Loading submap slices from serialized data.
[pbstream_to_ros_map_main.cc:70] Generating combined map image from submap slices.

其实可以不结束轨迹直接保存地图

报错

正式使用时，我在机器人工控机装好了 cartographer，在本地通过SSH启动，结果出现了报错：submapslist话题的md5值不同

建出来的图是这样的：

这是因为本机的rviz缺了一个插件，在编译carto时会装到rviz上，这就是一个棘手的问题，如果要在本机上开启rviz，就要在本机也把cartographer这一套编译通过，也就是需要装两次。如果本机安装不成功，还可以使用vncserver在远程机上启动rviz

参考：
安装过程

命名空间的三级参数如何设置

参数 /robot_pose 的设置

1
2

ros::NodeHandle  nh;
nh.getParam("robot_pose", robot_pose_ );

/move_base/controller_frequency的设置

1
2
3
4

// 在 move_base 节点内
ros::NodeHandle n("~/");
double control_frequency;
n.param("controller_frequency", control_frequency, 15.0);

参数 /move_base/Planner/linear_vel 的设置

1
2
3

// 在 move_base 节点内
ros::NodeHandle nh("~/" + string("Planner") );
nh.param("linear_vel", linear_vel_, 0.3);

1
2
3
4
5

ros::init(argc, argv, "my_node_name");
ros::NodeHandle nh1("~");
ros::NodeHandle nh2("~foo");
ros::Subscriber sub1 = nh1.subscribe("topic1", ...);
ros::Subscriber sub2 = nh2.subscribe("topic2", ...);

平时对nh，是用无参的构造函数，此时对应的话题是topic1。 加上~后，sub1订阅的话题是my_node_name/topic1。如果不是~，而是word，话题是/word/topic1

对于sub2，命名空间是~foo，相当于上面两种的组合，订阅的话题是my_node_name/foo/topic2

当然可以在launch文件修改命名空间的第一级，也就是 <node pkg="package", name="my_node", type="my_bin" />