DDS 基于 Topic 来实现发布-订阅模式，而且没有中心节点。是一种很好的端到端分布式通信中间件

DDS 本身并没有定义应该如何发现各节点，而是在 RTPS 中定义该行为。RTPS是 DDS 底层使用的协议，用来保证各种 DDS 的实现之间可以互操作。在 RTPS 中定义了两个独立的发现协议，分别是 PDP(Participant Discovery Protocol) 和 EDP(Endpoint Discovery Protocol)，PDP 用于发现各参与节点，EDP 用于发现每个节点提供的所有端点（Endpoints）

ROS2的发布订阅代码里，要指定QoS的历史深度

1
2

rclcpp::QoS::QoS(size_t  history_depth)
QoS(KeepLast(history_depth))

DDS 的发布者和订阅者是解耦的，即发布者不知道谁订阅它发布的数据，反之亦然。DDS 支持动态发现，即节点可以在不需要手动配置的情况下动态发现其他节点及其发布的主题。

QoS策略

无人机传图像到电脑端，可以用best effort模式，可以接受丢帧，但注重实时性。如果用手柄操作无人机，用reliable模式，不能丢失传送数据

DDS 的核心特性之一，它允许用户配置数据传输的行为。常见的 QoS 策略包括：

1. 可靠性 (Reliability)

RELIABLE：确保每条消息都被成功传输。如果消息未成功传输，会尝试重发。
BEST_EFFORT：没有重发机制，传输失败时不会重试，适用于对数据丢失容忍较高的场景。

1. 历史 (History)

KEEP_LAST：只保留最新的 N 条消息，队列满时会丢弃最旧的消息。
KEEP_ALL：保持所有历史消息，直到订阅者处理完所有消息。

1. 队列大小 (Depth)

设置消息队列的长度。如果队列满了，新的消息将根据历史策略被丢弃或覆盖。一般来说，depth 越大，存储的消息越多，可能增加延迟。

1. 截止时间 (Deadline)

设置消息传输的最大延迟时间。可以用于确保消息的时效性。

1. 寿命 (Lifespan)

设置消息的有效时间，超时后消息会被丢弃。

1. 可靠度（Reliability）和发送频率

控制消息的传输方式及频率，常用来减少带宽消耗。

• 优先级：为不同类型的数据设置不同的优先级。
• 持久性：确定数据是否会在订阅者连接之前持久保存。

• 数据过滤和选择（Content-Filtered Topics）

DDS 支持对订阅数据的 内容过滤，即订阅者只接收满足某些条件的数据。这对于减少不必要的数据传输和提高系统效率非常重要。

• 动态发现

DDS 节点能够自动发现网络中的其他节点和它们所发布的主题。这种机制使得在一个动态变化的系统中，节点可以不依赖静态配置而自动连接和通信。

==注意Fast RTPS和Fast DDS是同一个东西，不同的叫法

rmw的意思是ROS Middleware Interface

共享内存

ROS的底层通信都是基于XML-RPC协议实现的，以XML-RPC的方式传输数据存在一定的延时和阻塞。在数据量小、频率低的情况下，传输耗费的时间可以忽略不计。但当传输图像和点云等数据量较大的消息，或者执行有一定的实时性要求的任务时，因传输而耗费的时间就不得不考虑。Nodelet包就是为改善这一状况设计的，它提供一种方法，可以让多个算法程序在一个进程中用shared_ptr实现零拷贝通信，以降低因为传输大数据而损耗的时间。将多个node捆绑在一起，使用pluginlib管理，使得同一个manager里面的topic的数据传输更快。

ROS2提供借用消息，允许用户的程序从RMW实现借用消息内存，以消除ROS2应用程序和RMW实现之间的数据复制。此外，rmw_fastrtps通过Fast DDS提供共享内存传输和数据共享传递机制，以加快主机内通信。结合这两个功能（消息借用和数据共享），可以实现零拷贝消息传递管道，从而显著提高ROS2应用程序的性能。

在 rclcpp 中，loaned_message 是一种用于高效消息传递的机制。它允许节点直接从订阅者队列中借用一个消息，而不是复制它。这种机制在处理大型数据或需要高性能的场景中非常有用，因为它可以减少内存拷贝和相关的开销。使用完毕后，应该尽快归还消息，以便订阅者队列可以继续处理其他消息。

默认情况下，rmw_fastrtps_cpp和rmw_fastrts_dynamics_cpp都使用共享内存传输进行主机内通信，同时使用基于网络的传输 （UDPv4）进行主机间消息传递。

为了实现零复制消息传递，应用程序需要启用快速DDS数据共享机制，并使用Loaned Messages API：

要在Iron Irwini或更高版本中启用借用消息，唯一的要求是数据类型为纯旧数据。对于Humble，除了POD类型外，还需要启用快速DDS数据共享。

要启用快速DDS数据共享传递机制，需要加载以下XML配置文件，并且需要将环境变量RMW_FASTPS_USEQOSFROM.XML设置为1

但是FASTDDS需要配置完成零拷贝通讯

参考: # ROS2 DDS中间件

节点有四个主要状态：

1
2
3
4

Unconfigured
Inactive
Active
Finalized

1
2
3
4
5
6

Configuring
CleaningUp
ShuttingDown
Activating
Deactivating
ErrorProcessing

有7个函数用于监督流程，它们是：

1
2
3
4
5
6
7

create
configure
cleanup
activate
deactivate
shutdown
destroy

ros2 lifecycle 系列命令

• 获取节点状态

ros2 lifecycle get /my_lifecycle_node. 返回 unconfigured [1]

• 转换节点的状态

转换为configure状态: ros2 lifecycle set /my_lifecycle_node configure

除了configure，还可以是 cleanup, activate, deactivate, shutdown

• 获取节点状态转换时的信息

ros2 topic echo /my_lifecycle_node/transition_event

比如上面的状态转换过程，话题transition_event会输出

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

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 1
  label: unconfigured
goal_state:
  id: 10
  label: configuring
---
timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 10
  label: configuring
goal_state:
  id: 2
  label: inactive
---

状态转换必须按顺序，比如unconfigured，如果直接set activate，会失败，然后有提示信息。

• 获取当前状态可转换的目标状态

ros2 lifecycle list my_lifecycle_node

• 列出当前所有的生命节点

ros2 lifecycle nodes

service 获取当前节点的状态

ros2 service call /my_lifecycle_node/get_state lifecycle_msgs/srv/GetState

/change_state - 调用触发合法转换

/get_available_transitions - 显示合法的转换

/get_available_states - 列出所有状态

/get_transition_graph - 显示完整状态机

有用的函数

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

const State &   get_current_state ()
  Return the current State

 // 获得state的名称
std::string State::label() const


std::vector< State >  get_available_states ()
  Return a list with the available states
 
std::vector< Transition > get_available_transitions ()
  Return a list with the available transitions
 
const State&  trigger_transition (const Transition &transition)
  Trigger the specified transition

• hypot()用来求三角形的斜边长，其原型为：double hypot(double x, double y);，需要#include <stdio.h>

• fabs函数是求绝对值的函数，函数原型是extern float fabs(float x)，需要#include <math.h>

对double/float数据，一定要使用fabs函数。如果用了abs，就会出现bug，因为返回是int

• 反正切函数 atan2

atan2返回给定的 X 及 Y 坐标值的反正切值。反正切的角度值等于 X 轴与通过原点和给定坐标点 (Y坐标, X坐标) 的直线之间的夹角。结果以弧度表示并介于-pi到pi之间（不包括-pi）。 而atan(a/b)的取值范围介于-pi/2到pi/2之间，不包括±pi/2

• std::sin 等三角函数

• std::fmod

计算两个浮点数相除的余数

1
2

double x = 7.5, y = 2.1;
double result = std::fmod(x, y);    // 1.2

floor, ceil, round

std::floor 和 std::ceil都是对变量进行取整，只不过取整的方向不同。 std::floor是向下取整数，std::ceil是向上取整数。
比如输入3.6，前者输出3，后者输出4。但输入3.2，结果不变。

std::round 才是四舍五入

找最大最小

std::min(const T& a, const T& b); 求两个参数的最小值

std::max(const T& a, const T& b); 求两个参数的最大值

以下库函数需要 #include <algorithm>

minmax_element找出容器中最小和最大元素的迭代器，作为std::pair返回。时间复杂度为 O(n)

1
2

template< class ForwardIt >
std::pair<ForwardIt,ForwardIt> minmax_element( ForwardIt first, ForwardIt last );

1
2
3
4
5
6
7
8

std::vector<int> v = { 1, 2, 5, 4, 100, 0, -199, 33 };

auto result = std::minmax_element(v.begin(), v.end());
// 输出首次出现的最小元素
std::cout << "min element is: " << *result.first << '\n';

// 输出首次出现的最大元素
std::cout << "max element is: " << *result.second << '\n';

C++17 增加了min_element返回迭代器位置， 复杂度为 O(n)。 max_element返回迭代器位置，复杂度为 O(n)

1
2
3
4
5
6
7
8
9

std::vector<int> v{3, 1, 4, 1, 5, 9};

std::vector<int>::iterator min = std::min_element(v.begin(), v.end());
std::cout << "min element at: " << std::distance(v.begin(), min) << std::endl;
std::cout << "min value is: " << *min << std::endl; 

std::vector<int>::iterator max = std::max_element(v.begin(), v.end());
std::cout << "max element at: " << std::distance(v.begin(), max) << std::endl;
std::cout << "max value is: " << *max << std::endl;   

判断 inf, nan

1
2
3
4
5

bool isinf( float arg );

bool isfinite( float arg );

bool isnan( float arg );

numeric_limits

模板类，常用于提供很大很小的极值，需要#include <limits>

1
2
3
4
5

cout<<std::numeric_limits<int>::max()<<endl;
cout<<std::numeric_limits<long>::max()<<endl;

cout<<std::numeric_limits<int>::min()<<endl;
cout<<std::numeric_limits<long>::min()<<endl;

结果

1
2
3
4

2147483647
9223372036854775807
-2147483648
-9223372036854775808

std::optional

std::optional<T>代表一个可能存在的T值，实际上是一种Sum Type。常用于可能失败的函数的返回值中，比如工厂函数。在C++17之前，往往使用T*作为返回值，如果为nullptr则代表函数失败，否则T*指向了真正的返回值。但是这种写法模糊了所有权，函数的调用方无法确定是否应该接管T*的内存管理，而且T*可能为空的假设，如果忘记检查则会有SegFault的风险。

使用指针和optional的代码如下：

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

#include <optional>

// c++17 更安全和直观
std::optional<int>  func(bool  param)
{
    if(param)
    {
        return 1;
    }
    return nullopt;
}

int*  func2(bool  param)
{
    int p = 1;
    int* ptr = &p;
    if(param)
    {
        return ptr;
    }
    return nullptr;
}


auto f = func(false);
if(f.has_value() )
    cout << f.value() << endl;

auto f2 = func2(false);
if(f2)
    cout << *f2 << endl;

variant

std::variant 是 C++17 引入的一种类型安全的联合体，用来存储多个可能类型中的一种值，且保证使用时的类型安全。相比于传统的 union, std::variant不仅能够存储不同类型的值，还能自动管理复杂类型的构造与析构。

使用 std::variant 可以定义一个变量，该变量可以持有多种不同类型的值，但一次只能存储一种。std::variant 提供了一种类型安全的方式来处理多种类型，可以用在函数参数中以接受这些类型。

构造和赋值：您可以直接使用各种支持的类型初始化 std::variant, 并在需要时将其传递给函数。

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

std::variant<int, float, std::string> v;  // v 可以是 int、float 或 std::string
cout << "size of v: "<<sizeof (v) << endl;
v = 42;
cout << std::get<int>(v) << endl;

v = 12.3f;
cout << std::get<float>(v) << endl;
cout << std::get<1>(v) << endl;

// 可以使用 std::holds_alternative<T>(variant) 来判断 std::variant 当前是否持有某种类型
if(std::holds_alternative<int>(v) )
    std::cout << "v holds an int" << std::endl;
else
    std::cout << "v doesn't holds an int" << std::endl;

try {
    std::cout << std::get<int>(v);  // 当前不是 int 类型，会抛出异常
}
catch (const std::bad_variant_access& e)
{
    std::cout << "Wrong type access: " << e.what() << std::endl;
}

//使用 v.index() 获取当前存储值的类型在 std::variant 中的索引
std::cout << "current value index: " << v.index() << std::endl;  // 0 表示 int，1 表示 float，依次类推

v = "Hello, std::variant!";
cout << std::get<std::string>(v) << endl;

std::variant 可以作为函数参数，接受多种类型的值，这些类型是在定义 std::variant 时指定的

std::variant 主要用于以下场景:

1. 多类型返回值: 一个函数可能返回不同类型的值，例如成功返回数据，失败返回错误信息
2. 替代 union: std::variant 是 union 的现代、安全替代品，支持更多的类型和安全检查
3. std::variant 可以表示有限状态机的状态，每个状态用不同的类型表示

类型安全: 使用 std::visit 可确保访问的值是有效的，避免了类型错误

1
2
3

std::visit([](auto&& arg) {
        std::cout << "use visit to print value: " << arg << std::endl;  // 打印不同类型的值
    }, v);

std::any

std::any是一个可以存储任何可拷贝类型的容器，C语言中通常使用void*实现类似的功能，与void*相比，std::any具有两点优势：

std::any更安全：在类型T被转换成void*时，T的类型信息就已经丢失了，在转换回具体类型时程序无法判断当前的void*的类型是否真的是T，容易带来安全隐患。而std::any会存储类型信息，std::any_cast是一个安全的类型转换。
std::any管理了对象的生命周期，在std::any析构时，会将存储的对象析构，而void*则需要手动管理内存。
std::any应当很少是程序员的第一选择，在已知类型的情况下，std::optional, std::variant和继承都是比它更高效、更合理的选择。只有当对类型完全未知的情况下，才应当使用std::any，比如动态类型文本的解析或者业务逻辑的中间层信息传递。

Lambda表达式可以直接在需要调用函数的位置定义短小精悍的函数，而不需要预先定义好函数。适合短小不需要复用函数的场景

缺点:

1. Lamdba表达式语法比较灵活，增加了阅读代码的难度。如果我要找某个函数的调用，搜索函数名即可，但是lambda表达式往往没有名称，我要记住它的位置

2. 难以调试：lambda表达式往往是一种匿名函数，这意味着函数名称并不明确，所以在调试中很难分清楚是哪个函数出错了。

3. 存在捕获的性能开销：捕获变量或对象需要花费额外的时间，而这些时间开销可能会在一些精细的程序中成为问题

4. 不适合复用的场景

原理

编译器会把一个lambda表达式生成一个匿名类的匿名对象，并在类中重载函数调用运算符,实现了一个operator()方法

auto print = []{cout << "Hello World!" << endl; };

编译器会把上面这一句翻译为下面的代码:

1
2
3
4
5
6
7
8
9
10

class print_class
{
public:
    void operator()(void) const
    {
        cout << "Hello World！" << endl;
    }
};
//用构造的类创建对象，print此时就是一个函数对象
auto print = print_class();

1.捕获列表。捕获列表总是出现在Lambda函数的开始处。实际上，[]是Lambda引出符。编译器根据它来判断接下来的代码是否是Lambda函数，捕获列表能够捕捉上下文中的变量以供Lambda函数使用。

2.参数列表。与普通函数的参数列表一致。如果不需要参数传递，则可以连同括号()一起省略。

3.可变规则。mutable修饰符，但使用很少。默认情况下Lambda函数总是一个const函数，mutable可以取消其常量性。在使用该修饰符时，参数列表不可省略（即使参数为空）

4.异常说明。用于Lamdba表达式内部函数抛出异常，使用也很少

5.返回类型。 可以在不需要返回值的时候也可以连同符号->一起省略。此外，在返回类型明确的情况下，也可以省略该部分，让编译器对返回类型进行推导。

6.lambda函数体。内容与普通函数一样，不过除了可以使用参数之外，还可以使用所有捕获的变量。

捕获列表

在[]包括起来的是捕获列表，捕获列表由多个捕获项组成，并以逗号分隔。捕获列表有以下几种形式：

• []表示不捕获任何变量

1
2
3
4
5
6

auto function = ([]{
        std::cout << "Hello World!" << std::endl;
    }
);
 
function();

从这个例子可以看出，Lambda表达式可以作为仿函数

• [var]表示值传递方式捕获变量var

1
2
3
4
5
6
7

int num = 100;
auto function = ([num]{
        std::cout << num << std::endl;
    }
);
 
function();

• [=]表示值传递方式捕获所有父作用域的变量（包括this）

1
2
3
4
5
6
7
8
9

int index = 1;
int num = 100;
auto function = ([=]{
            std::cout << "index: "<< index << ", "
                << "num: "<< num << std::endl;
    }
);
 
function();

• [&var]表示引用传递捕捉变量var

• [&]表示引用传递方式捕捉所有父作用域的变量（包括this）

输入参数

除了捕获列表之外，lambda还可以接受输入参数。参数列表是可选的，并且在大多数方面类似于函数的参数列表。

1
2
3
4
5

auto function = [](int first, int second){
    return first + second;
};
     
function(100, 200);

可变规格mutable和异常使用较少，不研究了。

for_each应用实例

1
2
3

int a[4] = {11, 2, 33, 4};
sort(a, a+4, [=](int x, int y) -> bool { return x%10 < y%10; } );
for_each(a, a+4, [=](int x) { cout << x << " ";} );

find_if应用实例

1
2
3
4
5
6
7
8
9
10

int x = 5;
int y = 10;
deque<int> coll = { 1, 3, 19, 5, 13, 7, 11, 2, 17 };

auto pos = find_if(coll.cbegin(), coll.cend(), [=](int i) {
    return i > x && i < y;
});

if(pos != coll.end() )
    cout << *pos << endl;

Lamdba表达式应用于函数指针与function

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

#include <iostream>
#include <functional>
using namespace std;

int main(void)
{
    auto add = [](int a, int b) { return a + b; };
    std::function<int(int, int)> Add = [=](int a, int b) { return a + b; };

    cout << "add: " << add(1, 2) << endl;
    cout << "Add: " << Add(3, 4) << endl;

  return 0;
}

使用C++容器类访问成员时由于使用问题可能会遇到 terminate called after throwing an instance of ‘std::out_of_range’”或者”Abort message: ‘terminating with uncaught exception of type std::out_of_range

原因如下:

• 索引超出容器范围
• 容器为空，在尝试访问一个空容器的元素时，也会触发此异常，因为容器中没有任何元素可供访问。
• 错误的索引计算

我们可以使用std::out_of_range捕获这个异常

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

vector<int>  vec;
vec.push_back(1);
vec.push_back(2);

try
{
    cout<< vec.at(vec.size() ) <<endl;
}
catch (const std::out_of_range& err)
{
    std::cerr << "\nOut of range error:" << err.what()<< endl;
}

CMAKE_CXX_COMPILE_FEATURES变量用于获取当前C++编译器支持的编译特性列表，列表中是一些定义在CMAKE_CXX_KNOWN_FEATURES(C++已知特性)中的特性名字，比如cxx_lambdas即为当前编译器支持lambda表达式。

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

message("Your C++ compiler supports these C++ features:")
foreach(i ${CMAKE_CXX_COMPILE_FEATURES})
   message("${i}")
endforeach()
# list命令在CMAKE_CXX_COMPILE_FEATURES 查找 cxx_std_20, 如果能找到就说明编译支持C++20
list(FIND CMAKE_CXX_COMPILE_FEATURES  cxx_std_20  _cxx20_enable)
if(_cxx11_enable)
	message(STATUS "C++ 20 supported")
else()
	message(FATAL_ERROR "Compiler not supported C++ 20 standard")
endif()
unset(_cxx20_enable)

以下功能不太合理或者比较冷门，应该尽量少使用

• Exception

• 多继承

• 继承指定类型是public还是private，几乎全是public

• delete []

• 宏

• char和string之间的关系

• 隐式转换

• 模板

• 友元

• 数组类型，能退化成指针，但不能当返回值类型，还不能用另一个数组初始化。

这个算法很简单，可以说是对路径的拉直。可以用于处理A*。也可以说是无约束的降采样，可以消除小的抖动，但无法保证路径仍然安全。

有时路径规划获得的点太多，或者录制机器人路径获得的点太多，可以使用RDP算法，对得到的点进行缩减，用少量目标点表示路径。

首先，将起始点和终点连成一条线，找到剩下的点中距离这条线垂直距离最大的点P，记住点P和最大距离max，如果max小于设定的距离epsilon，则直接返回起始点和终止点就可以了。由于最大距离离这条直线也不远，因此可以把所有的点都看做在这条直线上，相当于把整个路径按照首尾拉直了。

如果max大于设定的距离epsilon，那么开始递归，以点P为中心将线段分为两部分，每一部分都重复上述过程，直到递归结束，相当于对两部分各自拉直。

参考：
一种路径优化方法-拉直法
矢量数据压缩算法提取直线（RDP）

圆

void cv::circle(InputOutputArray img, Point center, int radius, const Scalar& color, int thickness = 1, LineTypes lineType = LINE_8, int shift = 0);

• img: 输入输出参数，表示待绘制的目标图像。
• center: 输入参数，表示圆心坐标，是一个 cv::Point 类型的对象。
• radius: 输入参数，表示圆的半径。
• color: 输入参数，表示绘制圆的颜色以及透明度，是一个 cv::Scalar 类型的对象。
• thickness: 可选参数，表示圆线条的宽度。默认值为 1 表示绘制一个像素宽度的圆，如果设置为负值，则表示绘制一条填充的圆。

• lineType
  : 可选参数，表示圆边界的类型，可以取以下几个值：
  cv::LINE_4: 表示绘制四个相邻的点的圆边界，默认值。
  cv::LINE_8: 表示绘制八个相邻的点的圆边界。
  cv::LINE_AA: 表示绘制抗锯齿的圆边界。

• shift: 可选参数，表示坐标点像素值所占用的位数，默认值为 0。

矩形

void cv::rectangle(InputOutputArray img, Rect rect, const Scalar& color, int thickness = 1, LineTypes lineType = LINE_8, int shift = 0)

• img: 输入输出参数，表示待绘制的目标图像。
• rect: 输入参数，表示矩形，是一个 cv::Rect 类型的对象，可以通过传递左上角和右下角坐标的方式来定义一个矩形。
• color: 输入参数，表示绘制矩形的颜色以及透明度，是一个 cv::Scalar 类型的对象。
• thickness: 可选参数，表示矩形边框的宽度。默认值为 1 表示绘制一个像素宽度的矩形，如果设置为负值，则表示绘制一条填充的矩形。
• lineType: 可选参数，表示矩形边框的类型，可以取以下几个值：
  cv::LINE_4: 表示绘制四个相邻的点的矩形边框，默认值。
  cv::LINE_8: 表示绘制八个相邻的点的矩形边框。
  cv::LINE_AA: 表示绘制抗锯齿的矩形边框。
• shift: 可选参数，表示坐标点像素值所占用的位数，默认值为 0。

Rect函数的基本形式是Rect(x, y, width, height)，其中x和y代表矩形左上角的坐标，width和height分别代表矩形的宽度和高度。

如果创建一个Rect对象rect(100, 50, 50, 100)，有以下常用函数

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

rect.area();     //返回rect的面积 5000

rect.size();     //返回rect的尺寸 [50 × 100]

rect.tl();       //返回rect的左上顶点的坐标 [100, 50]

rect.br();       //返回rect的右下顶点的坐标 [150, 150]

rect.width();    //返回rect的宽度 50

rect.height();   //返回rect的高度 100

rect.contains(Point(x, y));  //返回布尔变量，判断rect是否包含Point(x, y)点