智能指针(二) shared_ptr

shared_ptr 内部包含两个指针,一个指向对象,另一个指向控制块,控制块中包含一个引用计数和其它一些数据。由于这个控制块需要在多个shared_ptr之间共享,即多个shared_ptr可以共享同一块内存,所以它也是存在于 heap 中的。

如果没有共享所有权的必要,就不必使用shared_ptr,而非unique_ptr

由于支持共享所有权,shared_ptr支持拷贝和赋值运算, 也支持移动。如果对象在创建的时候没有使用共享指针存储的话,之后也不能用共享指针管理这个对象了。

避免使用原生指针来创建shared_ptr指针

shared_ptr销毁对象的情况:

  1. 最后一个智能指针离开作用域
  2. 用其他的shared_ptr给一个shared_ptr初始化
  3. 最后一个智能指针调用 reset

shared_ptr的缺点:

  • 内存占用是原生指针的两倍,因为除了要管理一个原生指针外,还要维护一个引用计数

  • 使用了性能低的原子操作:考虑到线程安全问题,引用计数的增减必须是原子操作。 而原子操作一般情况下都比非原子操作慢

  • 两个shared_ptr可能出现循环引用,永远不能释放指向的对象

线程安全性

shared_ptr有两个成员,指向对象的指针ptr和管理引用计数的指针ref_count。引用计数本身是原子操作,是线程安全的,但 shared_ptr的赋值操作由复制对象指针和修改使用计数两个操作复合而成, 因此仍不是线程安全的。如果要从多个线程读写同一个shared_ptr 对象,还是需要加锁。

陈硕专门写了这篇文章分析这个问题,
也可以看我自己的这篇文章,子线程里能写shared_ptr指向的对象,回到主线程就变了。

尽量使用 make_shared()

为了节省一次内存分配,原来shared_ptr<Foo> x(new Foo); 需要为 Foo 和 ref_count 各分配一次内存,现在用make_shared()的话,可以一次分配一块足够大的内存,供 Foo 和 ref_count 对象容身。

常见用法

1
2
3
4
5
6
7
8
    int num = 12;
int* p = new int(num);
// shared_ptr<int> p1 = &num; // error
shared_ptr<int> p2 = boost::make_shared<int>(num);
shared_ptr<Foo> p3(p);

// cout << *p2 <<endl;
// cout << *p3 <<endl;

p1的用法是错的,p2和p3正确,但是不要同时使用,改用p3(p2)即可

1
2
3
boost::shared_ptr<Foo> a;
cout<< a.use_count()<<endl;
a->out();

执行a->out()会报错,原因是a没有指向对象,应该这样定义:boost::shared_ptr<Foo> a(new Foo());

再看这样的代码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
boost::shared_ptr<Foo> a(new Foo());            // 让a指向对象
cout<< a.use_count()<<endl; // 1

boost::shared_ptr<Foo> b = a; // 另一个指针也指向同一个对象
cout<< a.use_count()<<endl; // 2
cout<< b.use_count()<<endl; // 2

a.reset(); // 不执行析构函数,实际执行 delete a; a = NULL;
a->out(); // 报错
b->out(); // 正常
cout<< a.use_count()<<endl; // 0
cout<< b.use_count()<<endl; // 1
b.reset(); // 执行析构函数
cout<<"end"<<endl;

只有对象的引用计数为0的时候,才执行类的析构和free其内存:

1
2
3
4
5
6
7
8
9
boost::shared_ptr<Foo> a(new Foo());
boost::shared_ptr<Foo> b = a;

cout<<a<<endl;
cout<<b<<endl;

a.reset(new Foo()); // a重新初始化,指向另一个地址
cout<<a<<endl;
cout<<b<<endl;

运行结果:

1
2
3
4
5
0x99fc20
0x99fc20

0x9a0c70
0x99fc20

不要把一个原生指针给多个shared_ptr管理

1
2
3
int* ptr = new int;
boost::shared_ptr<int> p1(ptr);
boost::shared_ptr<int> p2(ptr);

这样做会导致ptr会被释放两次。在实际应用中,保证除了第一个shared_ptr使用ptr定义之外,后面的都采用p1来操作,就不会出现此类问题。

可以在标准容器里存储boost::shared_ptr,但不能存储std::auto_ptrboost::scoped_ptr,后两者不能共享对象所有权.

1
2
3
std::vector<boost::shared_ptr<int> > v; 
v.push_back(boost::shared_ptr<int>(new int(1)));
v.push_back(boost::shared_ptr<int>(new int(2)));

自定义删除器

默认情况下,shared_ptr调用delete()函数进行资源释放,即delete p;。但是如果shared_ptr指向一个数组而不是一个简单的指针,应该调用delete[] p,此时可以将一个回调传递给shared_ptr的构造函数来定制删除器。

主要是利用了构造函数template<class Y, class D> shared_ptr(Y * p, D d);,第一个参数是要被管理的指针, 与其它形式的构造函数一致; 第二个参数称为删除器, 他是一个接受Y*的可调用物, d(p)的行为应类似与delete p, 而且不应该抛出异常。有了删除器, 我们就可以管理一些更复杂的资源, 比如数据库连接, socket连接。

其实有很多种用法,只列举常用的普通函数法

1
2
3
4
5
6
7
8
9
Derived *d = new Derived[5];
boost::shared_ptr<Derived> p1(d, deleter);

// deleter函数定义
void deleter(Derived* d)
{
delete[] d;
cout<<"delete"<<endl;
}

参考:官方说明
boost智能指针之shared_ptr


上位机-ROS通信

因为要求在Windows上间接运行ROS的程序, 采用的通信方式是Web API. Web API可以使用任何类型的通信协议,数据交互格式为XML以及JSON。但主要是JSON,因为它比XML更加轻量,这就是使得JSON在解析速率方面更快,对带宽的要求更低。实际使用的还是Http的GET方式,所以把ROS程序做成CGI的形式, 放到mini-httpd的网络目录里供windows调用

Web API的客户端系统(调用者)和服务系统(提供者)彼此独立,调用者可以轻易的使用不同的语言(Java,Python,Ruby等)进行API的调用。

Web API的测试工具是POSTMAN, 在Windows和Linux平台下均有

这里要多说一些内容, 机器人上用到的Web API通信比较多,如果自己在文档上总结,不太方便且容易出错,看到仙知机器人使用Swagger总结的很好: RoboRoute Web API 使用手册

Swagger是一款RESTFUL接口的文档在线自动生成+功能测试功能软件,随着现在许多公司实现了前后端分离,swagger越来越受欢迎了。swagger是有两个版本的,而且区别还挺大的,一个是swagger-ui也就是swagger1;还有一个是springfox-swagger也就是swagger2. 推荐用前者.

Swagger UI 提供了一个可视化的UI页面展示描述文件。接口的调用方、测试、项目经理等都可以在该页面中对相关接口进行查阅和做一些简单的接口请求。该项目支持在线导入描述文件和本地部署UI项目。


Boost教程(一)安装配置和noncopyable类

查看Boost版本: grep BOOST_LIB_VERSION /usr/include/boost/version.hpp 或者 dpkg -s libboost-dev | grep 'Version'

Boost中有一个noncopyable类,它把copy构造函数和赋值运算符都声明为private,可以让自定义的类继承它

1
2
3
4
5
6
7
8
9
class noncopyable
{
protected:
noncopyable() {}
~noncopyable() {}
private: // emphasize the following members are private
noncopyable( const noncopyable& );
const noncopyable& operator=( const noncopyable& );
};

上面拷贝构造函数和赋值构造函数都声明为private,这样不论什么派生方式,子类对此都是无权访问的,从而达到禁止拷贝的目的。

构造函数为什么声明成protected呢? 首先肯定不能为private,不然无法构造子类实例。
如果为public,那么外部是可以创建noncopyable这么一个实例的,可是这个实例是完全没有意义的,我们用不到,该类只有在被继承之后才有意义。
所以此处声明为protected才是合适的,既保证外部无法直接构造一个无意义的noncopyable实例,又不影响构造子类实例。

但是如果派生类继承它之后,又定义自己的copy构造和赋值运算符,调用者还是能够通过赋值和copy构造等手段来产生一个新的对象,这种情况干脆不要继承noncopyable类


union和大端小端

大端: 低位的数据存放在高地址,高位的数据存放在低地址
小端: 低位的数据存放在低地址,高位的数据存放在高地址

现在的CPU一般都是小端. 网络编程中,TCP/IP统一采用大端方式传送数据,所以有时我们也会把大端方式称之为网络字节序

结合union理解这个概念比较容易,看下面程序:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
union my{
int a;
char bytes[4];
};

my un;
un.a = 135201034;
// a为4个字节, 二进制表示就是: 00001000 00001111 00000001 00001010
// 分别转化为十进制就是 8 15 1 10
int a0 = static_cast<int>(un.bytes[0]) ;
int a1 = static_cast<int>(un.bytes[1]) ;
int a2 = static_cast<int>(un.bytes[2]) ;
int a3 = static_cast<int>(un.bytes[3]) ;

cout<< "address a0: "<< &a0 <<" "<<a0 <<endl;
cout<< "address a1: "<< &a1 <<" "<<a1 <<endl;
cout<< "address a2: "<< &a2 <<" "<<a2 <<endl;
cout<< "address a3: "<< &a3 <<" "<<a3 <<endl;

联合体占了4个字节,对a赋值后,显然bytes数组要分担这4个字节的数据,这里为了表示清楚,把每个bytes又转为int(四个字节),我们知道数组的内存地址占用是连续的,而且第一个元素在低地址,依次为高地址。

运行结果:

1
2
3
4
address a0: 0x7fff768cc180  10
address a1: 0x7fff768cc184 1
address a2: 0x7fff768cc188 15
address a3: 0x7fff768cc18c 8

可以看出转换后,每个占了4字节,低地址的a0对应的是a的低8位,所以说这里是小端. 如果是大端, 地址和数据的对应关系就要倒过来.


智能指针(一) unique_ptr

智能指针三个优点:

  • 明确资源的所有权
  • 避免忘记delete,造成内存泄露
  • 对于 exception 情况,执行内存释放(Effective C++ 中的条款)

智能指针类重载了解引用运算符(*)和成员指向运算符(->),同时为了能够在堆中管理各种类型,几乎所有的智能指针都是模板类,包含其功能的泛型实现。

C++11 中的auto_ptr已经废弃. 现有的 unique_ptrshared_ptrweak_ptr和原生指针加起来构成了指针的完整四件套。它们都在头文件<memory>里面,最常用的是原生指针(没所有权语义的时候),其次是unique_ptr,后两个除非特定场合需求,能不用就不用。 拷贝shared_ptrweak_ptr都涉及到atomic操作,其开销比起拷贝、解引用一个指针都是大很多的。

unique_ptr

unique_ptr代表的是专属所有权,之所以叫这个名字,是因为它只能指向一个对象,即当它指向其他对象时,之前所指向的对象会被摧毁,不能进行复制操作只能进行移动操作。两个unique_ptr也不能指向一个对象. 看源码发现,拷贝构造函数和赋值运算符都加了delete:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
template <typename T, typename D = default_delete<T> >
class unique_ptr
{
public:
explicit unique_ptr(pointer p) noexcept;
~unique_ptr() noexcept;
T& operator*() const;
T* operator->() const noexcept;
unique_ptr(const unique_ptr &) = delete;
unique_ptr& operator=(const unique_ptr &) = delete;
unique_ptr(unique_ptr &&) noexcept;
unique_ptr& operator=(unique_ptr &&) noexcept;
// 省略 ...
private:
pointer __ptr;
};

强行使用会报错:

这里需要注意: 既然不能拷贝, 就不能在函数中将unique_ptr作为参数了,因为传参是一个产生副本的过程,用 move(unique_ptr)取代

  • unique_ptr内部存储一个原生指针,当unique_ptr析构时,它的析构函数将会负责析构它持有的对象,还可以使用自定义的 删除器
  • unique_ptr提供了operator*()operator->()成员函数,像 raw pointer 一样,我们可以使用*解引用unique_ptr,使用->来访问unique_ptr所持有对象的成员。
  • unique_ptr并不提供 copy 操作,但提供了 move 操作,因此可以用std::move()来转移unique_ptr, 把一个unique_ptr的内存交给另外一个unique_ptr对象管理,。转移之后,当前对象不再持有此内存,新的对象将获得专属所有权
  • C++14 提供了std::make_unique<T>()函数用来直接创建unique_ptr,但 C++11 没有

unique_ptr和原生指针的大小是一样的,内存上没有任何的额外消耗,性能是最优的

如果没有为unique_ptr指定对象,get()返回0

常用方法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
class Test
{
public:
Test() { cout<< "construct" <<endl; }
~Test() { cout<< "destruct" <<endl; }
};

std::unique_ptr<Test> p1(new Test());
cout<< "p1: "<<p1.get()<<endl; // 0x19b7c20
// std::unique_ptr<Test> p2(p1) // 错误用法, 不支持拷贝构造函数
std::unique_ptr<Test> p2 = std::move(p1);
// 转移所有权到p2
cout<< "p1: "<<p1.get()<<endl; // 0
cout<< "p2: "<<p2.get()<<endl<<endl; // 0x19b7c20

// p2释放所有权, 只剩p3原生指针
// release并不会摧毁其指向的对象,不执行析构,与reset不同
Test* p3 = p2.release();
cout<< "after release "<<endl;
cout<< "p2: "<<p2.get()<<endl; // 0
cout<< "p3: "<<p3 <<endl<<endl; // 0x19b7c20

move不执行析构,否则新的智能指针无法指向对象了.

reset

reset有两种用法

  • 如果不加参数,就会销毁对象(执行析构函数),重置智能指针

  • 如果加原生指针做参数,就会先销毁原来指向的对象,然后指向原生指针指向的对象

    1
    2
    3
    4
    5
    6
    7
    8
    Test* p3 = new Test();
    std::unique_ptr<Test> p4(new Test());
    cout<<"p4: "<<p4.get()<<endl;

    p4.reset(p3);
    cout<< "after reset "<<endl;
    cout<< "p4: "<<p4.get()<<endl;
    cout<< "p3: "<<p3 <<endl;

运行结果:

1
2
3
4
5
6
7
8
construct   // p3
construct // p4
p4: 0x7adc50
destruct //
after reset
p4: 0x7acc20
p3: 0x7acc20
destruct

需要注意: release不执行析构, reset执行析构

自定义删除器

unique_ptr的定义删除器方式和shared_ptr不同,因为模板的参数不同,前者还需要指定删除器类型.

原型有:

  • std::unique_ptr up(t,d);
  • std::unique_ptr up(d); // 空的指针

T为指针管理的对象类型, D为删除器类型, t为管理的对象, d为删除器函数名称

1
2
3
4
5
6
7
void myclose(Test* t)
{
cout<< "close func"<<endl;
}

Test t;
std::unique_ptr<Test, decltype(myclose)*> p1(&t, myclose);

运行结果:

1
2
3
construct
close func
destruct

decltype用于获取myclose的类型, *表面它是一个指针类型,即函数指针.

make_unique 不是‘std’的成员 , 原因是make_unique为C++14才特有的, 如果使用gcc版本小于6.2,编译就会报错,vs2015 msvc 也可以

参考:
深入 C++ 的 unique_ptr
C++ 智能指针的正确使用方式


链表程序

单链表程序如下:

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
struct LinkedList
{
int data;
LinkedList* next;
};

LinkedList* root, *second, *third;
root = new LinkedList;
second = new LinkedList;
third = new LinkedList;

root->data = 10;
root->next = second;
second->data = 20;
second->next = third;
third->data = 30;
third->next = nullptr;

LinkedList* guy = new LinkedList;
guy->data = 90;
guy->next = second->next;
second->next = guy;
while(root)
{
cout<< root->data <<endl;
root = root->next;
}


deque queue

deque可以看做是vectorlist的折中容器,类似vector,它支持随机访问(其实是假象),即支持[]以及at(),但是性能没有vector好。

deque和vector的区别:

  1. 在头部插入删除元素的时间复杂度是O(1)
  2. 没有容量的概念,所以没有reserve, capacity函数。因为它是动态的以分段连续空间组合而成,随时可以增加一段新的空间并链接起来,不像 vector 那样,当旧空间不足而重新配置一块更大空间,然后复制元素,再释放旧空间

deque还是不如list的任何位置插入删除,它的中间部分插入和删除元素和vector一样,时间复杂度还是O(n)。所以没有list的reverse函数

底层机制

这下问题就来了,既然没有reservecapacity,显然它和vector的底层机制不同。

deque采用类似索引的结构管理内存,采用一块所谓的map(不是数据结构的map )作为中控器,map是一块连续的内存空间,每个元素(称之为节点)都是指针,指向另一块连续的线性空间,称为缓冲区,map实际上是指针的指针T**
deque数据结构
deque的迭代器包含4个:

  • cur:迭代器当前所指元素

  • first:此迭代器所指的缓冲区的头

  • last:缓冲区尾

  • node:指向管控中心

deque的最初状态是有一个缓冲区,调用clear函数的作用就会清除整个deque,释放所有空间而只保留一个缓冲区。如果调用erase清楚某个元素,而这个元素是所在缓冲区唯一的元素,会将此缓冲区释放。

与vector不同,deque的内存大小是可缩减的。deque的内存区块不再被使用时,会被释放。

deque维护start和finish两个迭代器,分别指向第一缓冲区的第一个元素和最后一个缓冲区的最后元素的下一个位置。它还要记住当前map的大小,如果map的节点不足,就需要重新配置。

为了指定缓冲区的大小,必须指定 alloc 为空间配置器。比如指定数据int类型,缓冲区大小为4的deque: deque<int, alloc, 4>。 map的初始大小至少是8,最多是所需节点数+2,按刚才的例子,要存放50个元素,就需要13个节点,那么map的初始大小就是15。 如果插入的元素太多,map会不够大,需要扩充,和vector的内存扩充一样,先配置一块更大的,把原来的节点全拷贝过来,再释放原来的map。 对于平时的空间扩充,如果缓冲区中还有备用的空间,那么直接使用备用的空间;否则另外配置一个缓冲区,将其信息记录到缓冲区地址表里

在除了首尾两端的其他地方插入和删除元素,都将会导致指向deque元素的任何pointers、references、iterators失效。

特点

  • deque的元素存取和迭代器操作会比vector稍微慢一些,因为它的内部结构会多一个间接过程

  • deque迭代器是特殊的指针,而不是vector那样的一般指针,它需要在不同的区块之间跳转

  • 不支持对内存分配时机的控制,也就是没有reserve

  • 缓冲区的内存地址是连续的,但不同缓冲区之间不是连续的

  • 中间添加元素,会使迭代器、指针、引用失效

  • 头尾添加元素,现有元素的内存地址不变,指针和引用不失效,但迭代器可能失效,因为可能分配新的内存块

最好采用deque的情形

  • 需要在两端插入和删除元素

  • 随机访问元素的情况较少,主要是访问两端元素

  • 要求容器释放不再使用的元素

cartographer中的成员变量,凡是以queue结尾的,基本都是deque类型。 比如timed_pose_queue,它的使用只有back, front, pop_back, pop_front几个函数,根本没有用到遍历和随机访问

1
2
3
deque<std::unique_ptr<Constraint>>   constraints_;
constraints_.emplace_back();
auto* const constraint = &constraints_.back();

constraints_是个vector,可先添加一个空元素,然后再对最后一个元素进行赋值,其实为添加一个新元素(cartographer中很多都是如此做法,目的可减省一个变量空间和赋值时间)

queue

单向队列中的数据是先进先出,queue只是进一步封装别的数据结构,并提供自己的接口,如果不指定容器,默认是用deque来作为其底层数据结构的

单向队列一共6个常用函数front()back()push()pop()empty()size(),不支持随机访问,所以没有[]运算符和at

1
2
3
4
5
6
7
8
9
10
11
12
#include <queue>

queue<int> q;
q.push(1);
q.push(2);
q.push(3);
q.push(4);

qDebug()<< q.size() << " "<< q.empty();
qDebug() <<q.front() << " " << q.back() ;
q.pop();
qDebug() <<q.front() << " " << q.back() ;

1
2
3
4    false
1 4
2 4

list

list底层数据结构为双向链表,以结点为单位存放数据,结点的地址在内存中不连续,每次插入或删除一个元素,就增加或释放一个元素空间,占用内存比vector多

list不支持随机访问,适用于对象数量变化频繁,插入和删除频繁。随机访问元素的时间复杂度为O(n),插入删除为O(1),后两者只需要改变元素的指针

list删除迭代器时,其后面的迭代器都不会失效,将前面和后面连接起来即可。

list插入和删除数据,需要对现有数据进行遍历,但在首部插入数据,效率很高。

list的迭代器不是普通指针,因为节点在内存中不是连续存在的。它是一个结构体,内部有一个普通指针指向节点,实现递增递减、解引用、取值等运算符。

常用方法

直接赋值

1
2
3
4
5
6
7
8
9
10
11
std::list<int> list1={5,2,4,39,1};
std::list<int> list2={7,6,1,9};
cout<< list1.size()<<endl; // 5
list1 = list2; // 直接赋值
cout<< list1.size()<<endl; // 4

list<int>::iterator ite;
for(ite=list1.begin(); ite!=list1.end(); ite++)
{
cout<< *ite <<endl;
}

排序和反转

1
2
3
4
5
std::list<int> list1={5,2,4,39,1};
list1.sort();
print(list1); // 1 2 4 5 39
list1.reverse();
print(list1); // 39 5 4 2 1

merge函数有点问题,list1.merge(list2);应当是将list2与list1合并再排序,但是我发现list2插入到了list1中间的部分,把list1分割开了,可能是编译器的问题,最好还是不要使用。

其他

  • 对list中连续而相同的元素去重,移除到只剩一个,不连续的不处理

  • clear 函数的作用是清楚整个list的所有节点, 析构节点的对象,释放节点的空间

  • remove 函数的作用是将数值为value的所有元素移除


源码分析(一) 总体逻辑

文件amcl_node.cpp最主要的几个函数是构造函数,handleMapMessage, laserReceived。尤其是最后一个

杂项

sigintHandler回调函数在关闭程序之前只有一句amcl_node_ptr->savePoseToServer();,也就是关闭前把最近一次的机器人位姿存入参数服务器,这就可以理解为什么关掉程序后,用rosparam get获得的initial_pose_x等位姿仍然是以前的。

amcl根据输入参数来决定运行方式。从代码中可以看到amcl支持数据回放的,运行时只需要通过参数--run-from-bag指定bag文件即可

从main函数可以看出,amcl的所有业务逻辑都是由AmclNode类的构造函数完成

AmclNode构造函数

通过参数服务器,根据launch文件里的大量参数,对各类变量赋值

成员函数updatePoseFromServer,该函数就是用来从参数服务器上获取机器人的初始位姿和位姿误差的协方差矩阵的。

发布amcl_poseparticlecloud两个主题,分别用于输出机器人位姿估计和粒子集合。

接着注册三个服务:global_localization用于获取机器人的全局定位,request_nomotion_update则用于手动的触发粒子更新并发布新的位姿估计,set_map用于设定机器人位姿和地图信息。 这几个平时都用不到



接下来构建激光传感器的消息过滤器对象和tf2的过滤器,并注册回调函数laserReceived。

1
2
3
4
5
6
7
8
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(
*laser_scan_sub_,
*tf_,
odom_frame_id_,
100 );
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
this, _1));

这里的message_filter为ROS系统提供了一些通用的消息过滤方法, 它对接收到的消息进行缓存,只有当满足过滤条件后才输出,在需要消息同步的时候应用比较多。这里主要是同时监听激光扫描消息和里程计坐标变换,同步两者的输出。 这一套路在GMapping中也有用到。

订阅用于初始化机器人位姿估计的主题initialpose

根据运行参数use_map_topic_获取地图,或者订阅地图主题,或者通过requestMap请求static_map服务。因为一般取false,所以后面我们直接看requestMap函数,也就是如下逻辑:

定义m_force_update为false,后面会用到

最后定义一个计时器用于每隔15s检查一次激光雷达的接收数据,如果期间没有收到新的数据给出告警

requestMap 和 handleMapMessage

requestMap函数请求static_map服务成功后,调用函数handleMapMessage处理接收到的地图数据,进一步完成AmclNode的初始化工作,内容庞大的其实是handleMapMessage

handleMapMessage的流程图在这里

释放了与地图相关的内存和对象之后,通过函数convertMap将地图消息转换成amcl中的地图数据结构。

接着构建粒子滤波器对象,并完成初始化:

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
// 构建粒子滤波器对象
pf_ = pf_alloc(min_particles_,
max_particles_,
alpha_slow_,
alpha_fast_,
(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
(void *)map_ );
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

// 从参数服务器获取初始位姿及方差放到pf中
updatePoseFromServer();
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0]; // initial_pose_x
pf_init_pose_mean.v[1] = init_pose_[1]; // initial_pose_y
pf_init_pose_mean.v[2] = init_pose_[2]; // initial_pose_a

pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

// 初始化高斯粒子滤波器
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

这里面有两个重要函数pf_allocpf_init

构建里程计对象,根据launch文件里的参数配置里程计模型

构建雷达传感器对象,根据运行参数laser_model_type_构建不同模型的雷达

最后考虑到,在接收到地图数据之前,有可能已经接收到了机器人的初始位姿,这里调用函数applyInitialPose处理

pf_alloc

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
56
57
58
59
60
61
62
63
64
65
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
int i, j;
pf_t *pf;
pf_sample_set_t *set;
pf_sample_t *sample;

srand48(time(NULL));

pf = calloc(1, sizeof(pf_t));

pf->random_pose_fn = random_pose_fn;
pf->random_pose_data = random_pose_data;

pf->min_samples = min_samples;
pf->max_samples = max_samples;

// Control parameters for the population size calculation. [err] is
// the max error between the true distribution and the estimated
// distribution. [z] is the upper standard normal quantile for (1 -
// p), where p is the probability that the error on the estimated
// distrubition will be less than [err].
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
pf->current_set = 0;
for (j = 0; j < 2; j++)
{
set = pf->sets + j;

set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));

for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}

// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc(3 * max_samples);

set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));

set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}

pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;

//set converged to 0
pf_init_converged(pf);
return pf;
}

pf_init

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
// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
pf_pdf_gaussian_t *pdf;

set = pf->sets + pf->current_set;

// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);

set->sample_count = pf->max_samples;

pdf = pf_pdf_gaussian_alloc(mean, cov);

// Compute the new sample poses
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample(pdf);

// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}

pf->w_slow = pf->w_fast = 0.0;

pf_pdf_gaussian_free(pdf);

// Re-compute cluster statistics
pf_cluster_stats(pf, set);

//set converged to 0
pf_init_converged(pf);

return;
}

在命名空间中用非成员函数取代成员函数

这个就是Effective C++条款23,假如类有多个成员函数,现在需要在一个函数里调用这几个函数,那么最好不要把它定义为成员函数,而是类外的普通函数。

最常见的做法是定义一个namespace,把类和普通函数都放到里面,namespace可以跨越多个文件,但类不能。这正是C++标准库std的风格,std有数十个文件,每个声明std的部分机能,这样能 降低编译的依存性。

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
// 头文件
namespace test {

class Base
{
public:
Base();
~Base();
void setA(int a);
void setB(int b);
private:
int m_a;
double m_b;
};
// 这里声明,但不能定义函数体,否则编译不通过
void setAB(Base& obj, int a, int b);
}

// 源文件
test::Base::Base():
m_a(0),
m_b(0)
{
cout<<"base constructor"<<endl;
}

test::Base::~Base()
{
cout<<"base destructor"<<endl;
}

void test::Base::setA(int a)
{
m_a = a;
}

void test::Base::setB(int b)
{
m_b = b;
}
//必须加test:: ,否则编译不通过
void test::setAB(Base& obj, int a, int b)
{
obj.setA(a);
obj.setB(b);
}

// 调用
test::Base b;
test::setAB(b, 43, 23);

setAB函数在类Base之外,这样的封装性比作为成员函数要好,它并不增加能访问类private成员的函数数量,有较低的编译相依度。

类放到命名空间后,继承类可以像平常那样继承,不必也放到这个命名空间里。