# Cpp或Python(ROS2)有限状态机-行为树数学模型及虚拟力场本地导航算法避障

<details>

<summary>关键词</summary>

C++ | Python | ROS2 | 回调 | 定时器 | 服务参数 | 服务器/客户端 | 执行器 | 有限状态机 | 行为树 | 虚拟力场 | 本地导航算法 | 避障 | 节点 | ROS 接口 | ROS消息主题 | ROS发布者 | ROS订阅者 | 行为建模 | 机器人关节 | 工业机器人 | 比例控制 | YAML | Gazebo 模拟器 | 日志记录 | XML | 虚拟机器人 | 激光扫描 | 几何位置转换子系统 | CMake | RViz 3D可视化 | 动作捕捉 | 生命期节点 | Nav2 | 2D矢量（向量） | 反应行为 | 吸引力矢量 | 排斥矢量 | 结果矢量 | 计算图 | Cpplint | 跟踪对象 | 图像分析 | OpenCV | HSV 范围过滤 | 关节控制 | PID控制 | 自适应蒙特卡洛

</details>

{% embed url="https://viadean.notion.site/Cpp-Python-ROS2-d530f39545f248ab9157dd96751f1e70?pvs=4" %}
关联链接
{% endembed %}

{% content-ref url="../zhi-dian-mi-jin-brief" %}
[zhi-dian-mi-jin-brief](../zhi-dian-mi-jin-brief)
{% endcontent-ref %}

### 要点

1. C++解释ROS2细节：节点初始化、自定义和接口连接代码片段
   1. 回调示例：订阅回调，定时器，服务包括参数调用，接收客户端响应
   2. 执行器示例：创建一个具有两个节点应用，一个节点启动一个计时器，并每秒两次询问第二个节点是否正在使用服务，一个节点将仅托管在线服务；
   3. 接口：数据分发服务，单个服务器和客户端示例：A.对所传送的数据进行自查，并用 CLI 工具可视化交换的消息。B.在发布者和订阅者间，插入节点侦听主题
   4. ROS2实体任务组织：几种有限状态机转换(C++和Python工具)；行为树的机器人和人工智能应用示例，适用于ROS2 的行为树（C++和Python库）；行为建模为分层状态机（Python工具）;拓展行为树模型（Python工具）；任务规划（C++工具）
   5. ROS2 控制器：机器人关节闭环控制模拟（Python无延迟比例控制示例）。
   6. 测试ROS2代码：C++和Python测试框架，基于测试的模拟器；示例代码：工业界机器人Gazebo模拟器。
   7. ROS2 参数管理：ROS2参数第三方（C++或Python）编辑YAML工具，C++示例代码：驻留ROS2中参数守护服务器。
   8. ROS2 消息记录：C++示例代码：ROS2 主题发布器，使用命令行在运行时修改节点记录器的日志级别，以不同的日志记录级别将消息打印到屏幕上。
2. 使用C++，XML
3. 编辑ROS2节点，使其能够发布和订阅信息，在Gazebo中使用虚拟机器人模拟节点，使用RViz2观察感官和几何信息。
4. C++和Python分别实现有限状态机数学计算模型避免障碍物：定义消息格式，激光扫描获取几何位置数据并转换，计算发布和订阅主题之间关系图数据，代码片段解释，执行代码后，打开终端运行模拟器观察机器人如何避障。制作CMake编译清单。
5. C++节点变换计算：ROS2 几何变换子系统，在RViz中查看虚拟机器人几何变换子系统；对ROS2节点发布几何变换广播；使用基于速度的驱动模型，虚拟机器人激光检测到障碍物后几何变换；制作RViz中可视化节点的3D视觉元素；分析“障碍物检测器节点”，“障碍物监测节点”从几个变换子系统提取检测对象几何变换，执行“障碍物检测器节点”，“障碍物监测节点”，在固定模拟框中发布“障碍物检测器节点”，“障碍物监测节点”。ROS 2 中使用动作捕捉系统
6. C++算法和节点实现：使用虚拟力场本地导航算法，使用二维矢量：“吸引力矢量”，“排斥向量”和“结果矢量”，避开障碍物；查看基于摄像头信息的反应性跟踪行为，在Gazebo中模拟观察；使用生命期节点并创建自定义信息。
7. C++实现：代码编辑行为树数学模式，执行行为树控制子系统（感应信息和发送速度）；行为树控制Nav2导航子系统和主动视觉子系统：Gazebo 模拟机器人房屋巡视。

### ROS2

ROS1 对比 ROS2

* ROS1有自己的中间件，它使用网络堆栈将信息从节点发送到正确的套接字，中间件再次接收该信息以将信息发送到正确的节点（过于简化）。 ROS2放弃了中间件，现在使用数据分发服务（DDS），这是一种第三方中间件，是可靠的行业标准。
* Rosmaster 是一项促进两个节点相互查找的服务（如果它们希望通信）。 接下来，它们直接相互连接。 如果在此过程中rosmaster死亡，这些节点仍然会保持连接，但没有新的节点可以加入通信网络。在ROS1中，每个节点都需要知道master的位置（即使系统运行在多台机器上）。 ROS2取消了rosmaster，节点可以直接找到对方。
* ROS支持C++和Python两种语言，底层实现也有所不同。 例如，在 Python 中每个订阅者都有自己的线程，但在 C++ 中则没有。 这对于 ROS1 来说是正确的，但 ROS2 具有相同的共享 C 实现。 通用代码库通过 C++ 和 Python 语言的 ROS 客户端库 API 公开。
* ROS1不允许一个进程中有多个ROS节点，而 ROS2 中改变了。
* 在 ROS1 中更改 python 文件不需要您构建项目，ROS2 需要您再次构建项目。
* Action Services 不是核心 ROS1 库的一部分（而是 actionlib），而 ROS2核心库有动作服务器。
* Windows支持ROS2，但是建议使用 Linux。
* ROS1 主要使用基于 XML 的启动文件。 ROS2有一个支持排序约束的python启动系统。

应用案例：创建发布者和订阅者

创建名为“ros2\_ws”的工作区：

```sh
 mkdir -p ~/ros2_ws/src
```

编译工作区：

ROS1使用catkin，但ROS2使用colcon构建包

```bash
 cd ros2_ws
 colcon build
```

此时，您将在终端中看到“0 个包已完成”（因为还没有包）。此外，您现在将在 ros2\_ws.txt 文件中拥有构建、安装和日志文件夹。

创建一个新的发布者包（Python实现）

```bash
 cd ros2_ws/src
 ros2 pkg create --build-type ament_python talker_listener
```

同样，我们也可以制作一个cpp包。请查看ROS2官方指南。

输出结果：

```bash
 ros2experiments$ros 2 pkg create --build-type ament_python talker_listener
 going to create a new package
 package name: talker_listener
 destination directory: /home/sharad/demos/ros2_ws/src
 package format: 3
 version: 0.0.0
 description: TODO: Package description
 maintainer: ['sharad <share@email.com>']
 licenses: ['TODO: License declaration']
 build type: ament_python
 dependencies: []
 creating folder ./talker_listener
 creating ./talker_listener/package.xml
 creating source folder
 creating folder ./talker_listener/talker_listener
 creating ./talker_listenēr/setup.py
 creating ./talker_listener/setup.cfg
 creating folder .//talker_listener/resource
 creating ./talker_listener/resource/talker_listener
 creating ./talker_listener/talker_listener/_int__.py
 creating folder ./talker_listener/test
 creating ./talker_listener/test/test_copyright.py
 creating ./talker_listener/test/test_flake8.py
 creating ./talker_listener/test/test_pep257.py
```

我们现在有一个空包。

导航到代码目录（我们将在其中放置节点文件）：

```sh
 cd ros2_ws/src/talker_listener/talker_listener
```

> 注意：该目录与包同名

制作 talker 节点

```sh
 gedit talker_node.py
```

使用VSCode编辑代码：

```python
 import rclpy
 from rclpy.node import Node
 ​
 from std_msgs.msg import String
 ​
 ​
 class MinimalPublisher(Node):
 ​
     def __init__(self):
         super().__init__('minimal_publisher')
         self.publisher_ = self.create_publisher(String, 'topic', 10)
         timer_period = 0.5  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
         self.i = 0
 ​
     def timer_callback(self):
         msg = String()
         msg.data = 'Hello World: %d' % self.i
         self.publisher_.publish(msg)
         self.get_logger().info('Publishing: "%s"' % msg.data)
         self.i += 1
 ​
 ​
 def main(args=None):
     rclpy.init(args=args)
 ​
     minimal_publisher = MinimalPublisher()
 ​
     rclpy.spin(minimal_publisher)
     minimal_publisher.destroy_node()
     rclpy.shutdown()
 ​
 ​
 if __name__ == '__main__':
     main()
 ​
```

rclpy 有 API 可以在 ROS 中使用底层 C 代码库，如上述所示。rclpy 有 API 和类来抽象我们在 ROS1 中自己做的很多东西。 Node 类抽象节点初始化，创建发布者、订阅者、服务、客户端。在这里，我们继承了 Node 的所有方法和属性。

* 我们不自己初始化节点
* 我们使用 Node 中的“create\_publisher”方法（第 11 行）来创建发布者
* 我们使用 Node 中的 create\_timer 方法（第 13 行）来使用其计时器并设置回调方法。我们在ROS1中使用rate来做类似的事情
* 我们只需spin节点，我们的计时器就会根据设置的周期进行回调

> 注意：rclpy.spin 被阻塞

添加依赖项：

返回找到 package.xml 并添加以下行

```
 <exec_depend>rclpy</exec_depend>
 <exec_depend>std_msgs</exec_depend>
```

我们的包依赖于 rclpy 和 std\_msgs。
