list|C++C++ 模板与STL

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的所有元素移除

在命名空间中用非成员函数取代成员函数|C++C++ 面向对象

这个就是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成员的函数数量，有较低的编译相依度。

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

内联函数|C++C++ 面向对象

内联函数最初的目的:

  1. 代替部分#define 宏定义
  2. 替代普通函数:提高程序的运行效率

如果一些函数被频繁调用，不断地有函数进入函数栈，会造成栈空间的大量消耗。因此引入了内联函数。

函数调用是有时间和空间开销的。执行一个函数之前要将实参、局部变量、返回地址等压入栈中，然后执行函数体中，执行完毕后还要让之前压入栈中的数据都出栈。如果函数体代码比较多，需要较长的执行时间，那么函数调用机制占用的时间可以忽略:如果函数只有一两条语句，那么调用机制的时间开销就就不容忽视。

内联函数是在编译阶段将函数展开，而不用将函数地址压入栈，避免调用的开销，但增大了可执行程序的体积。因此内联函数一般比较小，用于大量调用的场合。如果在类体内定义实现，那么就是内联函数；也可以在类内用inline声明，在体外实现。

其实上面这些还没触及到本质，还有以下几条:

  1. inline只是我们对编译器提出的一个申请，是否真按内联函数处理取决于编译器，比如开启优化选项时才考虑内联函数

  2. 函数不能有循环和递归，不能太大，否则仍是普通函数

  3. 编译器的内联看起来就像是代码的复制与粘贴，这与预处理宏是很不同的：宏是强制的内联展开，可能将会污染所有的命名空间与代码，将为程序的调试带来困难，所以最好用内联函数代替宏


  • 内联函数无法随程序库升级而升级，比如修改了一个内联函数，那么所有用到它的地方都要重新编译，而普通函数做修改后，程序只需要重新链接它，效率比重新编译高

  • 如果一个内联函数会在多个源文件中被用到，那么最好把它定义在头文件中，否则必须在调用该函数的每个文件中定义

  • 构造函数和析构函数不适合做内联函数，编译器在编译期间会给你的构造函数和析构函数额外加入很多的代码，像成员函数的构造和析构等代码，所以通常构造和析构函数比表面上看起来的要多，并不适合作为内联函数

内联函数和宏的区别：

  1. 内联函数展开在编译阶段，宏在预处理阶段

  2. 内联函数会经过编译器安全检查，宏定义的参数没有类型的概念，只有在宏展开以后，才由编译器检查语法，这就存在很多的安全隐患

详细内容看《Effective C++》条款30

总结Effective C++条款|C++C++ 面向对象

最近重读了《Effective C++》，对一些复杂的知识点会专门写文章分析，这里将一些小的知识点总结一下，但不会涵盖书里的所有内容。

条款2

  • 良好的用户自定义类型的特征是它尽量能与内置类型兼容

  • 宁可用编译器替换预处理器

  • 尽量多用const,而不是宏定义.宏属于模块化的设计概念,会破坏封装性

  • 常量可能比#define产生较小量的代码.

  • 如果类需要用到多个常量,不要用const数组,而是改用枚举

  • 对于宏形式的函数,最好用内联函数取代

条款3

  • 希望迭代器指向的东西不可改动,需要const_iterator

条款4

  • 永远在使用对象前初始化.对于int x;,有些编译器会将x初始化为0,有些却不会,所以无论之后有没有用到x,都要先对其初始化,比如int x=0;

  • 成员变量如果是const或引用,它们只能用成员列表初始化的方法进行初始化

条款5

  • 编译器自动生成的析构函数不是virtual,除非这个类的基类的析构函数是virtural

  • 编译器自动生成的4个函数都是public

  • 如果基类的copy构造函数和copy运算符是private,编译器不会为派生类生成它们

  • 派生类的copy构造函数可以尽量不定义

条款6

  • 定义uncopyable类是很好的禁止使用copy构造函数和copy运算符的方法，它的析构函数可以不是virtual,派生类不必以public继承它,也没有成员变量,也可以用于多重继承

  • 类的不可拷贝特性是可以继承的，例如凡是继承自uncopyable的类都不能使用copy构造函数和赋值运算符

条款7

  • 如果基类析构函数不声明为virtual，析构时不会调用派生类的析构，可查看原因分析

  • 如果一个类不做基类，就不要有virtual析构函数或其他虚函数，因为虚指针会增大类的体积。 反过来，只要一个类做基类，就要有virtual函数

  • 当类至少有一个虚函数时，为它声明virtual析构函数，否则编译器有报警

  • 对于抽象类，可以将析构函数声明为pure virtual析构函数

条款8

  • 析构函数里不要抛出异常，否则会导致不确定行为

条款9

  • 构造函数和析构函数中都不要调用virtual函数，因为基类构造时，virtual函数不会下降到派生类阶层，或者说此时的virtual函数还不是virtual函数

  • 对于存在多个构造函数的情况，为避免代码重复，要把同样的代码放到一个函数里，比如init

条款10

  • 赋值运算符(包括+= -= *=)的返回最好是return *this，这符合STL等标准库的风格

条款12

  • 自定义copy构造函数或运算符时，需要复制所有的成员变量，如果少复制了，编译器不会报警或报错

  • 派生类的copy构造函数或运算符，无法像平时那样对基类的private成员变量赋值，因为无法访问private的变量，只能显式调用基类的operator =， 比如： Base::operator=(obj);

条款15

  • 多使用智能指针shared_ptr，它可以返回原始指针，显式方法是get()函数，隐式方法是取指针操作符->

条款16

  • new和delete必须配对，[]必须都有或都没有。千万避免new没有[]，delete有[]，这会导致程序不停运行析构函数

条款18

  • 有时的形参可以用wrapper类型，而不是内置类型

  • 保证接口的一致，比如STL容器都有个size的成员函数

  • 可以让一些返回指针的函数返回智能指针，比如工厂函数

条款21

  • 函数内返回对象时，不要返回其引用。因为引用指向局部变量，而局部变量在函数退出前销毁，所以会出现无定义行为。

条款22

  • 成员变量应该都是private，将它们隐藏在函数接口的背后，如果放到public，直接使用成员变量会降低封装性。如果破坏了成员变量，会破坏太多的客户码

  • 成员变量的封装性和成员变量的内容改变时破坏的代码量成反比

  • protected变量被消灭时，所有用到它的派生类的代码都要破坏，所以封装性也很差

条款24

  • 若函数参数都要进行类型转换，应该使用非成员函数

  • 成员函数的反面是非成员函数，不是friend函数，friend应该尽量避免

条款26

  • 如果某个类的对象没有用到，就不应该声明，否则运行构造和析构函数都会耗费成本

  • 对象初始化用构造函数比=运算符的效率高

  • 对变量定义后应马上初始化，然后马上使用，中间不要隔太远

  • 变量定义在循环内比外面更好，后者造成作用域更大，对程序维护性不好

条款27

  • 代码中尽量避免dynamic_cast，它会降低效率

  • 避免连续的cast转型，尤其是dynamic_cast

条款32

  • public继承是一种is-a的关系，每一个派生类的对象也是基类的对象

  • 程序的错误最好能在编译期检测出来，而不是运行期

条款34

  • public继承涉及到函数接口继承和实现的继承

  • 纯虚函数实际上是可以提供定义的，这个用法比较罕见，知道即可

  • 虚函数使派生类继承了其缺省实现，但这可能造成危险

条款36

  • 不要重新定义继承而来的non-virtual函数，虽然没有错误，但违反了is-a原则，这种情况下干脆不要使用public继承
构造函数的成员初始化列表 (二)|C++C++ 面向对象

根据 Effective C++，类的成员变量最好 都用成员列表初始化 的方法进行初始化，对内置类型没什么不同，但对类对象会提高效率。 成员初始化顺序是其声明的顺序,不是初始化列表中的顺序,二者最好一致

构造函数内赋值成员变量，先调用默认构造函数，再调用赋值运算符。 如果用成员列表初始化， 只调用拷贝构造函数，提高了效率

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class Derived : public Base
{
public:
    Derived();
    Derived(Base obj)
    {
        m_b = obj;
        cout<<"derived constructor"<<endl;
    }
    ~Derived();
    Derived(const Derived& obj);
    Derived& operator=(const Derived& obj);

private:
    Base m_b;
};

实际调用:

1
2
3
Base b;
cout<<"----------------"<<endl;
Derived d(b);

运行结果:

1
2
3
4
5
6
7
8
9
10
11
12
13
base constructor
----------------
base copy constructor
base constructor
base constructor
base operator =
derived constructor
base destructor
----------------
derived destructor
base destructor
base destructor
base destructor

第一行是Base b的结果，第二行是运行构造函数时，b传递的参数副本，所以调用copy构造函数;
第三行是成员变量m_b的构造函数; 第四行是运行派生类的构造函数前先运行的基类构造函数;
第五行就是=运算符; 析构过程就简单了，不再分析



但实际中一般不这么用，Derived的成员变量会是Base* m_b，此时的调用是这样的:

1
2
3
4
5
Base *b = new Base();
Derived d(b);

delete b;
b = NULL;

结果是:

1
2
3
4
5
6
7
8
base constructor
----------------
base constructor
derived constructor
base destructor
----------------
derived destructor
base destructor

这样简单多了

const成员变量和成员函数|C++C++ 面向对象

  • 在类成员函数后面加const关键字用于声明const成员函数，它不能改变类的成员变量，也不能调用该类中普通的成员函数，最好不要涉及非const的成员变量

  • const成员变量只能通过成员列表初始化进行初始化，任何成员函数均不能改变它的值

  • 只能调用const成员函数访问const对象，那是它的唯一对外接口

  • 若不希望传入的实参对象被修改，应该将形参定义为const的指针变量或者引用

const成员函数的定义是这样形式：

1
int getConst() const;

前面不用加const了，否则会有报警：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
class Base
{
public:
	void setM(int M)	//setter都不能加const
	{
		m = M;
	}
	int getM() const
	{
		return m;
	}
private:
	int m;
}

const Base b;
b.setM(2);

getM就是常量成员函数，如果给setM加const，那么就会报错，因为它修改m的值。定义的常对象b只能调用getM，调setM会报错。

问题

1
2
3
4
5
6
7
8
9
10
template <typename T>
void registerNav2Action(const BT::BehaviorTreeFactory& factory, const string& action_name, const T& nav2_action)
{
    BT::NodeBuilder builder =
            [](const std::string & action_name, const BT::NodeConfig & config)
    {
        return std::make_unique<T>(action_name, config);
    };
    factory->registerBuilder<T>(action_name, builder);
}

代码会报错： error: passing ‘const xxx’ as ‘this’ argument discards qualifiers

在一个加了const限定符的成员函数中，调用了非const成员函数，报错的意思就是缺少限定符。

factory作为const引用，调用了非const函数 BehaviorTreeFactory::registerBuilder，所以报错

使用const引用做函数形参类型|C++C++ 面向对象

本问题是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

显然只有f的构造和析构过程，对于副本，只调用了useDerived函数，没有构造和析构，也没有copy构造。

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

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

显然副本的copy构造和析构都有了，但没有副本的构造函数，效率明显降低

避免对象切割问题( 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

此时如果传入f，会把它按Base对象处理，而丧失了派生类的特性，调用的还是基类版本的函数，这不是我们的目的。但是如果用const Base& obj，就会根据传入的类型处理，结果会调用派生类版本的函数，也就是体现了多态。 但是注意：形参为常引用时，只能调用const成员函数。

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

机器人运动模型|ROSROS机器人

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

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

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

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

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

用于规划的速度模型

IMU在ROS中的使用 (三) 标定内外参|ROSROS机器人

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

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

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

IMU在ROS中的使用 (二) 发布话题|ROSROS机器人

通过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