代价地图(二) 话题服务和部分源码

平常所说的costmap(代价地图)有下面三层,这三层组合成了master map(最终的costmap),供给路线规划模块使用。:

  • 静态层: 为了做全局规划,机器人需要一个超越其传感器的地图,以了解墙壁和其他静态障碍物的位置。 静态地图可以用SLAM算法生成,它表示了costmap中不会变化的大部分

  • 障碍层: 该层从诸如激光和RGB-D相机的高精度传感器收集数据,并将其放置在二维网格中,追踪障碍物。 ObstacleCostmapPlugin在二维层面标记和射线追踪障碍物,theVoxelCostmapPlugin是从三维追踪。

  • 膨胀层: 在每个致命障碍周围插入缓冲区,增加到costmap的值取决于离最近的障碍的距离(膨胀半径)。机器人肯定会发生碰撞的地点有致命的代价,而且周围的区域有一些小的非致命的成本。这些值确保机器人不会撞上致命的障碍物,也不愿太靠近

文件夹costmap_2d\plugins提供了这三层和voxel层的源文件,costmap_2d包中的costmap_plugins.xml定义了这几层和VoxelLayer。

我自己定义的障碍物也可以是一层,假如我不想让机器人通过一个freespace就可以自己插入个障碍物,主要的接口是costmap_2d::Costmap2DROS,在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap,各个层可以被独立的编译。写法模仿obstacle_layer.cpp,在CMakeLists里添加后,一起编译成liblayers.so

rotate_recovery, move_base, move_slow_and_clear, global_planner, clear_costmap_recovery(这个库又在move_base中调用), dwa_local_planner, base_local_planner都使用了libcostmap_2d.so这个库

src文件夹中的costmap_2d_markers.cpp, costmap_2d_cloud.cpp, costmap_2d_node.cpp各自编译成一个可执行文件,其余文件联合编译成库文件libcostmap_2d.so

costmap相关的话题和服务

省略每一层和两个代价地图的parameter_descriptions和parameter_updates。以parameter_descriptions 和 parameter_updates结尾的话题用于dynamic_reconfigure机制,不必关注。echo话题会只获得一次消息,几乎不占用带宽,用rostopic bw没有结果。

1
2
3
4
5
6
7
8
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint

# 局部代价地图的区别仅仅是没有静态层
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint

/move_base/global_costmap/costmap 的类型是 nav_msgs/OccupancyGrid,所以不能直接订阅,需要转为代价地图.

参数~<name>/always_send_full_costmap,   (bool, default: false). 如果为true,那么每次更新,完整的costmap都会发布到话题~<name>/costmap. 如果为false, 只有部分 costmap 更新时,会发布到话题 ~<name>/costmap_updates。 设置always_send_full_costmap为false后,带宽大幅减少

话题~<name>/costmap_updates, 类型map_msgs/OccupancyGridUpdate,the value of the updated area of the costmap

除了两个代价地图和每一层的set_parameters服务之外,服务就只有/move_base/clear_costmaps

服务set_parameters是由 Dynamic Parameters server创建,我在导航框架的源码里没找到这个关键词,怀疑它的发布在ROS源码里。

部分源码

move_base里初始化代价地图是使用的Costmap2DROS, move_base里没有直接使用过Costmap2D类的对象

1
2
3
4
5
6
7
8
9
10
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
// 只有两行,类成员变量赋值: stop_updates_ = true; initialized_ = false;
planner_costmap_ros_->pause();

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
// 省略... 初始化global_planner local_planner
// 很简单的代码,对各层地图插件调用activate函数,激活各层地图
planner_costmap_ros_->start();
controller_costmap_ros_->start();


代价地图(一) 基础配置

地图

代价地图的参数定义在三个文件里,分别用于全局代价地图,局部代价地图,通用代价地图(用于两个代价地图的参数)

movebase建立了两个costmap,其中`planner_costmap_ros是用于全局导航的地图,controllercostmap_ros`是用于局部导航用的地图。

costmap原点在左下角,OpenCV图片的原点在左上

通用代价地图

参数文件common_costmap_parameters.yaml,这些参数会影响两个代价地图:

1
2
3
4
5
6
7
8
9
10
11
12
13
footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]]
inflation_radius: 0.05 #before: 1.8 0.3 1.2
obstacle_range: 2.5
raytrace_range: 3.0
cost_scaling_factor: 0.18
observation_sources: scan
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true

map_type: costmap

  • footprint: 机器人底盘的轮廓,由一系列二维数据表示,单位为米,每个表示机器人边界的点,逆时针或顺时针方向都可以,轮廓一般是个矩形,机器人中心相当于[0,0]。本参数其实用于表示内切圆和外接圆,它们以一定方式膨胀障碍物以适应机器人。一般为安全起见,本参数比真实轮廓稍微大一些。

  • robot_radius: 与footprint并列,用于圆形底盘,这个设置就简单多了,就是个半径值,单位米

  • min_obstacle_height: 传感器读数的最小高度(以米为单位)视为有效。通常设置为地面高度。

  • obstacle_range: 设置机器人检测障碍物的最大范围,意思是说超过该范围的障碍物,并不进行检测,只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物

  • raytrace_range: 在机器人移动过程中,实时清除代价地图上的障碍物的最大范围,更新可自由移动的空间数据。假如设置该值为3米,那么就意味着在3米内的障碍物,本来开始时是有的,但是本次检测却没有了,那么就需要在代价地图上来更新,将旧障碍物的空间标记为可以自由移动的空间

全局代价地图

全局地图是从静态地图创建的,使用了后者的数据,只是全局地图将静态地图中的障碍物膨胀化, 依据机器人的足迹半径.

千万注意 pgm 地图文件,边界必须是黑边,而且只给一圈黑色像素不够黑,至少4个,否则不能产生代价地图。

pgm文件.png
生成代价地图.png

相应的参数文件global_costmap_params.yaml:

1
2
3
4
5
6
7
8
9
10
11
12
13
global_costmap:
global_frame: map
robot_base_frame: base_link # 或者 base_footprint
update_frequency: 2.0 #before: 5.0
publish_frequency: 2.0 #before 0.5
static_map: true
rolling_window: false # 与static_map相反
transform_tolerance: 0.5 # tf变换允许的最大时间

plugins:
- {name: static, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}

plugins表示你要在costmap加载哪几层,static,obstacles,inflation是不可缺少的。 这些层在common_costmap_parameters.yaml中定义,然后添加到全局和局部代价地图的yaml文件里。如果没有定义,就不能添加到后两者。

全局代价地图对应的话题是/move_base/global_costmap/costmap,注意类型是nav_msgs/OccupancyGrid
全局代价地图

加载代价地图后,可以在Rviz里看到基本参数信息

记录一个bug

全局代价地图如果正常生成,理应和地图的大小一样,是2000 x 2000

Costmap2D::worldToMap在刚启动时的打印结果
刚启动move_base

发导航命令后,代价地图的大小很奇怪地变为200, 原点变为(0,0)

1
2
3
4
5
6
7
8
9
10
11
Costmap2DR0S transform timeout. Current time: 1687663324.0445, global_pose stamp 321.6492, tolerance: 0.3000
[1687663324.0756730651]: Could not get robot pose, cancelling reconfiguration
[1687663324.6495552631]: MoveBase received move_base goal (48.000000, 58.000000), type: 3
[1687663324]: wx: 46.032079, wy: 46.095481, origin.x: 0.000000, origin_y_: 0.000000

[1687663324]: mx: 920, my: 921 , size_x_: 200, size_y_: 200, resolution.: 0.050000

[1687663324]: wx: 46.032079, wy: 46.095481, start_x_i: 920, start_y_i: 921
[1687663324]: The robot's start position is off the global costmap. Planning will always fail, you sure the robot has been properly localized ?
[1687663324.]: move_base failed to find a plan to point (48.00, 58.00) !
[1687663324.]: wx: 46.032090, wy: 46.095525, start_x_i: 920, start_y_i: 921

局部代价地图

局部规划器用局部代价地图计算出局部路径,局部代价地图是依据机器人的传感器数据创建的,与静态地图无关,它由用户给出宽度和高度。不管机器人怎么运动,它总在局部代价地图的中心,不再这个动态窗口内的障碍信息会drop

相应的参数文件local_costmap_params.yaml:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
local_costmap:
global_frame: odom # 局部代价地图运行的坐标系,也可以是map
robot_base_frame: base_link # 也可以是map
update_frequency: 5.0 # 局部代价地图的更新频率,太大会影响消耗CPU
publish_frequency: 5.0 # 局部代价地图更新的发布频率,如果机器人运动很快,需要增大
static_map: false # 只能是false
rolling_window: true # 只能是true,代价地图在一个rolling window内更新
width: 4 # rolling window的宽度,在rviz显示时会四舍五入
height: 4 # rolling window的高度,在rviz显示时会四舍五入
resolution: 0.02 # rolling window的分辨率
transform_tolerance: 0.5 # tf变换的最大延迟

plugins:
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: obstacles, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

加载局部代价地图成功后

局部代价地图的origin_x_和origin_y_(在costmap_2d.cpp中的成员变量)如图解释,显然随着机器人的运动会变化。

局部代价地图有一个更新的过程:

  1. 订阅传感器相关主题,根据接受的传感器数据更新代价地图
  2. 执行标记和清除动作,前者是向代价地图insert障碍信息,后者是从代价地图remove障碍信息。两种动作在障碍层中定义
  3. 计算每个网格的代价值
  4. 对每个包含障碍的cell进行障碍膨胀,从每个占据的cell按一定的膨胀半径向外扩张代价值

两个代价地图的设置中,膨胀层要放到所有层的最后,否则全局路径不是贴着膨胀层,而是障碍层。

Rviz的map包含3个Color Scheme,costmap, map, raw。若要增加主题,只能写一个Rviz插件或者修改Rviz的源码

参考:代价地图全面分析总结


cmake教程(四)ROS中的catkin_make

使用catkin_make编译时,有时编译内容太长,而错误在终端的上面,使用滑条仍有可能看不到。这时可以使用catkin_make > path/file.log将编译内容输出到某个文件,这样就容易查看了。 catkin本身不会存储编译过程的日志

catkin_package()

这个函数十分重要,观察CMakeLists就可以发现,它是唯一的非cmake函数,显然ROS的catkin_make就是靠它了,它的作用是指定功能包构建信息导出,在使用add_library()或者add_executable() 之前,必须调用此函数。

该函数有5个可选参数:

1
2
3
4
5
6
7
8
9
INCLUDE_DIRS  导出包含功能包的路径(即cflags)

LIBRARIES 项目中导出的库

CATKIN_DEPENDS 该项目所依赖的其他catkin项目

DEPENDS 此项目依赖的非catkin CMake项目

CFG_EXTRAS 其他配置选项

比如:
1
2
3
4
5
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp nodelet
DEPENDS eigen opencv)

这表示功能包文件夹中的“include”文件夹是导出头文件所在的位置。roscpp+nodelet是构建/运行此功能包需要存在的功能包,eigen+opencv是构建/运行此功能包时需要存在的系统依赖性。

catkin_package在/opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake文件中定义。devel和build文件夹内的内容基本都是由其生成的。

这个函数解析package.xml文件,提取出里面的参数,执行catkin_make时,会在workspace/devel/share里生成pkgConfig.cmake文件的,在cmake文件中find_package就会用到这些文件,这样其他的对这个软件包有依赖的程序就可以方便的使用了。

比如现在有sub这个包,编译之后会在devel/share/sub/cmake生成两个文件:pubConfig.cmake和pubConfig-version.cmake,然后到另一个包pub的CMakeLists里调用sub:find_package(sub),这样编译pub就通过了.假如把那两个文件删掉,然后使用catkin_make --pkg pub编译,就会报错:

对于build文件夹,生成是一些编译中的中间文件,比如用来存储一些环境变量之类的文件。这个文件夹意义不大。

在工程中显示其他源文件

新建CMake工程后,只有main.cpp,向目录添加文件foo.h和foo.cpp,但是它们不会在工程目录里显示,编辑CMakeLists如下

1
2
3
4
5
6
7
8
cmake_minimum_required(VERSION 2.8)

project(testStream)

include_directories(.)

# 或者add_library(Foo "foo.cpp")
add_executable(${PROJECT_NAME} "main.cpp" "foo.cpp")

然后执行cmake后就会显示

报错Policy CMP0046 is not set

Policy CMP0046 is not set
可以不用管,也可以按提示修改,最好先安装:sudo apt-get install libproj-dev

参考:Policy CMP0046问题


使用jsoncpp读写JSON

还可以用另一个库 nlohmann

从Github上下载后,使用老三步安装:

1
2
3
4
mkdir dir && cd dir
cmake ..
make
sudo make install

安装的路径如下:

在CMakeList里进行配置,别忘了最后链接库文件:-ljsoncpp

写JSON

1
2
3
4
5
6
7
Json::Value ret_json;

ret_json["msg"] = "Hello JSON";
ret_json["code"] = 123;
ret_json["ret"] = true;

cout << ret_json.toStyledString()<< endl;

运行结果:

1
2
3
4
5
{
"code" : 123,
"msg" : "Hello JSON",
"ret" : true
}

写JSON数组如下:

1
2
3
4
5
6
7
8
9
10
Json::Value ret_json,sub;
sub.append(Json::Value(123));
sub.append(Json::Value(456));
sub.append(Json::Value(789));

ret_json["msg"] = "Hello JSON";
ret_json["code"] = sub;
ret_json["ret"] = true;

cout << ret_json.toStyledString()<< endl;

结果为:

1
2
3
4
5
{
"code" : [ 123, 456, 789 ],
"msg" : "Hello JSON",
"ret" : true
}


欧拉角,旋转矩阵,旋转向量,四元数

欧拉角

参考欧拉角的理解

对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。参考系又称为实验室参考系,是静止不动的,而刚体坐标系则固定于刚体,随着刚体的旋转而旋转。

规定:XYZ坐标轴是旋转的刚体坐标轴;而xyz坐标轴是静止不动的实验室参考轴。

坐标系XYZ原与参考系xyz重合,旋转后,称xy平面与XY平面的交线为交点线,用英文字母N代表。zXZ顺规的欧拉角可以静态地定义如下:

  • α是x轴与交点线的夹角;

  • β是z轴与Z轴的夹角;

  • γ是交点线与X轴的夹角。


第一绕z轴旋转α,第二绕交点线(即X轴旋转后的轴)旋转β,第三绕Z轴旋转γ. 因此,此过程可分解为三个基本的旋转,从左到右依次代表绕着z轴的旋转、绕着交点线的旋转、绕着Z轴的旋转。即其旋转矩阵为

对于欧拉角,我们规定任何两个连续的旋转,必须是绕着不同的转动轴旋转。因此,一共有 12=3x2x2 种顺规。第一次旋转可以绕三个坐标轴中的任意一轴转动,有3种情况,第二次旋转可以绕除第一次旋转轴外的任意一轴转动,有2种情况,第三次旋转可以绕除第二次旋转轴外的任意一轴转动,有2种情况。

  • 经典欧拉角:z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y

  • 泰特-布莱恩角(Tait–Bryan angles):x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z

我们平时所说的roll, pitch, yaw 其实是泰特-布莱恩角,我简称之为TB角

可以看出两者的区别是:经典欧拉角的第一个旋转角度和第三个旋转角度都是围绕同一个轴的,而Tait-Bryan角使用三个轴上的旋转角度去表示

  • 内旋和外旋

同样的一个旋转,按旋转的坐标系又可分为内旋和外旋。

内旋是基于自身坐标系的旋转,旋转轴是动态的。内旋的旋转矩阵是按顺序右乘,

外旋是基于外部坐标系的旋转,旋转轴始终不变,也就是RPY。外旋的旋转矩阵是按顺序左乘。

所以欧拉角要知道旋转顺序和是否定轴旋转,而旋转矩阵和四元数则是一个姿态就对应的一个旋转矩阵或四元数。除了万向锁问题,这也是SLAM不常用欧拉角的另一个原因。

在使用欧拉角的场合,大多数情况下都是采取外旋 XYZ


转换

欧拉角和旋转矩阵

《SLAM十四讲》没有讲到这部分。坐标系A旋转到坐标系B,先绕Z轴旋转了yaw,这里遵循右手法则,大拇指朝坐标系方向,四指代表正向。或者说正向旋转是以沿X轴方向看,顺时针旋转为正向,否则为负向。那么相应的旋转矩阵为
绕坐标轴旋转的轴坐标是不会变化的,所以对应位置是1
之后绕旋转后坐标系的Y轴旋转了pitch,之后绕旋转后坐标系X轴旋转了roll,最终的旋转矩阵如下,即依次左乘相应的矩阵

典型举例

如图,大坐标系是base_link,小的是 camera

我们希望的坐标系关系是这样的

变化的过程是camera坐标系做内旋方式,也就是绕自身轴按照Z-Y-X的顺序旋转,即先绕自身轴Z,再绕自身轴Y,最后绕自身轴X,可得旋转矩阵(内旋是右乘)

判断欧拉角的正负: 使用右手系,旋转轴正方向面对观察者时,逆时针方向的旋转是正、顺时针方向的旋转是负。这里先绕Z轴转 -90°,再绕X轴转-90°,camera就能变成希望的样子。

最终的旋转矩阵

打开网站rotationconverter,在左上角的旋转矩阵输入计算的结果,在右下角出现XYZ对应的结果,而我们需要的是ZYX,即[ x: -90, y: 0, z: -90 ],这里并不是简单的把XYZ的结果倒序就行了。

我们要的欧拉角rpy就是 (-1.57, 0, -1.57)

旋转向量

旋转矩阵有几个缺点:SO(3)的旋转矩阵有9个量,但一次旋转只有3个自由度。因此,这种表达式冗余。而且对于旋转矩阵自身也有约束,它必须是正交矩阵,且行列式为1,这些约束会使求解变得困难。扩展到变换矩阵,就是用16个量表达了6个自由度的变换。

上面说的欧拉角是绕三个正交轴的依次旋转,其实任意旋转都可以用一个旋转轴和一个旋转角来表示。这只需要一个旋转向量即可,也就是说一个三维向量表示了旋转,再加上一个平移向量,这样就是6个自由度,没有冗余了。

四元数

Quaternion用一个冗余的变量解决了欧拉角的Singularity问题,在运算插值时也比较方便,因此四元数是程序中表示旋转最常用的一种表示方法

四元数——>旋转矩阵——>欧拉角 这个过程转出来的欧拉角,不可能是想要的。因为同一个四元数,可以用2个欧拉角来表示,而这个方法得到的结果有可能是用转角大于2π的方式表达的.

最好不要直接从四元数转欧拉角后,处理向量的旋转.

四元数加减法: 四元数的加法只需要将分量相加就可以了

四元数的共轭是让四元数的向量部分取负,记做 (w, -x, -y, -z)。 四元数和它的共轭代表相反的角位移,因为相当于旋转轴反向。

四元数的逆表示一个反向的旋转,定义为四元数的共轭除以它的模的平方,四元数和逆的乘积为实数 1。 如果是单位四元数,那么逆就是共轭

四元数的插值不能用简单的线性插值,而是用slerp。因为当时间t匀速变化时,代表姿态矢量的角速度变化并不均匀

Eigen::Quaterniond::Identity().slerp(t, q_last_curr)能够实现四元数的球面插值。
t ∈ [0, 1]为插值点,q_last_curr为两帧之间的旋转四元数,即针对两帧之间的旋转而线性插入一个四元数。

cartographer中重新实现的slerp

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
template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start,
const T* const end,
double factor)
{
// Angle 'theta' is the half-angle "between" quaternions. It can be computed
// as the arccosine of their dot product.
const T cos_theta = start[0] * end[0] + start[1] * end[1] +
start[2] * end[2] + start[3] * end[3];
// Avoid using ::abs which would cast to integer.
const T abs_cos_theta = ceres::abs(cos_theta);
// If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon]
// interval, then the quaternions are likely to be collinear.
T prev_scale(1. - factor);
T next_scale(factor);
if (abs_cos_theta < T(1. - 1e-5)) {
const T theta = acos(abs_cos_theta);
const T sin_theta = sin(theta);
prev_scale = sin((1. - factor) * theta) / sin_theta;
next_scale = sin(factor * theta) / sin_theta;
}
if (cos_theta < T(0.)) {
next_scale = -next_scale;
}
return {{prev_scale * start[0] + next_scale * end[0],
prev_scale * start[1] + next_scale * end[1],
prev_scale * start[2] + next_scale * end[2],
prev_scale * start[3] + next_scale * end[3]}};
}

参考:
四元数和欧拉角互相转换
欧拉角与旋转矩阵


Eigen(一) 安装配置 和 基础

Eigen是一个基于C++模板的线性代数库,安装后只需要包含头文件即可,它没有库文件,所以不需要链接,一般是INCLUDE_DIRECTORIES(/usr/include/eigen3)

安装:sudo apt-get install libeigen3-dev,这是最新版本

查看Eigen的版本:不能用命令查看,版本在Macros.h,在/usr/local/include/eigen3/Eigen/src/Core/util,可以执行命令: grep 'define EIGEN_....._VERSION\>' $(locate Macros.h | grep Core/util),结果是三个版本部分的宏定义。目前我所用为3.2.92

卸载Eigen:删除所有Eigen的文件即可

1
2
3
sudo rm -rf /usr/include/eigen3 /usr/lib/cmake/eigen3 /usr/share/doc/libeigen3-dev /usr/share/pkgconfig/eigen3.pc /var/lib/dpkg/info/libeigen3-dev.list /var/lib/dpkg/info/libeigen3-dev.md5sums
sudo updatedb
locate eigen

如果locate之后没有结果,那就是卸载成功了

要安装其他版本,就需要下载对应得deb安装,比如libeigen3-dev_3.2.0-8_all.deb,我使用gdebi安装时失败,报警 A later version is already installed ,最终发现还是要用dpkg安装,这一点以前没有发现,其他软件包应该也会有这样问题

矩阵Eigen::Matrix基本使用方法

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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#include <Eigen/Core>  // 核心文件
#include <Eigen/Dense> // 包含了常用函数,稠密矩阵的运算
#include <Eigen/Eigen>  // 稀疏部分

// 矩阵基本使用
Eigen::Matrix<float, 2, 3> m23;
m23.setZero(2,3); // 矩阵置为0,参数分别为行数和列数
cout << "After m23 initilized!" << endl;
cout<<m23<<endl; // 可以将矩阵放入标准输出流,输出结果就是矩阵形式,源码应当做了空格和换行处理
m23<<1,2,3,4,5,6;  // 矩阵赋值的常用方式
cout << "After m23 set value!" << endl;
m23(1,2) = 54;   // 单独给某元素赋值
// 输出元素的值
 cout<<m23(1,2)<<endl;


Eigen::Matrix<double,2,2> information;
information.fill(0.0); // 矩阵元素全赋值为 0.0

// 常用的类型Matrix3Xd和Matrix3Xf其实是动态矩阵,定义如下
// typedef Matrix< double, 3, Dynamic > Eigen::Matrix3Xd
// typedef Matrix< float, 3, Dynamic > Eigen::Matrix3Xf
// Eigen::Matrix3d 三阶方阵

Eigen::Matrix<float, 2, 3> m;
m.setRandom(2,3); // 矩阵用随机值初始化,但每次随机值似乎都是一样的
m = m23; // 将另一矩阵的值赋给m
m<<3,4,7,12,-2,10;
cout<<m23+m<<endl; // 矩阵加法
cout<<m23 * m.transpose()<<endl; // 矩阵乘法

Eigen::Matrix3d m33
m33<<1,6,3,4,12,5,7,8,9;
cout<<m33<<endl;
cout<<m33.rows()<<endl; // 行数
cout<<m33.cols()<<endl; // 列数
cout << "Column major input:" << endl << m33 << "\n";
// 获取第一列的元素
cout << "The first column is:" << endl << m33.col(0) << "\n";
cout << "The last column is: " << endl << m33.rightCols(1) << "\n";
cout << "The first row is: " << endl << m33.topRows<1>() << endl;
cout << "The last row is: " << endl << m33.bottomRows<1>() << endl;
// rowwise和colwise两个函数很奇怪,不要使用
cout<<m33.transpose()<<endl; // 转置矩阵
cout<<m33.sum()<<endl; // 所有元素的和
cout<<m33.trace()<<endl; // 矩阵的迹
cout<<10*m33<<endl; // 矩阵数乘
cout<<m33.isUnitary()<<endl; // 判断该矩阵是否是酉矩阵(矩阵各列是否为标准正交基)
cout<<m33.conjugate()<<endl; // 共轭矩阵
cout<<m33.adjoint()<<endl; // 伴随矩阵
cout<<m33.inverse()<<endl; // 逆矩阵,若不存在会报错
cout<<m33.determinant()<<endl; // 行列式结果,若不存在会报错

// 矩阵乘法, m23模板类型是float,需要转换,注意cast不能自动提示
Eigen::Matrix<double, 2, 1> m21 = m23.cast<double>() * v3d;

由于元素级运算在矩阵中很常见,所以Eigen对于Matrix其实内置了一些函数,均以cwise开头。mat.cwiseAbs()元素取绝对值、mat.cwiseSqrt()逐元素开根号、mat.cwiseMin()将两个矩阵中相应位置的最小值组成一个新矩阵

Eigen提供了一些基础的算法,如sum(),求所有元素和;prod(),求所有元素之积;mean(),求元素平均值;minCoeff(),最小元素;maxCoeff(),最大元素;trace(),求迹

Eigen::Isometry3d

Eigen::Isometry3d相当于一个四维矩阵

1
2
3
4
5
6
7
8
9
Eigen::AngleAxisd  v(M_PI / 3, Eigen::Vector3d(1,0,0).normalized() );
Eigen::Quaterniond q(1,0,0,0);
Eigen::Vector3d translate(1,2,3);
// 初始化为单位矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(translate);
// T.rotate(q.matrix() ) 或者 T.rotate(v)
T.rotate(q);
cout << endl << T.matrix() << endl;

rotate函数比较强大,可接受的参数有四元数、旋转矩阵、旋转向量

必须要调用 Isometry3d::Identity()对它进行初始化,后面的translate()和rotate()函数才有用。

稀疏矩阵

使用方式和普通矩阵不同

1
2
3
4
Eigen::SparseMatrix<double> hessian(2, 2);
// 注意稀疏矩阵的初始化方式,无法使用<<初始化
hessian.insert(0, 0) = 2.0;
hessian.insert(1, 1) = 2.0;

从子矩阵和向量拼接成新矩阵

比如现在有一个3X3的矩阵,一个行向量和一个列向量,二者的最后一个元素相同,要拼接成4X4的矩阵

mat = mat1.block(i, j, p, q); // 从矩阵 mat1 的 i 行 j 列开始获取一个 p 行 q 列的子矩阵
mat = mat1.block<p, q>(i, j); // 从矩阵 mat1 的 i 行 j 列开始获取一个 p 行 q 列的子矩阵(动态矩阵)

1
2
3
4
5
6
7
8
9
Eigen::Matrix4d m;
Eigen::Matrix3d rotation;
// 给 rotation赋值
m.block(0,0,3,3) = rotation;

Eigen::Vector4d Vcol, Vrow;
// 给 Vcol和Vrow 赋值
m.col(3) = Vcol;
m.row(3) = Vrow.transpose();

向量Eigen::Vector

1
2
3
4
5
6
7
8
9
10
11
12
// 常用这种方式,Xd 代表double型的任意行数向量,但后面的参数必须指定行数
Eigen::VectorXd v(3);
v<< 1,2,5; // 必须不多不少传入所有的元素值,否则报错
v(2) = 9; // 给第三个元素赋值


Eigen::VectorXd vv(12);
vv<< 1,2,3,4,5,6,7,8,9,10,11,12;
// 前三行
cout<< vv.block(0,0, 3,1).transpose() <<endl<<endl;
cout<< vv.block(3,0, 3,1) <<endl<<endl;
cout<< vv.block(6,0, 3,1) <<endl<<endl;

指定了3行,必须传入3个值,否则运行报错。

我们可以用Vector3f::UnitX(), Vector3f::UnitY 和 Vector3f::UnitZ()这三个函数表示三个方向的列向量,也就是输出结果为:

1
2
3
4
5
X   Y   Z

1 0 0
0 1 0
0 0 1

其实向量只是一个特殊的矩阵,但是Eigen也为它单独提供了一些简化的块操作,如下三种形式:

head tail segment

  • 获取向量的前n个元素:vector.head(n);

  • 获取向量尾部的n个元素:vector.tail(n);

  • 获取从向量的第i个元素开始的n个元素:vector.segment(i,n);

1
2
3
4
5
6
Eigen::Vector4d v(4); 
v<< 1,2,3,4;

cout << "head: "<<v.head(1) <<endl; // 1
cout << v.segment(1,2) <<endl; // 2 3
cout << "tail: "<<v.tail(1) <<endl; // 4

Vector 转 Matrix

1
2
3
4
Eigen::Vector3d v;
v << 1,2,3;
// 这个转换十分罕见
Eigen::Matrix<double, 3, 1> m = v.template cast<double>();

从Eigen::Vector构建对角矩阵,其中Vector的元素占据矩阵的主对角线

1
2
Eigen::Vector3d v(1,2,3);
Eigen::Matrix3d m = v.asDiagonal();

从多个Eigen::Vector拼接成Eigen::Matrix

1
2
3
4
5
6
7
8
Eigen::Vector3d v(3);
v<< 2.1, 4.9, 0.4;
std::vector<Eigen::Vector3d> vv;
for(int i=0;i<4;i++)
vv.push_back( v );
// 拼接成3X4的矩阵
Eigen::Map<Eigen::Matrix3Xd> matrix(&vv[0].x(), 3, 4); //首元素地址,行,列
cout << matrix <<endl;

Map类用于通过C++中普通的连续指针或者数组 (指针地址连续)来构造Eigen里的Matrix类,这就好比Eigen里的Matrix类的数据和raw C++array 共享了一片地址,也就是引用。

有个庞大的Matrix类,在一个大循环中要不断读取Matrix中的一段连续数据,如果每次都用block operation 去引用数据会太繁琐。于是就事先将这些数据构造成若干Map,那么以后循环中直接操作Map就行了。

Eigen::Map

Map类用于通过C++中普通的连续指针或者数组来构造Eigen里的Matrix类,这就好比Eigen里的Matrix类的数据和raw C++array 共享了一片地址,也就是引用。 比如有个API只接受普通的C++数组,但又要对普通数组进行线性代数操作,那么用它构造为Map类。 实际上Map类并没有自己申请一片空内存,只是一个引用

1
2
3
4
5
6
7
8
9
10
11
12
13
14
using DynamicArray = Eigen::Array<int, Eigen::Dynamic, Eigen::Dynamic>;

std::vector<int> cells{1,2,3,4,5,6,7,8,9};
for(auto& v: cells)
cout << v << " ";
cout << endl << endl;
// 维度不要超过 3x3,否则出现invalid数据
Eigen::Map<const DynamicArray> full_correspondence_costs(cells.data(), 3, 3);
cout << full_correspondence_costs <<endl;


int data[] = {1,2,3,4,5,6,7,8,9};
Eigen::Map<Eigen::RowVectorXi> m(data, 4);
cout << "The mapped vector v is: " << m << "\n";

运行结果:
1
2
3
4
1 4 7
2 5 8
3 6 9
The mapped vector v is: 1 2 3 4

参考:
Eigen矩阵库使用说明


ROS和ROS2的优缺点

ROS1的缺点

  1. roscore (或者说 master) 的问题:系统连续运行长了,master 莫名宕机,某个节点可能突然失效。单 master 结构不利于多个机器人组成的集群。
  2. ROS 有延迟:TCPROS+序列化的过程开销大,拷贝次数太多,
  3. 安装与运行体积较大:运行在配置较低的 arm 上会占用过多资源,对嵌入式设备不够友好
  4. 参数服务器机制设计不好:程序退出但 roscore 不退出时,参数没有更新,或者说还留在参数服务器里


a. ROS 2采用全新的架构,底层基于DDS(Data Distribution Service)通信机制,支持实时性、嵌入式、分布式、多操作系统,ROS2支持的系统包括Linux、windows、Mac、RTOS,甚至是单片机等没有操作系统的裸机。

b. ROS 2的通讯系统是基于DDS,取消了master,同时在内部提供了DDS的抽象层实现,有了这个抽象层,用户就可以不去关注底层的DDS使用了哪个商家的API。

c. ROS2基于DDS进行数据传输,而DDS基于RTPS的去中心化的通信框架,这就去除了对roscore的依赖,系统的稳定性强,对资源的消耗也得到了降低。

d. 由于ROS 缺少Qos机制,topic的稳定性与质量难以保证;ROS2则提供了Qos机制,对通信的实时性、完整性、历史追溯等功能有了支持,这便大幅加强了框架功能,避免了高速系统难以适用等问题。

e. ROS2中引入了节点生命周期管理的概念,各个节点的状态是可管理的。比如如果想从建图功能切到导航功能。在ROS1中,需要先将建图功能的程序杀干净,然后再启动导航程序。在这个场景里,大可让建图程序休眠,而不用杀掉。切换功能时只需要激活相应功能的节点即可。

ROS2的缺点

  1. DDS 过于复杂(最大问题):ROS2 的底层是 DDS(如 Fast DDS、Cyclone DDS)。问题在于配置复杂(QoS、discovery、domain),不同实现行为不一致,debug 非常困难。实际表现是同一套代码换个 DDS 行为就变了,有时能通信但收不到数据,或延迟异常。很多人误解 ROS2 不等于高性能。实际上默认配置可能比 ROS1 更慢,QoS / DDS 配不好延迟更大,只有调优后才更强。结论:ROS2 最大门槛不是 API,而是 DDS。

  2. 分布式“太真实”,导致不稳定感:ROS1 中心化简单粗暴;ROS2 去中心化是分布式系统。这带来节点发现有延迟、比如param list出结果太慢、node list 不一致、偶发通信异常等问题。本质上架构更先进,但行为不再“可预测”。

  3. QoS 很强,但很容易用错:QoS 是核心优势也是坑。常见问题如 publisher / subscriber QoS 不匹配会导致收不到数据,reliability / history 配错会让延迟飙升。在 ROS1 至少“能收到”,在 ROS2 可能“完全没数据”。

  4. 调试难度明显上升:ROS1 的 rostopic echo 和 rqt_graph 基本能看明白。ROS2 受 DDS 黑盒和 QoS 影响,很多问题表现为“看起来一切正常,但就是不工作”。

  5. ROS2 引入 Python 格式的 launch 和 lifecycle node,学习成本高


cmake教程(五)函数

调用cmake文件

比如在CMakeLists.txt中 include("${YOUR_CMAKE_DIR}/functions.cmake"),然后可以调用其中的函数。

target_include_directories

file GLOB_RECURSE

格式为 file(GLOB variable [RELATIVE path] [globbingexpressions]...)

典型应用:

1
2
file(GLOB_RECURSE cpps *.cpp)
message(STATUS "cpps: ${cpps}")

GLOB_RECURSE 与GLOB类似,区别在于它会遍历匹配目录的所有文件以及子目录下面的文件。

1
2
3
4
5
file(GLOB_RECURSE ALL_SRCS "*.cc" "*.h")
file(GLOB_RECURSE ALL_TESTS "*_test.cc")
file(GLOB_RECURSE ALL_EXECUTABLES "*_main.cc")
list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS})
list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES})

aux_source_directory

1
2
3
4
aux_source_directory(src SRCS)
list(REMOVE_ITEM SRCS "src/main.cpp")

MESSAGE(STATUS "${SRCS_BT_Actions}")

包含src目录中的所有文件,list又去掉了src/main.cpp

option

option(<variable> "<help_text>" [value])
对于value,不给定或给定其他值都默认 OFF

实际使用时可以作为开关使用

1
2
3
4
5
6
7
cmake_minimum_required(VERSION 3.5)
project(test_6)

option(OPENGL_VIEW "Enable GLUT OpenGL point cloud view" ON)
if(OPENGL_VIEW)
...
endif()

foreach

foreach与C++里的功能一样,语法略有不同

1
2
3
4
5
set(dirs abc foo nfk)         # 三个字符串加到dirs里面
message(STATUS "dirs: ${dirs}")
foreach(dir ${dirs})
message(STATUS "dir: ${dir}")
endforeach(dir ${dirs})

运行结果:

1
2
3
4
-- dirs:    abc;foo;nfk
-- dir: abc
-- dir: foo
-- dir: nfk


cmake教程(三)find_package和pkg_check_modules

find_package

find_package用于加载第三方库,可以将需要的部分指定为组件,例如

1
2
3
4
5
6
7
8
set(PACKAGE_DEPENDENCIES
cartographer_ros_msgs
eigen_conversions
geometry_msgs
urdf
visualization_msgs
)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})

  • REQUIRED可选字段:表示一定要找到包,找不到的话就立即停掉整个cmake。而如果不指定REQUIRED则cmake会继续执行。
  • COMPONENTS: 可选字段,表示查找的包中必须要找到的组件,如果有任何一个找不到就算失败,类似于REQUIRED,导致cmake停止执行。


使用Find_Package寻找模块时,每一个模块都会产生如下变量:

1
2
3
_FOUND
_INCLUDE_DIR
_LIBRARY or _LIBRARIES

如果_FOUND为真,需要把_INCLUDE_DIR加入到INCLUDE_DIRECTORIES中,_LIBRARY加入到TARGET_LINK_LIBRARIES中

module 模式

find_package将先到module路径下查找 Find<name>.cmake。首先它搜索 ${CMAKE_MODULE_PATH}中的所有路径,然后搜索 /usr/share/cmake-3.5/Modules.比如find_package(Boost)搜索的文件是/usr/share/cmake-3.5/Modules/FindBoost.cmake

如果在CMakeLists.txt中没有下面的指令:

1
set(CMAKE_MODULE_PATH  "Findxxx.cmake文件所在的路径")

那么cmake不会搜索CMAKE_MODULE_PATH指定的路径,此时cmake会搜索第二优先级的路径.

如果cmake需要的是Find<name>.cmake,但是没在路径找到,可以添加路径到环境变量:

1
set(CMAKE_MODULE_PATH  ${CMAKE_MODULE_PATH} "Findxxx.cmake文件所在的路径")

config 模式

如果按照module模式未找到,cmake将会查找 <Name>Config.cmake 或 <lower-case-name>-config.cmake 文件。cmake会优先搜索自定义的xxx_DIR路径。如果在CMakeLists中没有下面的指令:

1
set(xxx_DIR  "xxxConfig.cmake文件所在的路径")

那么cmake就不会搜索xxx_DIR指定的路径.而是到/usr/local/lib/cmake/xxx/中搜索,比如/usr/local/lib/cmake/yaml-cpp/yaml-cpp-config.cmake,如果还没有就失败了。一般情况下,对第三方库执行make install都会把cmake文件放到这个文件夹里,如果编译报错就需要找到文件然后手动复制过去,但是不是光复制xxxConfig.cmake,还要有xxxTargets.cmake,否则find_package就会报错:
缺abslTargets.cmake.png

上面两种添加路径到环境变量的方法要根据寻找的文件进行设置,比如我在QtCreator+Cmake中使用OpenCV,直接find_package(OpenCV REQUIRED)会报错 找不到OpenCVConfig.cmake ,先用locate尝试寻找FindOpenCV.cmake和OpenCVConfig.cmake,发现只有电脑上只存在后者,所以要用config模式: set(OpenCVDIR "/opt/ros/kinetic/share/OpenCV-3.3.1-dev")



按上面的方法一般都能寻找库成功,如果还是失败,就用下面这种 优先级最高的强制方法: </font>

1
find_package(OpenCV REQUIRED PATHS /usr/local/share/OpenCV NO_DEFAULT_PATH) 

相关的宏

cmake找到任意一个之后就会执行这个文件,然后这个文件执行后就会设置好一些cmake变量。比如下面的变量(NAME表示库的名字,比如可以用Opencv 代表Opencv库):

1
2
3
4
<NAME>_FOUND	# 布尔量
<NAME>_INCLUDE_DIRS or <NAME>_INCLUDES
<NAME>_LIBRARIES or <NAME>_LIBRARIES or <NAME>_LIBS
<NAME>_DEFINITIONS

我们可以在CMakeList中用下面代码检验find_package的结果:

1
2
3
4
5
6
7
8
9
10
11
find_package(but_velodyne  REQUIRED)
if (but_velodyne_FOUND)
MESSAGE (STATUS "definitions: ${ButVELODYNE_DEFINITIONS}")
MESSAGE (STATUS "include dirs: ${ButVELODYNE_INCLUDE_DIRS}")
MESSAGE (STATUS "lib dirs: ${ButVELODYNE_LIBRARY_DIRS}")

include_directories(${ButVELODYNE_INCLUDE_DIRS})
target_link_libraries (helloworld ${ButVELODYNE_LIBRARY_DIRS})
else()
MESSAGE (STATUS " but_velodyne not found")
endif(but_velodyne_FOUND)

如果cmake在两种模式提供的路径中没有找到对应的Findxxx.cmake和xxxConfig.cmake文件,此时系统就会提示最上面的那些错误信息。

ROS中的find_package

对于catkin_make,它会搜索ROS Package的安装目录,不必用set指定搜索目录.比如std_msgs对应的文件路径在/opt/ros/kinetic/share/std_msgs/cmake/std_msgsConfig.cmake.这两个文件是库文件安装时自己安装的,将自己的路径硬编码到其中。

ROS中常常需要多个package,比如roscpp,rospy,std_msgs。我们可以写成:

1
2
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)

这样的话,每个依赖的package都会产生几个变量,这样很不方便。所以还有另外一种方式:
1
2
3
4
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)

这样,它会把所有pacakge里面的头文件和库文件等等目录加到一组变量上,比如catkin_INCLUDE_DIRS,这样我们就可以用这个变量查找需要的文件了,最终就只产生一组变量了。


REQUIRED 参数:其含义是指是否是工程必须的,表示如果包没有找到的话,cmake的过程会终止,并输出警告信息。对应于Find.cmake模块中的 NAME_FIND_REQUIRED 变量。

COMPONENTS参数:在REQUIRED选项之后,或者如果没有指定REQUIRED选项但是指定了COMPONENTS选项,在它们的后面可以列出一些与包相关(依赖)的部件清单

编译ROS工作空间的问题

ROS的工作空间用的时间一长,就会创建很多package,有些package的编译又用到了其他的package,这时单纯使用catkin_make就会出现问题。比如package A用到了B,B里又用到了C。即使catkin_make --pkg也不行,因为它会把所有的package都处理一遍,此时处理到A时就会报错,会显示找不到B和C的cmake文件。它们的路径在devel/share/B/cmake

比较笨的方法就是在A的CMakeLists里先把B和C注释掉,同样在B的CMakeLists里把A注释掉,先用catkin_make --pkg C编译C,再依次编译B和A。

找了好长时间没找到很好的解决方法,最后只能预先编译好cmake文件放到SVN上,以后就算B和C的程序改变,也可以编译。

Qt库的情况

以上问题都还简单,问题是在ROS中调用Qt的情况,以Core模块为例,我们有下面的代码:

1
2
3
4
5
6
7
8
find_package(Qt5 COMPONENTS Core Xml)
target_link_libraries(bin Qt5::Core Qt5::Xml)

if(Qt5Core_FOUND)
MESSAGE(STATUS "##### ${Qt5Core_VERSION}")
MESSAGE(STATUS "##### ${Qt5Core_INCLUDE_DIRS}")
MESSAGE(STATUS "##### ${Qt5Core_LIBRARIES}")
endif(Qt5Core_FOUND)

运行结果是:

1
2
3
--  5.5.1
-- /usr/include/x86_64-linux-gnu/qt5/; /usr/include/x86_64-linux-gnu/qt5/QtCore; /usr/lib/x86_64-linux-gnu/qt5//mkspecs/linux-g++-64
-- Qt5::Core

首先注意:能使用Qt5的CMake最低版本是 3.1.0,Qt库的宏规则实际仍然符合cmake,不同的地方在于不需要`命令,只要target_link_libraries就足够,它会自动添加相应的include directories,compile definitions`等等

Qt5Core对应的cmake文件在Linux上有两个位置:

1
2
3
/home/user/Qt5.11.1/5.11.1/gcc_64/lib/cmake/Qt5Core/Qt5CoreConfig.cmake

/usr/lib/x86_64-linux-gnu/cmake/Qt5Core/Qt5CoreConfig.cmake

经过测试,CMakeLists中起作用的应当是第二个,应当是安装Qt时,在此目录生成了文件.这个路径与环境变量Qt5_DIR相近,在CMakeLists中用MESSAGE函数能看到:/usr/lib/x86_64-linux-gnu/cmake/Qt5

Boost

Boost比较特殊,cmake对它有特别照顾,使用命令cmake --help-module FindBoost可以看到极为详细的使用方法.使用Boost有时要加上REQUIRED COMPONENTS XXX,这是在搜索已经编译的库,但不会检查只有头文件的库.比如thread和system要加入COMPONENTS但asio不需要.

cmake中使用Boost的filesystem,thread模块:

1
2
3
4
5
6
7
find_package(Boost COMPONENTS system filesystem thread REQUIRED)

target_link_libraries(mytarget
${Boost_FILESYSTEM_LIBRARY}
${Boost_SYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
)

必须得加上system模块

pkg_check_modules

pkg_check_modules是 CMake 自己的 pkg-config 模块的一个用来简化的封装:你不用再检查 CMake 的版本,加载合适的模块,检查是否被加载,等等,参数和传给 find_package 的一样:先是待返回变量的前缀,然后是包名(pkg-config 的)。这样就定义了<prefix>_INCLUDE_DIRS和其他的这类变量,后续的用法就与find_package一致。

当安装某些库时(例如从RPM,deb或其他二进制包管理系统),会包括一个后缀名为 pc 的文件,它会放入某个文件夹下(依赖于系统设置,例如,Linux 为该库文件所在文件夹/lib/pkgconfig),并把该子文件夹加入pkg-config的环境变量PKG_CONFIG_PATH作为搜索路径。
pkg_check_modules实质上是检测系统中的 pkg-config 是否存在指定的 .pc 文件。

在我的电脑上执行echo $PKG_CONFIG_PATH,结果是:

1
/home/user/Robot/workspace/devel/lib/pkgconfig:/opt/ros/kinetic/lib/pkgconfig:/opt/ros/kinetic/lib/x86_64-linux-gnu/pkgconfig

在bash.rc里没有设置,但是能获得这个环境变量,这是因为我们的环境变量里设置了

1
2
source /opt/ros/kinetic/setup.bash
source /home/user/Robot/workspace/devel/setup.bash



在cmake中使用pkg_check_modules时,就会去上面的路径里搜索pc文件,例如

1
2
3
4
# bfl (Bayesian Filtering Library)是一个使用pkg-config的第三方库
# 先搜索cmake自己的PkgConfig模块,才能使用pkg_check_modules
find_package(PkgConfig)
pkg_check_modules(BFL REQUIRED orocos-bfl)

其中PkgConfig的路径在/opt/ros/kinetic/share/ros/core/rosbuild/FindPkgConfig.cmake

搜索对应库文件,发现在以下路径:

1
2
/opt/ros/kinetic/lib/liborocos-bfl.so
/opt/ros/kinetic/lib/pkgconfig/orocos-bfl.pc

现在我们可以获得对应的宏,使用这个库了:

1
2
include_directories(${BFL_INCLUDE_DIRS})
link_directories(${BFL_LIBRARY_DIRS})

参考:
CMake Manual For Qt
Boost的编译库列表


cmake教程(二) 库的使用

include_directories

参数应该是find_package调用生成的* _INCLUDE_DIRS变量以及需要包含的任何其他目录。如果您使用catkin和Boost,则include_directories调用应如下所示:

1
2
# 先调用find_package(Boost catkin),才能使用下面两个宏
include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})

include是目录名,不是命令,一般放头文件。使用message函数发现catkin_INCLUDE_DIRS是/opt/ros/kinetic/include/usr/include

不能直接include当前文件夹的头文件,这倒是奇怪。需要加一行include_directories(.)。另一种解决方法就是在add_executable里添加用到的头文件

添加库路径

1
LINK_DIRECTORIES(/usr/lib/) 

并不是必须的,如果能直接链接就不需要。

ADD_LIBRARY

生成库文件是ADD_LIBRARY,它的功能是用来指定库来构建。默认情况下,catkin构建共享库。

构建库如下PointManage:

1
2
3
4
5
6
7
add_library(PointManage src/point_manage.cpp)
target_link_libraries(PointManage
${catkin_LIBRARIES} -pthread -llog4cpp -lcgicc -lmysqlclient jsoncpp Qt5::Core Qt5::Xml
)

add_executable(point_node src/point_manage_node.cpp src/point_manage.cpp)
target_link_libraries(point_node ${catkin_LIBRARIES} -llog4cpp -lmysqlclient -pthread Qt5::Core Qt5::Xml)

库PointManage用到了Qt库,此时也需要加上target_link_libraries命令,编译才能成功。

add_library(lib a.cpp),如果a.cpp中使用了类Test,对应头文件test.h和源文件test.cpp。这种情况下,光include "test.h"不够,应当改成add_library(lib a.cpp test.cpp)

链接库

target_link_libraries必须放在add_executable(或者add_library)的后面。一般要先写好后者,然后再链接库:

1
2
3
4
5
add_executable(test "main.c")

target_link_libraries(test
${catkin_LIBRARIES}
) //必须跟上面的test同名,后者是很多共享库,大部分是Boost,显然ROS底层用的是Boost

但是注意,如果是生成动态库文件,最好不要再让它链接其他库文件,有时会失败。对于静态库,禁止链接其他库。

查看是否链接动态库成功,可以使用Linux的ldd命令

链接库时报警 缺函数.png
从报错内容可以看出是缺了distance函数,也就是要链接的so文件里没有distance函数

ALL_TARGET_LIBRARIES 变量

为了避免target_link_libraries后面跟很长一串库的名字,而且库增减的时候它也得跟着增减,我们在CMakeLists一开始定义一个变量: set(ALL_TARGET_LIBRARIES "")

然后在每个库对应的XX.cmake文件中,把库的名字合并到这个变量中去,list(APPEND ALL_TARGET_LIBRARIES ${XX_LIBRARIES}),这样在target_link_libraries就只使用ALL_TARGET_LIBRARIES这一个变量就好了。

install命令

安装网上下载的库时常常最后一步是make install,这是使用CMake的install函数完成的,该函数的参数:

1
2
3
4
TARGETS   目标安装
ARCHIVE DESTINATION 静态库和DLL(Windows).lib存根
LIBRARY DESTINATION 非DLL共享库和模块
RUNTIME DESTINATION 可执行目标和DLL(Windows)样式共享库

ROS的launch文件可以安装到${CATKIN_PACKAGE_SHARE_DESTINATION}:

1
2
3
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
PATTERN ".svn" EXCLUDE)

参考:
ROS CMakeLists Posted by G.J.先生
catkin_make的时候发生了什么