函数名就是个指针,它指向函数代码在内存中的首地址.

1
2
3
4
5
6
7
8
9

typedef double (*FuncType) (int data);
double func(int num)
{
    return num;
}

// 调用
FuncType ptr = func;
cout << ptr(11) <<endl;

第一行定义了一种函数指针,类型为FuncType,它指向的函数返回为 double, 形参为一个int. 然后定义了一个名为func的函数.

FuncType ptr = func;是声明一个指针, 类型为 FuncType, 指向func函数. 接下来就可以拿ptr当函数用了

1
2
3
4
5
6
7
8

void test(int id, FuncType foo)
{
    cout << foo(110) << endl;
    cout << "id: "<<id<<endl;
}

//调用
test(1, static_cast<FuncType>(func) );

定义一个函数test, 它第二个形参是类型为FuncType的函数指针. 调用时, 最好对第二个形参做转换, 这里用static_cast再合适不过.

异常是运行期出现的情况，编译不会报错。如果出现异常，它后面的代码不会执行，一般会显示 The program has unexpectedly finished. </font>。如果能处理好异常，就可以让后面的代码继续运行

throw就是抛出异常，后面可以接任何语句表示异常。比如throw 123;, throw "exception";

try里面的第一个语句必须包含throw，可以是个函数。之后的语句不再进行，直接进catch了

catch的参数是和throw一致的，比如下面的const char*，如果要catch任何类型，小括号内换成...

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

double division(int a, int b)
{
   if( b == 0 )
      throw "Division by zero condition!";
   return (a/b);
}


try {
    cout << division(1,0) <<endl;
    cout << "本句不执行" <<endl;
}
catch(const char* msg)
{
  cerr << msg << endl;	// 输出错误用cerr
}

C++ 提供了一系列标准的异常，定义在，我们可以在程序中使用这些标准的异常。它们是以父子类层次结构组织起来的。可以使用catch(std::exception e)

一些第三方库也提供了异常，使用时要注意。比如log4cpp:

1
2
3
4
5
6
7
8

try
{
    log4cpp::PropertyConfigurator::configure(config_base_path+"setting.conf");
}
catch (log4cpp::ConfigureFailure& f)
{
    std::cout << "Configure Problem: " << f.what() << std::endl;
}

ELF文件有三种：可执行文件，so共享库，o目标文件

打印文件校验和

二进制文件传输过程中有没有被损坏或者是否是同一个版本，看看校验和以及程序块计数:

1
2

# md5sum liblidar.so
615f8ede92bb7cca3d559a46397474b6  liblidar.so

打印ELF文件中的可打印字符串 strings

例如你在代码中存储了一个版本号信息，那么即使编译成elf文件后，仍然可以通过strings搜索其中的字符串甚至可以搜索某个.c文件是否编译在其中：

1

strings elfFile| grep "someString"

nm命令查看函数或者全局变量是否存在于elf文件

nm命令用于查看elf文件的符号信息。文件编译出来之后，我们可能不知道新增加的函数或者全局变量是否已经成功编译进去。这时候，我们可以使用nm命令来查看。当然也可以用来查看函数，比strings命令更精确

查看文件段大小 size

可以通过size命令查看各段大小：

1
2
3

# size cmdTest
text       data      bss      dec      hex  filename
1319      560        8     1887      75f   cmdTest

text段：正文段字节数大小
data段:包含静态变量和已经初始化的全局变量的数据段字节数大小
bss段：存放程序中未初始化的全局变量的字节数大小
当我们知道各个段的大小之后，如果有减小程序大小的需求，就可以有针对性的对elf文件进行优化处理。

为elf文件瘦身 strip

strip用于去掉elf文件中所有的符号信息：

1
2
3
4
5

# ls -al cmdTest
-rwxr-xr-x 1 hyb root 9792 Sep 25 20:30 cmdTest #总大小为9792字节
strip cmdTest
ls -al cmdTest
-rwxr-xr-x 1 hyb root 6248 Sep 25 20:35 cmdTest#strip之后大小为6248字节

查看elf文件信息 readelf

readelf用于查看elf文件信息，它可以查看各段信息，符号信息等，readelf -h cmdTest是查看elf文件头信息：

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

Magic:   7f 45 4c 46 02 01 01 00 00 00 00 00 00 00 00 00  #elf文件魔数字
Class:                             ELF64  #64位 elf文件
Data:                              2's complement, little endian#字节序为小端序
Version:                           1 (current)
OS/ABI:                            UNIX - System V #
ABI Version:                       0
Type:                              EXEC (Executable file)#目标文件类型
Machine:                           Advanced Micro Devices X86-64 #目标处理器体系
Version:                           0x1
Entry point address:               0x400440  #入口地址
Start of program headers:          64 (bytes into file)
Start of section headers:          4456 (bytes into file)
Flags:                             0x0
Size of this header:               64 (bytes)
Size of program headers:           56 (bytes)
Number of program headers:         9
Size of section headers:           64 (bytes)
Number of section headers:         28
Section header string table index: 27

参考：

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

class Foo
{
public:
    void   operator() ()
    {
        cout <<"Foo operator"<<endl;
    }
    int operator() (int val)
    {
        return val*10;
    }
};

Foo f;
f();    // Foo operator
cout<< f(5) <<endl;  // 50

Foo是定义了调用操作符()的类,它的对象就相当于函数名,因此operator()取名叫函数对象

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

struct Data {
    double dist;
    double confidence;

    bool operator <= ( double d ) const
    {
        return(dist <  d);
    }
    bool operator <= ( Data& d ) const
    {
        return(dist <=  d.dist);
    }
};


Data a, b;
a.dist = 1;
a.confidence = 1;

b.dist = 1.3;
b.confidence = 1;
if(a <= 1.0)
       cout << " 00000 " << endl;
if(a <= b)
       cout << " 11111 " << endl;

大多数雷达都是TOF测量法，只有深度信息，没有相机那样的纹理信息，也就没有视觉SLAM的运算负荷。

目前常见的激光雷达都是旋转扫描式的，内部长期处于旋转中的机械结构会给系统带来不稳定性，在颠簸震动时影响尤其明显。固态激光雷达的逐步成熟可能会为激光SLAM扳回这项劣势。

激光雷达的使用寿命问题已经被解决，能够保证长时间使用不会出现故障。比如在连续工作情况下，RPLIDAR-A2的设计使用寿命可长达5年以上。

雷达的光线遇到大雾、烟尘会受到遮挡，影响性能。

测试材质

选用雷达需要判断雷达是否适用于自己的场合，所以根据需要在以下场景进行测试

• 大理石瓷砖。 应用场景参考：酒店大堂、走廊、室外墙壁

• 玻璃。 应用场景参考：玻璃门窗，办公大厅、玻璃柜台。 激光会穿过透明玻璃，从而造成一定概率的漏检。可以增加一些辅助反射手段，比如粘贴磨砂贴纸，或配合其他非光学的传感器作为补充。 雷达有时会穿透玻璃，有时不会，所以临时在玻璃上贴磨砂纸。

• 不锈钢板。 应用场景参考：电梯、生产车间、港口码头

• 反光条。 应用场景参考：医院、生产车间、酒店大堂

误差源

• 发送和接收激光束的精确耗时误差，也就是计时设备的精度问题

• 目标材质的反射值特性，比如全黑的材料吸收了光的大部分能量，使得反射量极低；或者像镜子一样的材料会将大部分光反射到其它地方

• 运动畸变：由于激光雷达在跟随自动驾驶车辆前进的同时，对周围环境进行扫描建模，也就是说车辆相对于周围的环境是运动的，导致对环境测量的实际位置与真实位置存在偏差。但是扫描频率高，速度低时，可以不必考虑。

测距范围

指雷达能够测量的距离范围。如果实际障碍距离超出最大值，那么雷达数据会标记为无效点（不是距离为0）。市面上2D雷达最近距离也至少几厘米

实际情况下，雷达测距的最大值有可能因为工作环境而产生变化。雷达要想测距，需要接收到反射的激光。所以官方在测距范围这一项上添加了备注：“基于白色70%反射率物体”。

如果是吸收激光比较厉害的物体，例如黑色的表面又几乎不反光的物体，会导致反射光强度很弱，那么距离稍微远点，可能就测距失败了，这时候，该物体即使在标出的12米范围内同样无法测出。针对类似这样的物体，相当于实际测距的最大值变小了。透明的玻璃也是同样原因。

不过因为不同的物体和环境差异太大了，所以厂商也不太可能将全部情况测试一遍，更多的时候需要靠自己来实验，看是否能够适用实际的工作环境。

扫描角度

思岚雷达是360°扫描的。有些雷达例如SICK的一些雷达，扫描角度只有220°。 实际使用，通常也不需要完全的360°，特别是雷达放在结构的中间层，因为有结构固定装置的存在，必然会有遮挡。

测距分辨率

分辨率和精度是两个不同的概念，按照上述参数的意思，更准确来说应该指的是测距精度。

RPLIDAR的精度并不是恒定的一个百分比，简单的解释是，距离越远，反射光受到的干扰越大，自然精度下降了。实际上，不同批次的雷达精度之间也有一定的差异。正因为这些不确定性，官方文档给的是较保守的值。

1.5m范围内小于0.5mm的精度还是可以的，1.5米处约为万分之三点三。
当在最大距离12米的时候，如果精度下降到最差的1%，则误差为0.12m，也能接受。

扫描频率

衡量雷达一秒钟能转多少圈，直接改叫雷达转速也是可以的。

转速实际上跟雷达数据更新周期是挂钩的，比如说典型的10Hz，那就是说转一圈的时间大概是100ms，那么雷达数据差不多也是100ms一帧。 要跟scan话题的发布频率区分开，后者跟计算机性能有关。

LMS1xx系列的扫描频率是25～50Hz，角度分辨率为0.25°～0.50°
LMS5xx系列的扫描频率是25～100Hz，角度分辨率为0.1667°～1°

雷达自身的旋转是有方向的，大部分雷达都是逆时针旋转，与ROS中规定的一样，也有少部分雷达是顺时针旋转的，只不过使用起来有点不方便。

角度分辨率

正常来说，雷达转一圈，这一圈得到的测量点是均匀分布的，每个点之间间隔的角度就是所谓的角度分辨率了。

角度分辨率越小说明雷达转一圈得到的点数越多。例如，角度分辨率是0.45，则一圈是800个点，角度分辨率是0.9，则一圈是400个点。

不过，实际的角度分辨率其实不一定是固定的，即两个点之间的间距不一定是相同的，不过都在给出的分辨率范围内。在ROS中，雷达数据的标准格式认为角度分辨率是固定的，为了符合ROS标准，雷达的ROS驱动实际上做了角度补偿，将输出点修正为均匀分布的。

数据的强度

激光雷达的激光点是有能量的，不同品牌激光点的能量也不同。当能量太小时，远距离情况下可能存在返回不了数据的情况。

可以等阳光或者使用光束照射到墙面上，激光雷达再去看被光照射到的墙面，对比这时的点云效果。可以用照度仪测量此时的光强度。倍加福雷达的点云效果在高强度情况下非常好，不愧是用于反光板的雷达。

数据的精度

这是最重要的一个指标，表示激光雷达的数据跳动情况。现在一般厂商的雷达的精度都是2%。也就是100m的情况下，点的跳动幅度为2cm。但是，实际感觉能达到这个精度的雷达不是很多。

multi-echo

multi-echo可以分析每个测量光束的两个回波信号，这样在雨雪天可以提供可靠的测量结果。一般激光打到玻璃上会有部分穿透，导致测量不准，multi-echo使激光从玻璃上返回来，还能从玻璃后面的墙上返回来。

有的雷达具备这种特性，比如SICK-LMS111

参考：
从零开始搭二维激光SLAM —- 激光雷达数据效果对比
LakiBeam1雷达

查找速度

对序列式容器，如果元素已经排好序，那么查找速度可以达到logN的时间复杂度；如果是无序，只能是N

对关联容器，底层是红黑树，总能达到logN

有在任意位置插入元素的需求； 大量添加新元素的需求

最好用list，不要使用vector, deque

元素的排序

遍历元素的时候，序列容器输出的顺序和插入的顺序是一致的，关联容器就不一定了

sort()函数是快速排序的分段递归版本

关联容器的插入删除效率一般比用其他序列容器高(list除外)，因为不需要做内存拷贝和内存移动

STL 标准库提供了 4 种关联式容器，分别为 map、set、multimap、multiset

set

set的元素有序不重复，而且能根据元素的值自动进行排序。set中的键值不能直接修改，只能先删除再插入。底层采用红黑树。

set不支持随机访问，只能使用迭代器去访问。由于set放入一个元素就会调整这个元素的位置，把它放到合适的位置，所以set中只有一个insert插入操作。

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

set<int> s;
s.insert(5);
s.insert(2);
s.insert(3);
s.insert(1);
s.insert(2);
s.insert(4);
cout<<"set 的 size："<<  s.size() <<endl;  // 5
cout<<"set 中的第一个元素是 ："<<  *s.begin()<<endl;  // 1
cout<<"set 中的最后一个元素是:"<<  *s.end()<<endl;   // 5
set<int>::iterator it;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}    // 1  2  3  4  5
//    s.clear();
s.erase(++s.begin());
cout << "       after erase begin"<<endl;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}	 // 1  3  4  5
cout<<"lower bound: "<<*s.lower_bound(5)<<endl;  // 5
cout<<"upper bound: "<<*s.upper_bound(5)<<endl;  // 4
if(s.empty())
    cout << "set is empty !" <<endl;

multiset底层也是红黑树，但允许有重复数据

map 和 unordered_map

map适合存储一个数据字典，并要求方便地根据key找value。Map节点有一个Key和Value两个元素，Key不重复，Value可以重复。map可以通过key改变value的值

底层也是红黑树，所有元素都是有序的，红黑树的每一个节点都代表着map的一个元素，因此它的插入删除查找的时间复杂度为O(logN)

map支持随机访问(at函数和[])，这是set没有的。

1
2
3
4
5
6
7
8
9

map<int, string> m;
m.insert(pair<int, string>(1, "a"));
m.insert(pair<int, string>(2, "b"));
m.insert(pair<int, string>(3, "c"));
m.insert(pair<int, string>(4, "d"));
m.insert(pair<int, string>(5, "e"));

cout << m.upper_bound(3)->second <<endl;   // 大于
cout << m.lower_bound(3)->second <<endl;   // 不小于

1
2

d
c

缺点： 空间占用率高，因为map内部实现了红黑树，虽然提高了运行效率，但是因为每一个节点都需要额外保存父节点，孩子节点以及红/黑性质，使得每一个节点都占用大量的空间

unordered_map内部实现了一个哈希表，因此其元素的排列顺序是杂乱的，无序的，但查找速度非常的快。 缺点：哈希表的建立比较耗费时间。

几个重要文件的启动顺序：

1. 通过/boot/vm进行启动 vmlinuz

2. init /etc/inittab

3. 启动相应的脚本，并且打开终端

   1 2 3 4 5

   rc.sysinit rc.d(里面的脚本) rc.local

4. 启动login登录界面

5. 登录，此时执行sh脚本的顺序，每次登录的时候都会完全执行的

   1 2 3 4 5 6 7 8 9

   /etc/profile.d/file /etc/profile /etc/bashrc /root/.bashrc /root/.bash_profile

这里就清楚了，如果不登录还要加载环境变量，就只能把环境变量放到rc.local里

添加开机启动项： sudo vim /etc/profile.d/apps-bin-path.sh， 在其中放入可执行文件

函数在stack上分配的空间在此函数执行结束时会释放掉，这样就产生了一个问题: 如果想将函数中此变量的值保存至下一次调用时，如何实现？ 最容易想到的方法是定义为全局的变量，但这样最明显的缺点是 破坏了此变量的访问范围 (使得在此函数中定义的变量，不仅仅只受此函数控制). 想要使用全局变量的之前应该先考虑使用 static

全局变量和静态变量的存储都放在内存的全局区

全局变量和全局静态变量的区别

• 全局变量默认是有外部链接性的，作用域是整个工程，在一个文件内定义的全局变量，可以在另一个文件中使用。比较规范的方法是：在A.h中声明，比如extern int a;，但不能赋值，否则报错。在A.cpp中定义，int a=1;。然后在B.cpp中使用，cout << a <<endl;

• 全局静态变量是显式用 static 修饰的全局变量，作用域仅在声明此变量的文件，其他的文件即使用 extern 声明也不能使用。这样即使两个不同的源文件都定义了相同名字的static全局变量，它们也是不同的变量。

1
2
3
4
5
6
7
8
9

void test_static()
{
    static int n=0;
    n++ ;
    cout << n <<endl;
}
    test_static();
    test_static();
    test_static();

运行结果是

1
2
3

1
2
3

静态局部变量有以下特点：

1. 该变量在全局数据区分配内存；
2. 静态局部变量在程序执行到该对象的声明处时，被首次初始化，即以后的函数调用不再进行初始化。即上面的static int n=0;
3. 静态局部变量一般在声明处初始化，如果没有显式初始化，会被程序自动初始化为 0；比如上面的n可以不初始化为0
4. 始终驻留在全局数据区，直到程序运行结束。但其作用域为局部作用域，当定义它的函数或语句块结束时，其作用域随之结束。
5. 它和全局变量的区别:全局变量对所有的函数都是可见的，而static局部变量只对定义自己的函数体可见。

把局部变量改变为static变量后是改变了它的生存期和内存中的存储区域，作用域其实不变。 把全局变量改变为static变量是改变了它的作用域，限制了它的使用范围。

在一个package中新建文件夹，第一个文件夹为urdf，用来放置模型文件。第二个文件夹叫做meshes，用来放置外观纹理，一般是通过三维软件建模完成后，导出并放置在meshes中，第三个为launch文件夹。第四个是config文件夹，功能包的配置文件以及rviz的显示配置文件。

我们要准备这样的launch文件：

1
2
3
4
5
6
7
8
9

<launch>                                               
    <param name="robot_description" textfile="$(find robot_description)/urdf/robot.urdf" />
    <!-- 设置GUI参数，显示关节控制插件，配合下面的joint_state_publisher使用 -->
    <param name="use_gui" value="true" />
    <!-- 根据关节状态，创建tf关系，并发布到tf tree中  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <!-- 发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</launch>

一个简单的模型，就是一个底盘带一个轮子：

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

<?xml version="1.0" ?>
<robot name="robot">
	<material name="Black">
		<color rgba="0.15 0.15 0.15 1"/>
	</material>
	<material name="Blue">
		<color rgba="0 0 0.8 1"/>
	</material>

	<link name="base_link">
		<visual>
			<origin xyz="0 0 0" rpy="0 0 0" />
			<geometry>
				<box size="0.5 0.3 0.01" />
			</geometry>
			<material name="Black"/>
		</visual>
	</link>

	<link name="left_front_wheel_link">
		<visual>
			<origin xyz="0 0 0" rpy="1.5707 0 0" />
			<geometry>
				<cylinder radius="0.1125" length="0.064" />
			</geometry>
			<material name="Blue"/>
		</visual>
	</link>

	<joint name="left_front_joint" type="continuous">
		<origin xyz="0.1375 0.182 0" rpy="0 0 0" />
		<parent link="base_link" />
		<child link="left_front_wheel_link" />
		<axis xyz="0 1 0" />
	</joint>
</robot>

使用check_urdf robot.urdf可以检查语法,但是偶尔也有漏掉错误的时候.

第一行的xml版本声明,开头不能有空格, 也不能加注释. 在urdf文件中不能加中文注释

joint_state_publisher 和 robot_state_publisher

joint_state_publisher节点读取robot_description参数,这个就是我们的urdf文件.

use_gui参数,布尔类型,默认false,决定是否显示GUI界面. 这个小工具把所有joint做成slider形式,供用户测试,对continuous的joint,slider范围是-π到π

发布话题/joint_states, 消息类型sensor_msgs/JointState,可以echo查看

robot_state_publisher订阅话题/joint_states, tf变换也是它发布的,它有一个参数use_tf_static,用于决定是否使用tf_static latched 静态变换的广播,而这个参数默认true.

一般写成launch形式:

1
2
3
4
5
6

<launch>
    <param name="robot_description" textfile="$(find robot_description)/urdf/robot.urdf" />
    <param name="use_gui" value="false" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</launch>

现在可以启动完整的机器人程序了，有了map坐标系，也就有了map->base_link转换，现在移动机器人，rviz里的模型就会跟着移动了。

问题

rviz里没有显示机器人的模型，发现终端有这样的错误：

在urdf文件中对某个约束改名为camera，对相应的STL文件也改名后，在本地机的rviz打开RobotModel，结果终端报错：

但是rviz里没报错，tf树是正常的。最后发现还是URDF文件里的STL路径没有改过来

urdf_reader.cc

cartographer中的urdf_reader.cc是从urdf文件读取所有的坐标系变换，返回std::vector<geometry_msgs::TransformStamped>类型，然后使用下面的代码发送TF变换

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

#include "urdf_reader.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "urdf/model.h"
#include "absl/strings/str_split.h"

  tf2_ros::Buffer tf_buffer;
  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
  const std::vector<std::string> urdf_filenames =
      absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty());
  for (const auto& urdf_filename : urdf_filenames) {
    const auto current_urdf_transforms =
        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
    urdf_transforms.insert(urdf_transforms.end(),
                           current_urdf_transforms.begin(),
                           current_urdf_transforms.end());
  }
  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
  if (urdf_transforms.size() > 0)
      static_tf_broadcaster.sendTransform(urdf_transforms);
  else
  	  ROS_WARN("no transform in urdf file !");

参考：
古月居的urdf教程
urdf文件编码导致的缺少tf变换