Abo Hexo https://leo-wangbo.top/ All rights reserved 2026, Abo 卧龙凤雏团队欢迎你 阿波的博客 2025-11-10T19:03:02.000Z Abo 总体实现框架图：

找了个实际的变电站环境，改动了一下导入到了Gazebo中用作实验的仿真环境

目前有了一个初步的demo，等做完后开源并对算法进行详细的讲解

]]> https://leo-wangbo.top/p/%E5%81%9A%E4%BA%86%E4%B8%80%E4%B8%AA%E8%83%BD%E5%90%AC%E6%87%82%E4%BA%BA%E8%AF%9D%E7%9A%84%E5%B7%A1%E6%A3%80%E6%9C%BA%E5%99%A8%E4%BA%BA/ 2025-09-06T07:22:26.000Z 总体实现框架图：

找了个实际的变电站环境，改动了一下导入到了Gazebo中用作实验的仿真环境

 做了一个能听懂人话的巡检机器人 2025-11-10T19:03:02.000Z Abo 一、topic_state_monitor模块简介

此节点用于监控任意话题是否存在异常，例如超时和低频率。话题的诊断结果将通过ROS Diagnostics发布诊断信息。

  • 输入：任意名称、任意类型的话题
  • 输出：/diagnostics 诊断信息

二、topic_state_monitor模块启动流程

通过解析 topic_state_monitor 核心代码部分并逐级向上追溯，可得到 topic_state_monitor 模块完整的启动流程，如下图所示：

上层的两个 .launch.xml 文件主要配置了两个参数。一个是 tier4_system_component.launch.xml 中配置的 topics.yaml 文件路径。topics.yaml 文件内容是配置所有需要监控的话题信息以及监控阈值信息。配置文件的内容示例如下:

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
- module: control
  mode: [online, logging_simulation, planning_simulation]
  type: autonomous
  args:
    topic_type: autoware_control_msgs/msg/Control
    best_effort: false
    transient_local: false
    warn_rate: 5.0
    error_rate: 1.0
    timeout: 1.0
- module: control
  mode: [online, logging_simulation, planning_simulation]
  type: autonomous
  args:
    node_name_suffix: control_command_control_cmd
    topic: /control/command/control_cmd
    topic_type: autoware_control_msgs/msg/Control
    best_effort: false
    transient_local: false
    warn_rate: 5.0
    error_rate: 1.0
    timeout: 1.0
- module: localization
  mode: [online, logging_simulation, planning_simulation]
  type: autonomous
  args:
    node_name_suffix: transform_map_to_base_link
    topic: /tf
    frame_id: map
    child_frame_id: base_link
    best_effort: false
    transient_local: false
    warn_rate: 5.0
    error_rate: 1.0
    timeout: 1.0

代码2-1 topics.yaml配置⽂件⽰例

配置文件的各个参数的解释如下:

名称含义
module话题所属的模块
mode指定了配置适用的模式，通过设置 mode，可以控制配置文件在不同运行环境下是否生效
type指定话题的类型，根据不同的 type 分类，component_state_monitor 模块会分析 /diagnostics 诊断信息给出 /type/module 模块的状态（判断是否正常运行，消息类型为自定义的 ModeChangeAvailable）
node_name_suffix节点名称的后缀，用于动态生成节点名称
topic指定要监控的目标话题的名称
topic_type指定目标话题的数据类型
best_effort指定是否使用 "Best Effort" QoS策略。如果设置为 true，意味着使用“尽力而为”策略，即消息传输尽力而为，可能会丢失消息。设置为 false 则表示采用 "Reliable" QoS策略，确保消息的可靠传输，但可能带来更高的延迟。
transient_local指定是否使用 "Transient Local" QoS策略。如果设置为 true，即使订阅者在发布时不存在，消息也会暂时保存在中间件中，直到订阅者连接。此策略适用于需要历史消息的场景。如果设置为 false，则采用普通的QoS策略，只有当前连接的订阅者可以接收到发布的消息。
warn_rate话题发布频率警告阈值，当话题的发布频率低于该值时，话题状态将被设置为 WarnRate，即警告状态
error_rate话题发布频率错误阈值，当指定当话题的发布频率低于该值时，话题状态将被设置为 ErrorRate，即错误状态
timeout这个参数指定如果话题订阅在超过指定的时间（单位为秒）内没有收到消息，话题状态将被设置为 Timeout
frame_id坐标变换的父坐标系ID
child_frame_id坐标变换的子框架ID
window_size计算目标主题频率使用的窗口大小，例如，窗口大小为10表示计算过去10个发布周期的频率
uprate_rate定时器回调频率，用于设置更新和检查主题状态的频率
表2-1 topics.yaml配置⽂件参数解释

system.launch.xml 文件中设置了配置所选择的模式，根据模式的选择来监听不同的话题组。话题所属的模式在上述 topics.yaml 文件中设置。

1
<arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation

最终通过 component_state_monitor.launch.py 生成了一系列针对目标话题的监控节点。这些节点发布诊断数据到 /diagnostics 话题中。

三、topic_state_monitor模块运行效果

运行 Autoware 的规划demo样例后：

1
2
3
cd autoware
source ~/autoware/install/setup.bash
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

然后运行 ros2 node list 可以看到 topic_state_monitor 模块运行起来一些监控节点来对配置文件中的话题进行监控。监控节点名称为：“topic_state_monitor” + topic.yaml 中话题的 node_name_suffix 名称。

这些节点输出诊断信息到 ROS Diagnostics 诊断系统中，可以在 rqt_runtime_monitor 中看到诊断结果。

同时，component_state_monitor::StateMonitor 节点的 main.cpp 订阅 /diagnostics 话题并通过定时器周期性地检查不同模块整体的状态是否正常。输出的话题格式是 /type/module，例如，对于 type 为 launch 中的 map 模块，输出话题为 /launch/map。

话题输出的消息格式是自定义的 ModeChangeAvailable.msg 格式，用于表示某个模块是否可用。消息包含以下内容:

  • stamp：时间戳，表示消息生成的时间。
  • available：布尔值，表示该模块当前是否有效。true 表示模块正常，false 表示模块不可用。

四、topic状态监控部分实现原理

分析源码后总结下来，其核心是通过 component_state_monitor.launch.py 对 config.yaml 文件中要监控的 topic 一对一创建一个 node。

随后将 node 启动部分注册成一个插件，这样可以用同一个源码启动多个 node。在 node 源码中，根据监控 topic 是否为 tf 话题创建 tf 类订阅者和非 tf 类订阅者。这里针对非 tf 类 topic，使用了 rclcpp::SerializedMessage 一个通用的消息类型，表示经过序列化的消息。它不关心消息的具体内容，只是将消息传递为原始的字节流。

五、一些考量点

topic_state_monitor 模块可针对任意类型的 topic 生成一个 node 去监控 topic 的状态，通用性强。但如果需要监控的 topic 过多可能进程开销大，因此要对该模块对于性能的开销进行测试来判断是否可迁移到人形机器人上。

topic_state_monitor 模块采用了组合节点并将组合节点放入 container 中来启动一堆 node 去监控各个 topic。这样做的好处是将多个节点加载到同一个进程中，每个节点仍然保持独立的逻辑和功能，但它们可以更高效地共享数据和资源。这种方法不仅减少了通信延迟，而且降低了系统的整体资源消耗（例如，CPU时间内存使用）。

实际使用 Autoware 进行测试时发现，输入 ros2 component list 查看容器节点信息可知组合节点 /system/component_state_monitor/component 有被放入 /system/component_state_monitor/container 容器中。

但是查询系统进程资源占用情况时发现：启动的各个监控 node 并没有在同一个进程中进行，而是各自占用了一个进程，这样性能总开销过大。

猜测是启动文件中 node 的启动方式存在问题。经过分析发现问题出在 component_state_monitor.launch.py 启动文件对于 node 节点的启动是通过启动一个个 launch.xml 文件来生成监控 node 节点，并没有将 node 节点放入 container 容器中，因此占用了多个进程。

想要减少进程的占用，需要重构 component_state_monitor.launch.py 对于 node 节点的启动部分。修改后的代码如下:

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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
from collections import defaultdict
from pathlib import Path

import launch
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
import yaml

def create_diagnostic_name(row):
    return f"{row['module']}_topic_status"

def create_topic_monitor_name(row):
    diag_name = create_diagnostic_name(row)
    node_name_suffix = row["args"]["node_name_suffix"]
    # 替换非法字符，如冒号和空格为下划线(防止报错Couldn't parse remap rule: '-r __node......）
    node_name_suffix = node_name_suffix.replace(":", "_").replace(" ", "_")
    diag_name = diag_name.replace(":", "_").replace(" ", "_")
    return f"topic_state_monitor_{node_name_suffix}_{diag_name}"

def create_topic_monitor_node(row):
    diag_name = create_diagnostic_name(row)
    # 基础参数
    parameters = [{"diag_name": diag_name}]
    # 根据是否为 TF 类话题，添加不同的参数
    remappings = []
    if "topic_type" in row["args"]:
        # 非 TF 类话题
        parameters.append({"topic_type": row["args"]["topic_type"]})
    else:
        # TF 类话题
        parameters.append({"frame_id": row["args"]["frame_id"]})
        parameters.append({"child_frame_id": row["args"]["child_frame_id"]})
    # 其他参数
    for k, v in row["args"].items():
        if k not in ["topic_type", "frame_id", "child_frame_id"]:
            parameters.append({k: v})
    # 如果需要进行主题重映射，可以在这里添加 remappings
    # 假设 YAML 文件中不包含 remaps 字段，因此 remappings 为空
    # if "remaps" in row["args"]:
    #     for remap in row["args"]["remaps"]:
    #         remappings.append(remap)
    return ComposableNode(
        namespace="system",
        package="topic_state_monitor",
        plugin="topic_state_monitor::TopicStateMonitorNode",
        name=create_topic_monitor_name(row),
        parameters=parameters,
        remappings=remappings  # 目前为空
    )

def launch_setup(context, *args, **kwargs):
    # 获取启动模式和配置文件
    mode = LaunchConfiguration("mode").perform(context)
    config_file = LaunchConfiguration("file").perform(context)
    rows = yaml.safe_load(Path(config_file).read_text())
    rows = [row for row in rows if mode in row["mode"]]
    # 创建所有监控节点
    topic_monitor_nodes = [create_topic_monitor_node(row) for row in rows]
    topic_monitor_names = [create_topic_monitor_name(row) for row in rows]
    # 组织参数
    topic_monitor_param = defaultdict(lambda: defaultdict(list))
    for row in rows:
        topic_monitor_param[row["type"]][row["module"]].append(create_topic_monitor_name(row))
    topic_monitor_param = {name: dict(module) for name, module in topic_monitor_param.items()}
    # 创建主组件节点（StateMonitor）
    component = ComposableNode(
        namespace="component_state_monitor",
        package="component_state_monitor",
        plugin="component_state_monitor::StateMonitor",
        name="component",
        parameters=[{"topic_monitor_names": topic_monitor_names}, topic_monitor_param],
    )
    # 创建容器，并将所有ComposableNode添加到容器中
    container = ComposableNodeContainer(
        namespace="component_state_monitor",
        name="container",
        package="rclcpp_components",
        executable="component_container",
        composable_node_descriptions=[component] + topic_monitor_nodes,  # 将监控节点加入容器
        output="screen",
    )
    return [container]

def generate_launch_description():
    return launch.LaunchDescription(
        [
            DeclareLaunchArgument("file"),
            DeclareLaunchArgument("mode"),
            OpaqueFunction(function=launch_setup),
        ]
    )

代码修改后运行可发现监控 node 成功被放入 container 容器中在同一个进程中运行。

但是检查进程占用情况发现 component_state_monitor 进程总占用在 70% 左右，占用过高。

修改参数列表中的 window_size 与 update_rate，将其从默认的 10 调小至 2 后，占用可下降至 15% 左右。

经分析，TopicStateMonitor 的逻辑本身相对简单，只是维护一个时间戳队列、计算话题接收速率，并根据阈值进行状态判断。单独看这段代码，性能开销并不大，主要是一些基本的内存操作（push_back、pop_front）、时间计算和简单的条件判断。这些操作本身不是CPU占用的主要来源。

然而，当有 15个 这样的监控节点，每个节点都在高频率地接收消息并调用 update() 函数时，整体系统的CPU使用率就可能显著增加。由于 Autoware 系统繁杂臃肿，消息的频率不太好手动控制。为了控制变量研究消息频率与数量对监控模块占用系统资源的情况，于是手动创建一些节点，观察不同 topic 数量、频率与 QoS 策略下的系统资源占用情况。

测试编写脚本创建多个发布者发布 std_msgs/msg/String 格式的话题，然后启动 topic_state_monitor，其中 window_size 设为 2、update_rate 设为 1。之后对 topic_state_monitor 进程的 CPU 占用率进行监控 120秒，每秒测两次，将测试结果绘制成表格如下所示:

影响因素Topic数量Topic频率best_efforttransient_local平均占用率最小占用率最大占用率Topic频率对占用率的影响
155falsetrue3.55%2.0%5.0%
1510falsetrue4.53%3.0%6.0%
1515falsetrue5.63%3.0%7.0%
1520falsetrue8.44%4.0%12.0%
1535falsetrue10.23%4.0%14.0%
1540falsetrue11.36%8.0%16.0%
1545falsetrue13.87%10.0%18.0%
1550falsetrue14.30%10.0%18.0%
QoS策略对占用率的影响1550falsefalse12.47%8.0%16.0%
1550truetrue11.42%8.0%16.0%
1550truefalse11.17%6.0%16.0%
Topic数量对占用率的影响2050truefalse16.30%10.0%22.0%
1050truefalse6.54%4.0%12.0%
550truefalse4.54%2.0%8.0%
将表格绘制成折线图如下所示：![](/uploads/2025/03/topic频率对cpu占用影响-1024x718.png)![](/uploads/2025/09/topic数量对cpu占用影响.png)

由表格数据可以得出结论：

  • Topic 数量与频率的增加会增加 topic_state_monitor 模块对系统的占用。如果以 15% 的占用率为界限定义为对 CPU 占用较少、为可用状态的话，那么 15个50Hz的Topic以下 或者 更多的更低Hz的Topic、更少的更高Hz的Topic 为此监控系统工作的最佳状态。
  • 同时，QoS 策略也会对系统占用产生影响。设置 best_effort 为 true，即不使用 reliable 可靠性传输策略；设置 transient_local 为 false，即不使用 Volatile 持久性 QoS 策略。这样可以牺牲一点消息传输的可靠性来换取更低的资源占用。

小结

topic_state_monitor 模块可针对任意类型的 topic 生成一个 node 去监控 topic 的状态，通用性强。但如果需要监控过多高频的 Topic 消息可能会导致进程开销大。因此 Autoware 中对进行监控的 topic 进行了模块化分类处理，根据不同的应用场景选择性地监控关键模块的关键话题。例如，定位模块着重关注 map 到 base_link 的 tf 转换是否正常，控制模块着重关注发布的 control_cmd 控制 topic 是否正常。

]]> https://leo-wangbo.top/p/autoware%E4%B8%ADtopic_state_monitor%E6%A8%A1%E5%9D%97%E8%B0%83%E7%A0%94/ 2025-03-09T05:39:42.000Z 一、topic_sta]]> Autoware中topic_state_monitor模块调研 2025-09-10T01:34:49.000Z Abo 一、适配传感器

首先声明，踩了那么多坑后，明白一个道理，要想少出错，少踩坑，还是要遵循一切都参控官方文档的原则。不要怕官网文档繁杂又全是英语想省事，就一切都遵从csdn找到的别人整理的二手文档，包括我以下写的都只能当作参考，每个人硬件软件环境都不一样，具体步骤肯定有所差异，官网还是考虑最全的文档，英语有障碍可以下一个插件，推荐"沉浸式翻译"这个插件。
官网文档链接为:https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/

设置传感器数据通信接口

根据官方提供的流程图，可以看到各个节点之间的数据通信：https://autowarefoundation.github.io/autoware-documentation/galactic/design/autoware-architecture/node-diagram/

雷达点云设置

Autoware输入的点云为 ==/sensing/lidar/top/outlier_filtered/pointcloud== 和 ==/sensing/lidar/concatenated/pointcloud==（frame_id均为base_link）。
/sensing/lidar/top/==outlier_filtered==/pointcloud用于==定位==,
/sensing/lidar/==concatenated==/pointcloud用==于感知==；

编写雷达点云转换节点

打开终端并执行以下命令来创建工作空间和功能包：

1
2
3
mkdir -p /home/buaa/autoware_universe/autoware/src/sensor_driver/robosense_ws/src
cd /home/buaa/autoware_universe/autoware/src/sensor_driver/robosense_ws/src
ros2 pkg create --build-type ament_cmake lidar_transform

编写玩节点代码:

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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pcl_ros/transforms.hpp>
#include <pcl_conversions/pcl_conversions.h>  // 包含PCL转换头文件

class LidarTransformNode : public rclcpp::Node
{
public:
    // 构造函数
    LidarTransformNode() : Node("points_raw_transform_node")
    {
        // 初始化坐标转换参数
        // this->declare_parameter<double>("transform_x", 0.0);
        // this->get_parameter("transform_x", transform_x);
        transform_x = this->declare_parameter("transform_x", 0.0);
        transform_y = this->declare_parameter("transform_y", 0.0);
        transform_z = this->declare_parameter("transform_z", 0.0);
        transform_roll = this->declare_parameter("transform_roll", 0.0);
        transform_pitch = this->declare_parameter("transform_pitch", 0.0);
        transform_yaw = this->declare_parameter("transform_yaw", 0.0);
        RadiusOutlierFilter = this->declare_parameter("RadiusOutlierFilter", 1.0);

        std::cout << "robosense to base_link:" << std::endl
                  << "  transform_x:    " << transform_x << std::endl
                  << "  transform_y:    " << transform_y << std::endl
                  << "  transform_z:    " << transform_z << std::endl
                  << "  transform_roll: " << transform_roll << std::endl
                  << "  transform_pitch:" << transform_pitch << std::endl
                  << "  transform_yaw:  " << transform_yaw << std::endl;

        // Initialize translation
        transform_stamp.transform.translation.x = transform_x;
        transform_stamp.transform.translation.y = transform_y;
        transform_stamp.transform.translation.z = transform_z;
        // Initialize rotation (quaternion)
        tf2::Quaternion quaternion;
        quaternion.setRPY(transform_roll, transform_pitch, transform_yaw);
        transform_stamp.transform.rotation.x = quaternion.x();
        transform_stamp.transform.rotation.y = quaternion.y();
        transform_stamp.transform.rotation.z = quaternion.z();
        transform_stamp.transform.rotation.w = quaternion.w();

        // 创建发布者
        publisher_1 = this->create_publisher<sensor_msgs::msg::PointCloud2>(
            "/sensing/lidar/top/outlier_filtered/pointcloud",
            10);
        publisher_2 = this->create_publisher<sensor_msgs::msg::PointCloud2>(
            "/sensing/lidar/concatenated/pointcloud",
            10);

        // 订阅原始点云消息
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/rslidar_points",
            10,
            std::bind(&LidarTransformNode::pointCloudCallback, this, std::placeholders::_1));
    }

private:
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        // 过滤掉距离传感器较近的点
        pcl::PointCloud<pcl::PointXYZI>::Ptr xyz_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*msg, *xyz_cloud);
        for (size_t i = 0; i < xyz_cloud->points.size(); ++i)
        {
          if (sqrt(xyz_cloud->points[i].x * xyz_cloud->points[i].x + xyz_cloud->points[i].y * xyz_cloud->points[i].y +
                   xyz_cloud->points[i].z * xyz_cloud->points[i].z) >= RadiusOutlierFilter && !isnan(xyz_cloud->points[i].z))
          {
            pcl_output->points.push_back(xyz_cloud->points.at(i));
          }
        }
        sensor_msgs::msg::PointCloud2 output;
        pcl::toROSMsg(*pcl_output, output);
        output.header = msg->header;

        // 执行坐标转换
        sensor_msgs::msg::PointCloud2 transformed_cloud;
        pcl_ros::transformPointCloud("base_link", transform_stamp, output, transformed_cloud);

        // 发布转换后的点云消息
        publisher_1->publish(transformed_cloud);
        publisher_2->publish(transformed_cloud);
    }

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_1, publisher_2;

    double transform_x, transform_y, transform_z, transform_roll, transform_pitch, transform_yaw, RadiusOutlierFilter; // 添加 radius_outlier_filter 成员变量声明

    geometry_msgs::msg::TransformStamped transform_stamp;
};

int main(int argc, char **argv)
{
    // 初始化节点
    rclcpp::init(argc, argv);

    // 创建实例
    auto node = std::make_shared<LidarTransformNode>();

    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

然后编译：

1
colcon build --packages-up-to lidar_transform

然后遵从Autoware.Universe官网教程 https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-and-sensor-model/creating-sensor-model/ 更改/home/buaa/autoware_universe/autoware/src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_launch/launch 路径下的传感器启动文件

更改lidar.launch.xml

由于只用一个雷达，故不使用点云融合功能包，设置use_concat_filter为false,并发布lidar_transform点云tf转换功能包

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
66
67
68
69
70
<launch>
  <arg name="use_concat_filter" default="false"/>
  <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/>
  <arg name="pointcloud_container_name" default="pointcloud_container"/>
  <arg name="rviz_config" default="$(find-pkg-share rslidar_sdk)/rviz/rviz2.rviz"/>
  <group>
    <push-ros-namespace namespace="lidar"/>
    <group>
      <push-ros-namespace namespace="top"/>
      <node pkg="rslidar_sdk" exec="rslidar_sdk_node" output="screen">
          <!-- 添加其他必要的参数 -->
          <param name="queue_size" type="int" value="100"/>
          <param name="hardware_id"  value="none"/>
      </node>
      <!-- <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config)"/> -->
    </group>

    <include file="$(find-pkg-share lidar_transform)/launch/points_raw_transform.launch.xml">
        <arg name="transform_x" value="0.0"/>
        <arg name="transform_y" value="0.0"/>
        <arg name="transform_z" value="1.0"/>
        <arg name="transform_roll" value="0.0"/>
        <arg name="transform_pitch" value="0.0"/>
        <arg name="transform_yaw" value="0.0"/>
        <arg name="RadiusOutlierFilter" value="1.0"/>
    </include>

    <!-- <group>
      <push-ros-namespace namespace="left"/>
      <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml">
        <arg name="max_range" value="5.0"/>
        <arg name="sensor_frame" value="velodyne_left"/>
        <arg name="sensor_ip" value="192.168.1.202"/>
        <arg name="host_ip" value="$(var host_ip)"/>
        <arg name="data_port" value="2369"/>
        <arg name="scan_phase" value="180.0"/>
        <arg name="cloud_min_angle" value="300"/>
        <arg name="cloud_max_angle" value="60"/>
        <arg name="launch_driver" value="$(var launch_driver)"/>
        <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
        <arg name="container_name" value="pointcloud_container"/>
      </include>
    </group>

    <group>
      <push-ros-namespace namespace="right"/>
      <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml">
        <arg name="max_range" value="5.0"/>
        <arg name="sensor_frame" value="velodyne_right"/>
        <arg name="sensor_ip" value="192.168.1.203"/>
        <arg name="host_ip" value="$(var host_ip)"/>
        <arg name="data_port" value="2370"/>
        <arg name="scan_phase" value="180.0"/>
        <arg name="cloud_min_angle" value="300"/>
        <arg name="cloud_max_angle" value="60"/>
        <arg name="launch_driver" value="$(var launch_driver)"/>
        <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
        <arg name="container_name" value="pointcloud_container"/>
      </include>
    </group> -->

    <include file="$(find-pkg-share sample_sensor_kit_launch)/launch/pointcloud_preprocessor.launch.py">
      <arg name="base_frame" value="base_link"/>
      <arg name="use_intra_process" value="true"/>
      <arg name="use_multithread" value="true"/>
      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
      <arg name="use_concat_filter" value="$(var use_concat_filter)"/> 
    </include>
  </group>
</launch>

更改imu.launch.xml

添加雷达驱动，并发布雷达话题tamagawa/imu_link到base_link的tf转换

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
<launch>
  <arg name="launch_driver" default="true"/>
  <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
  <!-- add -->
  <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>

  <group>
    <push-ros-namespace namespace="imu"/>

    <group>
      <push-ros-namespace namespace="fdilink"/>
      <node pkg="fdilink_ahrs" name="ahrs_driver_node" exec="ahrs_driver_node" if="$(var launch_driver)">
        <param name="if_debug_" value="false"/>
        <param name="serial_port_" value="/dev/imu_usb"/>
        <param name="serial_baud_" value="921600"/>
        <param name="imu_topic" value="/sensing/imu/tamagawa/imu_raw"/>
        <param name="imu_frame_id_" value="tamagawa/imu_link"/>
        <param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
        <param name="Magnetic_topic" value="/magnetic"/>
        <param name="Euler_angles_topic" value="/euler_angles"/>
        <param name="gps_topic" value="/gps/fix"/>
        <param name="twist_topic" value="/system_speed"/>
        <param name="NED_odom_topic" value="/imu_odometry"/>
        <param name="device_type_" value="1"/>
      </node>
    </group>

    <node pkg="tf2_ros" exec="static_transform_publisher" output="screen"
        args="0.5 0 0 0 0 0 base_link tamagawa/imu_link"/>

    <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
      <arg name="input_topic" value="$(var imu_raw_name)"/>
      <arg name="output_topic" value="imu_data"/>
      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
    </include>

    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
      <arg name="input_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
    </include>
  </group>
</launch>

禁用自带的tf转换

由于上述启动传感器时都发布了对应到baselink的tf转换，故禁用掉官方源码中发布的车体传感器tf转换部分。打开/home/buaa/autoware_universe/autoware/src/universe/autoware.universe/launch/tier4_vehicle_launch/launch 文件夹下的vehicle.launch.xml
注释掉vehicle description部分

启动传感器

单独启动传感器的命令为:

1
ros2 launch tier4_sensing_launch sensing.launch.xml

二、适配底盘

https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/creating-vehicle-interface/
根据官网教程一步一步编写vehicle_interface来创建底盘驱动与Autoware之间的联系：

创建功能包

1
2
cd <your-autoware-dir>/src/vehicle/external
ros2 pkg create --build-type ament_cmake my_vehicle_interface

从 Autoware 订阅控制命令主题的一些必要主题以控制车辆，具体话题描述参考官网，其核心是将接收到的autoware控制话题==control/command/control_cmd==提取出控制指令转化为底盘可以接受的==cmd_vel==话题格式，同时将一些必要的车辆状态主题发布。
以下为脚本内容

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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from autoware_auto_control_msgs.msg import AckermannControlCommand
from autoware_auto_vehicle_msgs.msg import ControlModeReport, GearReport, SteeringReport, VelocityReport
from hunter_msgs.msg import HunterStatus
from tier4_vehicle_msgs.msg import BatteryStatus

class VehicleInterface(Node):
    def __init__(self):
        super().__init__('vehicle_interface')

        # 订阅 Autoware 的控制指令
        self.subscriber_control_cmd = self.create_subscription(
            AckermannControlCommand,
            'control/command/control_cmd',
            self.control_cmd_callback,
            10
        )

        # 发布小车的控制指令
        self.publisher_cmd_vel = self.create_publisher(Twist, '/cmd_vel', 10)

        # 订阅小车的状态信息
        self.subscriber_hunter_status = self.create_subscription(
            HunterStatus,
            '/hunter_status',
            self.hunter_status_callback,
            10
        )

        # 发布转向状态
        self.publisher_steering = self.create_publisher(SteeringReport, '/vehicle/status/steering_status', 10)

        # 发布速度状态
        self.publisher_velocity = self.create_publisher(VelocityReport, '/vehicle/status/velocity_status', 10)

        # 发布电池状态
        self.publisher_battery = self.create_publisher(BatteryStatus, '/vehicle/status/battery_charge', 10)

    def control_cmd_callback(self, msg):
        """
        处理从 Autoware 收到的控制命令，并将其转换为 Ackermann 结构小车的控制命令
        """
        twist = Twist()
        twist.linear.x = msg.longitudinal.speed  # 线速度
        twist.angular.z = msg.lateral.steering_tire_angle  # 转向角度

        self.publisher_cmd_vel.publish(twist)  # 发布转换后的控制命令

    def hunter_status_callback(self, msg):
        """
        处理从小车接收到的状态信息，并将其发布到 Autoware 的相关话题
        """

        # 发布转向报告
        steering_report = SteeringReport()
        steering_report.stamp = self.get_clock().now().to_msg()
        steering_report.steering_tire_angle = msg.steering_angle
        self.publisher_steering.publish(steering_report)

        # 发布速度报告
        velocity_report = VelocityReport()
        velocity_report.header.stamp = self.get_clock().now().to_msg()
        velocity_report.header.frame_id = "base_link"
        velocity_report.longitudinal_velocity = msg.linear_velocity  # 纵向速度
        velocity_report.lateral_velocity = 0.0  # 横向速度（假设为0）
        velocity_report.heading_rate = msg.steering_angle  # 航向变化率

        self.publisher_velocity.publish(velocity_report)

        # 发布电池状态报告
        battery_status = BatteryStatus()
        battery_status.stamp = self.get_clock().now().to_msg()  # 时间戳
        battery_status.energy_level = msg.battery_voltage  # 电池电压
        self.publisher_battery.publish(battery_status)

def main(args=None):
    rclpy.init(args=args)
    node = VehicleInterface()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

创建launch文件my_vehicle_interface.launch.xml：

1
2
3
4
<launch>
  <node pkg="my_vehicle_interface" exec="vehicle_interface.py" name="vehicle_interface" output="screen">
  </node>
</launch>

修改CMakeLists.txt与package.xml后编译：

1
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select my_vehicle_interface

如果运行遇到找不到py文件的问题，可能是没有赋予其运行权限
在 vehicle_interface.py 的开头添加执行指令

1
#!/usr/bin/env python3
1
chmod +x scripts/vehicle_interface.py

然后找到/home/buaa/autoware_universe/autoware/src/universe/autoware.universe/launch/tier4_vehicle_launch/launch 路径下的vehicle.launch.xml 将vehicle_interface部分改成自己的：

1
2
3
4
5
6
7
8
9
10
  <!-- vehicle interface -->
  <group if="$(var launch_vehicle_interface)">
    <node pkg="my_vehicle_interface" exec="vehicle_interface.py" name="vehicle_interface" output="screen">
      <param name="vehicle_id" value="$(var vehicle_id)"/>
      <param name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
      <param name="initial_engage_state" value="$(var initial_engage_state)"/>
    </node>

  </group>
</launch>

但是为了调试便利，还是注释掉上述部分，选择单独启动vehicle interface：

1
ros2 launch my_vehicle_interface  my_vehicle_interface.launch.xml
]]> https://leo-wangbo.top/p/autoware-universe%E9%80%82%E9%85%8D%E5%AE%9E%E8%BD%A6%E6%95%99%E7%A8%8B/ 2024-08-17T00:18:14.000Z 一、适配传感器

首先声明，踩了那么多坑后，明白一个道理，要想少出错，少踩坑，还是要遵循一切都参控官方文档的原则。不要]]>

Autoware.Universe适配实车教程 2024-08-18T06:10:09.000Z Abo Autoware.universe 安装步骤

克隆Autoware到本地

1
2
3
mkdir autoware_universe
cd autoware_universe/
git clone https://github.com/autowarefoundation/autoware.git -b humble

注意ROS的版本

安装NVIDIA 显卡驱动

若输入 sudo nvidia-smi 检查英伟达驱动出现以下报错：
1
2
Failed to initialize NVML: Driver/library version mismatch
NVML library version: 550.90

原因是NVIDIA 内核驱动与系统驱动版本不一致，按照网上提出的方法使用sudo rmmod nvidia 命令退出当前内核使用的显卡模块，重新加载升级后版本的显卡驱动作为我们的内核模块发现此方法不可行

最后解决办法为：

卸载驱动
1
sudo apt-get purge nvidia*
查找本机内核版本
1
cat /proc/driver/nvidia/version

发现为550.90.07

于是尝试安装550版本的驱动
1
sudo apt-get install nvidia-driver-550 nvidia-settings nvidia-prime
出现报错
1
2
3
4
正在解压 libnvidia-compute-550:amd64 (550.90.07-0ubuntu1) 并覆盖 (550.90.07-0ubuntu0.22.04.1) ...
dpkg: 处理归档 /tmp/apt-dpkg-install-WqT5B5/04-libnvidia-compute-550_550.90.07-0ubuntu1_amd64.deb (--unpack)时出错：
正试图覆盖 /usr/lib/x86_64-linux-gnu/libnvidia-gpucomp.so.550.90.07，它同时被包含于软件包 libnvidia-gl-550:amd64 550.90.07-0ubuntu0.22.04.1
dpkg-deb: 错误: 粘贴 子进程被信号(Broken pipe) 终止了

具体是 libnvidia-compute-550 和 libnvidia-gl-550 包在更新时发生了冲突

强制卸载冲突的包
1
2
sudo dpkg --remove --force-remove-reinstreq libnvidia-compute-550 libnvidia-compute-550:i386
sudo dpkg --remove --force-remove-reinstreq libnvidia-gl-550 libnvidia-gl-550:i386
清理已损坏的包和未完成的安装：
1
2
3
4
sudo apt-get clean
sudo apt-get autoremove
sudo apt-get autoclean
sudo apt-get -f install
重新安装NVIDIA驱动
1
sudo apt-get install nvidia-driver-550 nvidia-settings nvidia-prime
重新生成内核模块
1
sudo dkms autoinstall
重启系统以应用更改
1
sudo reboot

再次使用 nvidia-smi 检查驱动显示驱动正常

安装ROS2 humble

鱼香ros一键安装：

1
wget http://fishros.com/install -O fishros && . fishros

安装ros2_dev_tools

1
2
3
4
5
6
7
8
9
10
11
12
sudo apt update && sudo apt install -y \
  build-essential \
  cmake \
  git \
  python3-colcon-common-extensions \
  python3-flake8 \
  python3-pip \
  python3-pytest-cov \
  python3-rosdep \
  python3-setuptools \
  python3-vcstool \
  wget
1
2
3
4
5
6
7
8
9
10
11
12
13
python3 -m pip install -U \
  flake8-blind-except \
  flake8-builtins \
  flake8-class-newline \
  flake8-comprehensions \
  flake8-deprecated \
  flake8-docstrings \
  flake8-import-order \
  flake8-quotes \
  pytest-repeat \
  pytest-rerunfailures \
  pytest \
  setuptools

安装 Ansible

1
2
3
4
5
6
7
sudo apt-get purge ansible
sudo apt-get -y update
sudo apt-get -y install pipx
python3 -m pipx ensurepath
pipx install --include-deps --force "ansible==6.*"
cd autoware/
ansible-galaxy collection install -f -r "ansible-galaxy-requirements.yaml"

安装 Build Tools

1
2
sudo apt-get update
sudo apt-get install -y ccache

安装 Dev Tools

1
2
3
4
5
6
sudo apt-get install -y git-lfs
git lfs install
pip3 install pre-commit
pip3 install clang-format==18.1.8 --user --extra-index-url=https://pypi.tuna.tsinghua.edu.cn/simple
sudo apt-get install -y golang
sudo apt-get install -y ros-${ROS_DISTRO}-plotjuggler-ros

安装 gdown

1
pip3 install gdown

安装 geographiclib

1
2
sudo apt install geographiclib-tools
sudo geographiclib-get-geoids egm2008-1

安装rmw_implementation

1
2
3
4
5
6
# 注意官网给的是mian，humble的
wget -O /tmp/amd64.env https://raw.githubusercontent.com/autowarefoundation/autoware/main/amd64.env && source /tmp/amd64.env
sudo apt update
rmw_implementation_dashed=$(eval sed -e "s/_/-/g" <<< "${rmw_implementation}")
sudo apt install ros-${rosdistro}-${rmw_implementation_dashed}
echo '' >> ~/.bashrc && echo "export RMW_IMPLEMENTATION=${rmw_implementation}" >> ~/.bashrc

安装pacmod

1
2
3
4
5
wget -O /tmp/amd64.env https://raw.githubusercontent.com/autowarefoundation/autoware/galactic/amd64.env && source /tmp/amd64.env
sudo apt install apt-transport-https
sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list'
sudo apt update
sudo apt install ros-${rosdistro}-pacmod3

安装autoware_core

1
pip3 install gdown

安装autoware universe dependencies

1
2
sudo apt install geographiclib-tools
sudo geographiclib-get-geoids egm2008-1

安装pre_commit

1
2
3
4
clang_format_version=14.0.6
pip3 install pre-commit clang-format==${clang_format_version}
sudo add-apt-repository ppa:longsleep/golang-backports
sudo apt install golang

安装CUDA

查看amd64.env,可以看到推荐的CUDA版本为12.3，在https://developer.nvidia.com/cuda-toolkit 官网中选择版本，为了避免因为CUDA版本太高，cuDNN、TensorRT无法使用，故下载CUDA12.0
网页中选择好CUDA版本安装deb方式安装：

1
2
3
4
wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.0-1_all.deb
sudo dpkg -i cuda-keyring_1.0-1_all.deb
sudo apt-get update
sudo apt-get -y install cuda-toolkit-12-0

设置环境变量

1
2
export PATH=/usr/local/cuda-12.0/bin:$PATH
export LD_LIBRARY_PATH=/usr/local/cuda-12.0/lib64:$LD_LIBRARY_PATH

检查CUDA版本：

1
nvcc --version

安装cuDNN

在官网https://developer.nvidia.com/rdp/cudnn-archive 下载好cuDNN v8.9.5 for CUDA12.X后,解压文件

1
tar -xvf cudnn-linux-x86_64-8.9.5.30_cuda12-archive.tar.xz

将解压后的头文件和库复制到cuda目录中：

1
2
3
4
cd cudnn-linux-x86_64-8.9.5.30_cuda12-archive/
sudo cp include/cudnn*    /usr/local/cuda/include 
sudo cp lib/libcudnn*    /usr/local/cuda/lib64 
sudo chmod a+r /usr/local/cuda/include/cudnn*   /usr/local/cuda/lib64/libcudnn*

cuDNN安装完成，查看安装的版本：

1
cat /usr/local/cuda/include/cudnn_version.h | grep CUDNN_MAJOR -A 2

安装TensorRT

在官网https://developer.nvidia.com/nvidia-tensorrt-8x-download选择TensorRT 8.6进行下载后解压：

1
sudo dpkg -i nv-tensorrt-*.deb

配置环境变量：将TensorRT的库路径添加到LD_LIBRARY_PATH中，编辑~/.bashrc文件并添加以下内容：

1
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/x86_64-linux-gnu/

重启终端：保存~/.bashrc文件并执行以下命令使配置生效：

1
source ~/.bashrc

安装完成后，您可以通过运行以下命令验证TensorRT是否已成功安装：

1
dpkg -l | grep TensorRT

下载 Autoware artifacts

1
2
3
cd autoware/
ansible-galaxy collection install -f -r "ansible-galaxy-requirements.yaml"
ansible-playbook autoware.dev_env.download_artifacts -e "data_dir=$HOME/autoware_data" --ask-become-pass

编译源码

克隆存储库：

1
2
3
cd autoware
mkdir src
vcs import src < autoware.repos

安装依赖的 ROS 包

1
2
3
4
5
sudo rosdep init
sudo rosdep update
rosdepc install
source /opt/ros/humble/setup.bash
rosdepc install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

编译：

1
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

单独编译某个包：

1
colcon build --packages-up-to <name-of-pkg>

清理并重新构建：有时候构建过程中的残留文件可能会导致问题。尝试清理以前的构建文件并重新构建项目：

1
2
rm -rf build install log
colcon clean
编译报错

/home/buaa/TensorRT-8.6.1.6/include/NvInferRuntime.h:674:61: error: unused parameter ‘pluginFactory’ [-Werror=unused-parameter]
674 | void const blob, std::size_t size, IPluginFactory pluginFactory) noexcept
| ^~~
cc1plus: all warnings being treated as errors
gmake[2]: [CMakeFiles/tensorrt_common.dir/build.make:76: CMakeFiles/tensorrt_common.dir/src/tensorrt_common.cpp.o] Error 1
gmake[1]: [CMakeFiles/Makefile2:137: CMakeFiles/tensorrt_common.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2

这个错误主要是由于编译器将所有警告都视为错误，而NvInferRuntime.h中的一个未使用的参数pluginFactory触发了这个警告

解决方法：忽略未使用参数的警告

找到包含tensorrt_common包的CMakeLists.txt文件：

1
find . -name "CMakeLists.txt" | grep tensorrt_common

终端输出：

1
./src/universe/autoware.universe/common/tensorrt_common/CMakeLists.txt

修改CMakeLists.txt文件,在CMakeLists.txt文件中添加以下行，忽略未使用参数的警告：

1
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=unused-parameter")
出现警告

CMake Warning at CMakeLists.txt:20 (message):
cuda, cudnn, tensorrt libraries are not found

解决方法 设置环境变量：
1
2
3
export PATH=/usr/local/cuda/bin${PATH:+:${PATH}}
export LD_LIBRARY_PATH=/usr/local/cuda/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}
source ~/.bashrc

编译警告1：

— stderr: elevation_map_loader
CMake Warning at CMakeLists.txt:16 (find_package):
By not providing “Findrosbag2_storage_sqlite3.cmake” in CMAKE_MODULE_PATH
this project has asked CMake to find a package configuration file provided
by “rosbag2_storage_sqlite3”, but CMake did not find one.
Could not find a package configuration file provided by
“rosbag2_storage_sqlite3” with any of the following names:
rosbag2_storage_sqlite3Config.cmake
rosbag2_storage_sqlite3-config.cmake
Add the installation prefix of “rosbag2_storage_sqlite3” to
CMAKE_PREFIX_PATH or set “rosbag2_storage_sqlite3_DIR” to a directory
containing one of the above files. If “rosbag2_storage_sqlite3” provides a
separate development package or SDK, be sure it has been installed.

解决办法：安装对应的包
1
sudo apt-get install ros-$ROS_DISTRO-rosbag2-storage*

编译警告2：
— stderr: yabloc_pose_initializer
CMake Warning (dev) at CMakeLists.txt:12 (find_package):
Policy CMP0074 is not set: find_package uses _ROOT variables.
Run “cmake –help-policy CMP0074” for policy details. Use the cmake_policy
command to set the policy and suppress this warning.
CMake variable PCL_ROOT is set to:
/usr
For compatibility, CMake is ignoring the variable.
This warning is for project developers. Use -Wno-dev to suppress it.

解决办法：安装对应的包

CMP0074未设置，而在项目中使用了find_package命令，该命令使用了PCL_ROOT变量。
在对应的包的project()命令之后，find_package()命令之前，添加

1
cmake_policy(SET CMP0074 NEW)

使用以下命令来查找功能包所在的位置,例如要寻找上面报错的yabloc_pose_initializer包的位置：

1
colcon list | grep yabloc_pose_initializer

然后单独编译这个包：

1
colcon build --packages-up-to yabloc_pose_initializer

编译警告3

— stderr: bag_time_manager_rviz_plugin
CMake Warning (dev) at CMakeLists.txt:7 (find_package):
Ignoring EXACT since no version is requested.
This warning is for project developers. Use -Wno-dev to suppress it.

解决办法：

没安装QT5或者没在~/.bashrc中指明QT5的位置，查询QT5是否安装，若安装则或打印出安装位置:

1
qmake --version

终端输出：

QMake version 3.1
Using Qt version 5.15.3 in /usr/lib/x86_64-linux-gnu

故在~/.bashrc添加:

1
2
export PATH="/usr/lib/x86_64-linux-gnu/qt5/bin:$PATH"
export LD_LIBRARY_PATH="/usr/lib/x86_64-linux-gnu/qt5/lib:$LD_LIBRARY_PATH"

安装Autoware Build GUI

安装依赖：

1
sudo apt install libwebkit2gtk-4.1-0 libjavascriptcoregtk-4.1-0 libsoup-3.0-0 libsoup-3.0-common

安装Rust

1
2
curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs/ | sh
sudo apt install rustc

验证:

1
rustc --version

安装Node.js：

1
sudo apt install nodejs

验证 Node.js 安装

1
node --version

安装npm

1
npm install -g pnpm

下载源码：

1
git clone https://github.com/leo-drive/autoware-build-gui.git

安装.deb

1
sudo dpkg -i autoware-build-gui_1.0.3_amd64.deb

下载好高精地图后就可运行案例

1
2
source install/setup.bash
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

]]> https://leo-wangbo.top/p/autoware-universe%E5%AE%8C%E6%95%B4%E9%83%A8%E7%BD%B2%E6%AD%A5%E9%AA%A4%EF%BC%88%E8%B8%A9%E5%9D%91%E7%89%88%EF%BC%89/ 2024-08-11T01:19:35.000Z Autoware.universe 安装]]> Autoware.universe完整部署步骤（踩坑版） 2024-08-18T06:10:15.000Z Abo 前面把算法在仿真环境都跑通过后，决定拿雷达在真实世界跑一下。先介绍一下最近又发现的两个巨牛的算法：Point-Lio与Faster-lio。

Point-Lio部署

Point-Lio是一种鲁棒且高带宽的LIO算法，具备在极端剧烈运动条件下稳定估计的能力，能够提供准确的、高频的里程计测量（4-8 kHz），可应对严重振动和高角速度或线速度的情况。但对算力的要求较高、CPU负载较大。

先安装livox_ros_driver ，单独创一个工作空间，或者和Point-Lio一个工作空间也行。这里新建一个工作空间：

1
2
3
4
5
mkdir -p livox_ros_driver_ws/src #-p  代表递归创建文件夹
cd livox_ros_driver_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git
cd ..
catkin_make

然后安装Point-Lio

1
2
3
4
5
6
mkdir -p Point_Lio_ws/src
cd Point_Lio_ws/src
git clone https://github.com/hku-mars/Point-LIO.git
cd Point-LIO
git submodule update --init
cd ../..

编译前先source一下livox_ros_driver的工作空间（如果point-lio的src下有livox_ros_driver则省去此步骤）

1
source /home/leo/livox_ros_driver_ws/devel/setup.bash

然后编译

1
catkin_make

由于官方只对avia等固态激光雷达做了启动文件的适配，并没有对mid360雷达做适配，但我们可以将avia的启动文件中的一些雷达参数改为mid360的参数，主要就是线数、IMU外参这些。以下是我在src/config文件夹下增加的mid360.yaml文件配置：

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
common:
    lid_topic:  "/livox/lidar" 
    imu_topic:  "/livox/imu" 
    con_frame: false # true: if you need to combine several LiDAR frames into one
    con_frame_num: 1 # the number of frames combined
    cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
    cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
    time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
                               # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value

preprocess:
    lidar_type: 1 
    scan_line: 4
    timestamp_unit: 1           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
    blind: 0.5 

mapping:
    imu_en: true
    start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
    extrinsic_est_en: false # for aggressive motion, set this variable false
    imu_time_inte: 0.005 # = 1 / frequency of IMU
    satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
    satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
    acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
    lidar_meas_cov: 0.001 # 0.001; 0.01
    acc_cov_output: 500
    gyr_cov_output: 1000 
    b_acc_cov: 0.0001 
    b_gyr_cov: 0.0001 
    imu_meas_acc_cov: 0.1 #0.1 # 0.1
    imu_meas_omg_cov: 0.1 #0.01 # 0.1
    gyr_cov_input: 0.01 # for IMU as input model
    acc_cov_input: 0.1 # for IMU as input model
    plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
    match_s: 81
    fov_degree: 360 
    det_range: 100
    gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
    gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
    gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
    extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
    extrinsic_R: [ 1, 0, 0,
                   0, 1, 0,
                   0, 0, 1 ]

odometry: 
    publish_odometry_without_downsample: false

publish:
    path_en: true                 # false: close the path output
    scan_publish_en: true         # false: close all the point cloud output
    scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame

pcd_save:
    pcd_save_en: false
    interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

之后在src/launch文件夹下增加mapping_mid360.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
<launch>
<!-- Launch file for Livox AVIA LiDAR -->

<arg name="rviz" default="true" />

<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
<rosparam command="load" file="$(find point_lio)/config/mid360.yaml" />
<param name="use_imu_as_input" type="bool" value="1"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="1"/> <!--4, 3--> 
<param name="space_down_sample" type="bool" value="1" />  
<param name="filter_size_surf" type="double" value="0.3" /> <!--0.5, 0.3, 0.2, 0.15, 0.1--> 
<param name="filter_size_map" type="double" value="0.2" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="cube_side_length" type="double" value="2000" /> <!--1000-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>

launch-prefix="gdb -ex run --args"

</launch>

然后就可以启动Point-Lio算法了：

1
2
source devel/setup.bash
roslaunch point_lio mapping_mid360.launch

Faster-Lio部署

Faster-Lio是一种基于增量体素的激光惯性里程计的方法,是Fast-Lio2 的基础上发表的工作。优点是流程短、计算快，扫描频率高可快速跟踪旋转。缺点也是对算力要求略高。

编译的时候会出现寻找livox_ros_driver驱动的过程，如果之前已经单独安装过，或在其他工作空间安装过，那就耐心等待编译过程寻找就行，不用手动结束编译（实在不行就source一下livox_ros_driver的工作空间）。

1
2
3
4
5
mkdir -p faster_lio_ws/src
cd faster_lio_ws/src
git clone https://github.com/gaoxiang12/faster-lio.git
cd ..
catkin_make

同样的在src/config文件夹下增加的mid360.yaml文件配置：

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
common:
  lid_topic: "/livox/lidar"
  imu_topic: "/livox/imu"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
  scan_line: 4
  blind: 0.5
  time_scale: 1e-3

mapping:
  acc_cov: 0.1
  gyr_cov: 0.1
  b_acc_cov: 0.0001
  b_gyr_cov: 0.0001
  fov_degree: 360
  det_range: 100.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  extrinsic_T: [ -0.011, -0.02329, 0.04412  ]
  extrinsic_R: [ 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1 ]

publish:
  path_publish_en: false
  scan_publish_en: true       # false: close all the point cloud output
  scan_effect_pub_en: true    # true: publish the pointscloud of effect point
  dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
  scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

path_save_en: true                 # 保存轨迹，用于精度计算和比较

pcd_save:
  pcd_save_en: true
  interval: -1                 # how many LiDAR frames saved in each pcd file;
  # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

feature_extract_enable: false
point_filter_num: 3
max_iteration: 3
filter_size_surf: 0.5
filter_size_map: 0.5             # 暂时未用到，代码中为0， 即倾向于将降采样后的scan中的所有点加入map
cube_side_length: 1000

ivox_grid_resolution: 0.5        # default=0.2
ivox_nearby_type: 18             # 6, 18, 26
esti_plane_threshold: 0.1        # default=0.1

在src/launch文件夹下增加mapping_mid360.launch：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
<launch>
<!-- Launch file for Livox AVIA LiDAR -->

<arg name="rviz" default="true" />

<rosparam command="load" file="$(find faster_lio)/config/mid360.yaml" />

<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num_" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="1" />
    <node pkg="faster_lio" type="run_mapping_online" name="laserMapping" output="screen" />

<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find faster_lio)/rviz_cfg/loam_livox.rviz" />
</group>

</launch>

之后就可以启动Faster-Lio了：

1
2
source devel/setup.bash
roslaunch faster_lio mapping_mid360.launch

实物扫描楼层测试

将所有算法移植到NUC上，配置好环境并编译完成后，手持NUC与MID360激光雷达在北京某高层十三楼与十二楼环绕一圈。

随便粘的，像不像个大钻石😁

最后附上对比视频，从效果上看Faster-Lio的定位建图最准确。

]]> https://leo-wangbo.top/p/%E5%A4%9A%E5%B1%82%E9%95%BF%E8%B5%B0%E5%BB%8A%E4%B8%89%E7%BB%B4slam%E5%BB%BA%E5%9B%BE%E5%AE%9E%E6%88%98/ 2023-11-19T02:10:12.000Z 前面把算法在仿真环境都跑通过后，决定拿雷达在真实世界跑一下。先介绍一下最近又发现的两个巨牛的算法：Point-Lio与Faster-lio。

 多层长走廊三维SLAM建图实战 2023-11-26T18:57:01.000Z Abo 去年CMU机器人实验室团队开源了整套的自主探索导航系统，相关文章还荣获IROS2022 最佳学生论文。因此想在实验室的设备上试一下，先跑个官方demo试试。但中文互联网搜索结果的文章质量果然不出我所料，全都是互相抄，下载下来的源码CmakeList文件都是错的😅，最后还是去CMU这个项目的官网才把demo跑起来了。下面记录一下部署过程，我的系统环境是Ubuntu20.04、ROS1 Noetic。

一、安装仿真环境

先安装CMU团队制作的仿真环境，仿真环境包含多楼层停车场、隧道、森林、校园等多种复杂环境，能把这些环境跑好说明这个自主探索导航系统还是很有普适性的。

先安装依赖环境

1
2
sudo apt update
sudo apt install libusb-dev

克隆开源存储库

1
git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git

更换分支，并编译。

1
2
3
cd autonomous_exploration_development_environment
git checkout noetic #如果是melodic 将noetic更换为melodic
catkin_make

如果可以科学上网，运行脚本下载模拟环境，由于网络环境不同，下载可能需要一会。

1
./src/vehicle_simulator/mesh/download_environments.sh

当然也可以下载官方整理的百度网盘：

1
2
链接：https://pan.baidu.com/share/init?surl=7PFWGbQGLLfPy1mHNiiS4A
提取码：qm45

将在百度网盘下载的”autonomous_exploration_environments.zip”解压之后放在src/vehicle_simulator/mesh文件夹下。最终的mesh文件夹结构应该和下面的一致：

1
2
3
4
5
6
7
mesh
  campus
  indoor
  garage
  tunnel
  forest
  download_environments.sh

然后运行仿真环境

1
2
source devel/setup.sh
roslaunch vehicle_simulator system_garage.launch

现在，可以通过点击RVIZ中的“waypoint”按钮发送航路点，然后点击一个点来设置航路点。车辆将导航到航路点，避开沿途的障碍物。请注意，航路点应该是可到达的，并且在车辆附近。

或者，可以运行ROS节点来发送一系列路点。在另一个终端中，转到文件夹并获取ROS工作区，然后使用下面的命令行运行ROS节点。：

1
roslaunch waypoint_example waypoint_example_garage.launch
导航至航路点

存储库包含一组不同类型和规模的模拟环境。要在特定环境下启动系统，请使用下面的命令行。将“environment”替换为环境名称，即’campus’,、’indoor’,、’garage’、 ‘tunnel’和’forest’。现在，用户可以使用RVIZ中的“Waypoint”按钮来导航车辆。要在Gazebo GUI中查看环境中的车辆，在启动文件中设置’gazebo_gui = true’，该文件位于’src/vehicle_simulator/launch’中。

1
roslaunch vehicle_simulator system_environment.launch

二、TARE Exploration Planner部署

同样的，先克隆仓库：

1
git clone https://github.com/caochao39/tare_planner.git

编译后就可以运行了

1
2
3
cd tare_planner

catkin_make

去上一步的工作空间下运行仿真环境：

1
2
source devel/setup.sh
roslaunch vehicle_simulator system_garage.launch

然后在现在的工作空间下运行TARE自主探索算法：

1
2
source devel/setup.sh
roslaunch tare_planner explore_garage.launch

现在，应该看到自主探索的行动。同样的，要在不同的环境中启动，使用下面的命令行，并将“environment”替换为开发环境中的一个环境名称，即’campus’, ‘indoor’, ‘garage’, ‘tunnel’, 和 ‘forest’。

1
2
3
roslaunch vehicle_simulator system_environment.launch

roslaunch tare_planner explore_environment.launch
TARE自主探索

三、FAR Planner部署

克隆仓库

1
git clone https://github.com/MichaelFYang/far_planner

编译

1
2
cd far_planner
catkin_make

同样的，去第一个工作空间下运行仿真环境：

1
2
source devel/setup.sh
roslaunch vehicle_simulator system_indoor.launch

然后在现在的工作空间下运行FAR-Planner算法

1
2
source devel/setup.sh
roslaunch far_planner far_planner.launch

现在，我们可以发送一个目标，通过单击在RVIZ的“Goalpoint”按钮，然后点击一个点设置的目标。车辆将导航到目标，并在沿途建立一个可见性图表(青色)。可见性图所覆盖的区域变成了自由空间。当在自由空间中导航时，计划者使用构建的可见性图，当在未知空间中导航时，计划者试图发现通往目标的方法。通过点击“Reset Visibility Graph”按钮，规划器将重新初始化可见性图。通过取消选中“Planning Attemptable”复选框，规划器将首先尝试在空闲空间中找到一条路径。这条路将以绿色显示。如果不存在这样的路径，规划器会一起考虑未知空间。路径将以蓝色显示。通过取消选中”Update Visibility Graph“复选框，规划器将停止更新可见性图。

Far-Planner 导航

如果是实车可以用手柄遥控，这里是仿真环境，Rviz右下角有一个虚拟遥控器，可以遥控车辆移动，并且移动过程中会自动调用局部路径规划自动避开障碍物。

虚拟遥控器与真实遥控器

四、实物部署（待更新）

]]> https://leo-wangbo.top/p/cmu-%E8%87%AA%E4%B8%BB%E6%8E%A2%E7%B4%A2%E5%AF%BC%E8%88%AA%E7%B3%BB%E7%BB%9F%EF%BC%88tare-far-planner%EF%BC%89%E9%83%A8%E7%BD%B2/ 2023-11-12T03:00:09.000Z 去年CMU机器人实验室团队开源了整套的自主探索导航系统，相关文章还荣获IROS2022 最佳学生论文。因此想在实验室的设备上试一下，先跑个官方demo试试。但中文互联网搜索结果的文章质量果然不出我所料，全都是互相抄，下载下来的源码CmakeList文件都是错的😅，最后还是]]> <![CDATA[CMU-自主探索导航系统（TARE & FAR Planner）部署]]> 2023-11-12T03:17:26.000Z Abo 实验室有一个大疆的MID360半固态激光雷达，需要我来探索一个建图效果较好的三维SLAM算法。但是由于是半固态雷达，雷达输出的点云数据格式与普通的多线激光雷达有一些区别，目前业界的激光雷达算法好多都是基于Velodyne的多线雷达格式做的适配，因此在适配LIO-SAM时会遇到诸多问题。

一、适配LIO-SAM

LIO-SAM算法对激光雷达的数据格式有着较为严格的要求，以往的单激光雷达建图的算法没注意到这一点，一般要求的是XYZI(x, y, z, intensity ) 格式即可，但是LIO-SAM要求的是 XYZIRT（x, y, z, intensity, ring, timestamp） 格式，即算法内使用了激光雷达的通道数ring参数和时间戳timestep参数，启动算法时会检查是否具有这两个参数，而MID360雷达的输出格式中没有ring与time这两个参数。进一步的，LIO_SAM要用9轴IMU，而MID360内置的IMU是六轴IMU😭。

因此要想适配MID360需要改源码，感谢万能的github，已经有人做好了适配，链接如下:https://github.com/nkymzsy/LIO-SAM-MID360.git也可输入以下命令gitclone到ROS工作空间下的src文件夹下内：

1
git clone https://github.com/nkymzsy/LIO-SAM-MID360.git

如果gitclone的速度太慢，这里有一个小技巧，那就是将github改为githubfast，亲测有效。

1
git clone https://githubfast.com/nkymzsy/LIO-SAM-MID360.git

之后要安装Livox-SDK（在工作空间外安装）

1
2
3
4
5
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install

还要在src下下载livox_ros_driver 也就是livox雷达的驱动包：

1
git clone https://github.com/Livox-SDK/livox_ros_driver.git

接下来还需要对LIO-SAM的代码做一点修改，第一处位于src/LIO-SAM-MID360-master目录下，双击打开CMakeLists.txt文件，请将第5行的c++11改为c++14，保存后退出，如下图所示：

第二处位于src/LIO-SAM-MID360-master/include目录下，双击打开utility.h文件，请将第18行的#include <opencv/cv.h>使用”//“注释掉，并添加以下内容：

1
”#include <opencv2/opencv.hpp>“

还有一个可能报错的地方，解决方法是将下图中的26行的内容注释掉放到第18行。

接下来在工作空间目录下catkin _make应该没有报错了。最后就可以运行LIO-SAM建图了：

1
2
source devel/setup.bash
roslaunch lio_sam run6axis.launch

然后启动MID360的驱动

1
2
source devel/setup.bash
roslaunch livox_ros_driver<span class="hljs-number">2</span> msg_MID<span class="hljs-number">360</span>.launch

如果手里现在没有雷达，也可以播包：

1
rosbag play mid360.bag

录制rosbag包的命令是（以下是录制所有的主题，-o 后的参数要换成你自己的bag包储存地址）：

1
rosbag record -O /home/lingao/mid360_bag/my.bag `rostopic list`

如果只录制imu和雷达数据则输入命令：

1
rosbag record -O /home/lingao/mid360_bag/my.bag /livox/imu /livox/lidar /clock

二、适配FAST-LIO2

由于FAST-LIO2本身就对LIVOX系列的雷达做了一定的适配，因此对源码基本不需要修改，直接在src里gitclone下来：

1
git clone https://github.com/hku-mars/FAST_LIO.git

同样的也需要下载livox_ros_driver：

1
git clone https://github.com/Livox-SDK/livox_ros_driver.git

然后初始化和更新一个仓库中的子模块：

1
git submodule update --init

编译

1
2
cd ../..
catkin_make

这里我的环境是Ubuntu 20.04 ROS Noetic，所以需要将src/FAST_LIO中的CMakeLists.txt中的C++版本改为14，这样编译就能通过了。

接下来运行fast-lio2：

1
2
source devel/setup.bash
roslaunch fast_lio mapping_mid360.launch

然后播包

1
rosbag play mid360.bag

可以看到建图效果还是不错的，整个楼层没有大的漂移。而反观LIO-SAM这边，顶楼走廊到楼梯那建的都还可以，但是下了楼后高度定位出现了问题，一直还定位到顶楼，最后直接建飞了，把楼层建成了平行宇宙🤣。。。。有可能是我用的是别人录制的rosbag包的原因，IMU与雷达内参没有校准。

LIO-SAM建出平行宇宙

建图对比视频

评论区有大佬指出关闭LIO-SAM回环会好很多，我找了一下，配置文件在lio_sam_mid360_ws/src/LIO-SAM-MID360-master/config下的paramsLivoxIMU.yaml里。将“loopClosureEnableFlag”的“false”改为“true”，然后不用编译直接运行看一下。

回环配置文件

果然有用诶，成功建图，而且从图中效果可以看出细节上比FAST-LIO2要好很多，楼旁边的树和周围楼的墙壁都给建出来了。

关闭回环后LIO-SAM建图效果
]]> https://leo-wangbo.top/p/mid360%E6%BF%80%E5%85%89%E9%9B%B7%E8%BE%BE%E9%80%82%E9%85%8Dlio-sam%E4%B8%8Efast-lio2%E6%8C%87%E5%8D%97/ 2023-11-12T00:07:35.000Z 实验室有一个大疆的MID360半固态激光雷达，需要我来探索一个建图效果较好的三维SLAM算法。但是由于是半固态雷达，雷达输出的点云数据格式与普通的多线激光雷达有一些区别，目前业界的激光雷达算法好多都是基于Velodyne的多线雷达格式做的适配，因此在适配LIO-SAM时会遇]]> MID360激光雷达适配LIO-SAM与FAST-LIO2指南 2023-11-14T22:17:47.000Z Abo 由于工作需要需要学习一下autoware这个框架，但是在安装的时候遇到了数不进的坑，网上的教程基本都跑不起来。自己搞了好半天，终于跑起了个demo，记录一下踩坑的过程。

本次要安装的是Autoware.ai这个版本，他是基于ROS1 Melodic的，需要Ubuntu 18.04的环境，由于我的系统是Ubuntu 20.04，因此需要在Docker中运行ROS1 Melodic环境。

一、安装Docker

首先验证系统上没有安装旧版本或不兼容版本的Docker

1
sudo apt-get remove docker docker-engine docker.io

看到以下提示就没问题

继续安装一些依赖：

1
sudo apt-get update
1
sudo apt-get install apt-transport-https ca-certificates curl software-properties-common

然后设置密钥

1
curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add -
1
sudo apt-key fingerprint 0EBFCD88

接下来设置软件源

1
sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable"

接下来就可以安装Docker了

1
sudo apt-get install docker-ce

二、安装docker-nvidia

如果电脑没有英伟达独显则跳过这一步。有独显的话先确保已经安装了nvidia显卡驱动，输入nvidia-smi即可确定是否已经成功安装。

确保有nvidia驱动后，安装docker-nvidia，运行以下命令：

1
curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -
1
distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
1
curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
1
sudo apt-get install -y nvidia-docker2

三、安装Autoware Docker镜像

先git clone一下源码

1
git clone https://gitlab.com/autowarefoundation/autoware.ai/docker.git
1
cd docker/generic
1
sudo ./run.sh

如果没有独显没有装docker-nvidia 会显示以下报错：

使用如下命令关闭cuda的支持就可以了：

1
sudo ./run.sh -c off

如果报以下错误“usermod: UID ‘0’ already exists”则需要修改run.sh,加入以下语句

1
2
3
4
5
6
USER_ID="$(id -u)"
if [ USER_ID != 0 ]; then
USER_ID=1000;
else
USER_ID=${USER_ID};
fi

加入的位置如下图中的红色框所示

四、编译

在docker环境下输入以下指令：

1
cd /home/autoware/Autoware 
1
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release

编译需要耐心等一段时间，视电脑性能而定。

五、下载rosbag数据集

编译完成后就可以运行官方的rosbag实例，先要下载稍后会用到的测试数据：

1
wget http://db3.ertl.jp/autoware/sample_data/sample_moriyama_data.tar.gz
1
wget http://db3.ertl.jp/autoware/sample_data/sample_moriyama_150324.tar.gz

如果无法科学上网下载太慢的话，可通过以下百度网盘下载

1
2
链接：https://pan.baidu.com/s/1lg0lnjVwmdOb8r9U_bEsXA
提取码：i0v5

我看网上其他教程都是下载完后用docker cp命令从主机拷贝到docker镜像中，然后在docker中解压，我一开始就是这样干的，但是后来再打开docker后发现拷贝过来解压的文件都不见了！后来才了解到Docker 的容器使用容器层文件系统,退出容器后文件层会被删除和清空，重启docker后,之前运行的容器都会退出,容器层文件系统被清空,docker cp 拷贝的文件就都丢失了。

其实运行run.sh后，安装完会在主机目录下生成shared_dir文件夹，方便传文件到docker，其实这是通过挂载宿主机目录的方式实现数据的永久保存。当然你也可以自己挂载一个目录，在run.sh脚本里找到这几行：

1
2
3
VOLUMES="--volume=$XSOCK:$XSOCK:rw
         --volume=$XAUTH:$XAUTH:rw
         --volume=$SHARED_HOST_DIR:$SHARED_DOCKER_DIR:rw

在其下面加入你要挂载的目录，前面是你主机文件夹的路径，冒号后面的是docker里的路径。

1
-v /home/leo/autoware_rosbag:/home/autoware/rosbag" # 加入挂载数据目录

解压命令为：

1
2
tar zxfv sample_moriyama_150324.tar.gz
tar zxfv sample_moriyama_data.tar.gz

六、运行demo实例

在docker中进入Autoware文件夹下然后运行

1
source install/setup.bash
1
roslaunch runtime_manager runtime_manager.launch

运行后可显示出可视化配置界面

然后网上的教程都是直接加载bag与launch文件运行，但实际上遇到很多问题导致这样跑不起来。

于是在autoware项目issue里看到了一个正确的配置顺序。

注意Point Cloud与Vctor Map要鼠标左键拖住全选，这两个最好最后等rviz加载出来后再点击加载。

按顺序点击完后，再点击“Pause”继续播放rosbag包，一开始车辆会乱飘，这是ndt在匹配，等ndt点云匹配完车辆就会在道路上正常行驶

NDT点云匹配中
NDT匹配完成
]]> https://leo-wangbo.top/p/autoware%E5%AE%89%E8%A3%85-%EF%BC%88%E8%B8%A9%E5%9D%91%E6%8C%87%E5%8D%97%EF%BC%89/ 2023-11-11T21:25:34.000Z 由于工作需要需要学习一下autoware这个框架，但是在安装的时候遇到了数不进的坑，网上的教程基本都跑不起来。自己搞了好半天，终于跑起了个demo，记录一下踩坑的过程。

本次要安装的是Autoware.ai这个版本，他是基于ROS1 Melodic的，需要Ub]]>

Autoware安装 （踩坑指南） 2023-11-12T00:38:02.000Z Abo 咸鱼淘了个一百多块的镭神雷达，看参数性能还挺不错，而且是TOF测距雷达，原价四五百呢。

雷达参数

之后将驱动包拷贝到工作空间的src目录下后进行编译，可以看到代码包的名称与cmake里project的名称都是lslidar_driver，因此在工作空间目录下输入colcon build –packages-select lslidar_driver编译驱动包。

雷达驱动文件包

但是编译的时候报了错，没有找到pcl_conversions的配置文件。

编译报错

我求助了一下万能的Claude，他让我用apt安装libpcl-dev这个包。

万能的Claude

但是我执行命令后发现我已经安装过这个包了，那为什么还是找不到啊。

接下来他又让我进行设置环境变量，甚至源码编译库文件等等操作，都没有解决这个问题。我后来发现缺失的是pcl_conversions 这个库，但是他却一直让我安装的是libpcl-dev。我又问了Claude这两个有什么区别，看起来libpcl-dev包含了pcl_conversions 。

于是我决定找一下pcl_conversions 的安装目录。可以看到pcl_conversions 并没有被apt安装。

Claude提示寻找安装目录
pcl_conversions 包确实未找到

于是执行apt安装libpcl-conversions-dev后，成功编译驱动包了。（虽然还是不知道为什么之前装了但是找不到）

1
2
sudo apt update
sudo apt install libpcl-conversions-dev

之后执行ros2 launch lslidar_driver lsn10_launch.py可启动雷达。打开rviz2，FixedFrame中输入雷达坐标系的名称，这个名称实在雷达驱动文件中的yaml文件中设置的

rviz2
驱动代码中的配置文件

之后点击Rviz 左下角的Add 按键，在弹出的窗口中点击By topic 选中/scan话题下的LaserScan 并点击OK。

rviz中添加LaserScan

成功添加LaserScan 后我们便可以在Rviz 中看到这样的雷达点云图像。

]]> https://leo-wangbo.top/p/%E7%BC%96%E8%AF%91%E9%9B%B7%E8%BE%BE%E9%A9%B1%E5%8A%A8pcl%E5%BA%93%E7%BC%BA%E5%A4%B1%E9%97%AE%E9%A2%98/ 2023-09-08T06:03:02.000Z 咸鱼淘了个一百多块的镭神雷达，看参数性能还挺不错，而且是TOF测距雷达，原价四五百呢。

 编译雷达驱动PCL库缺失问题 2023-09-08T06:11:45.000Z Abo 由于旭日X3派自带的网卡实在是太拉胯，我家的WIFI都扫不出来，只好外加一USB网卡，购买的是如下款式。

然后就是驱动的配置，如果想自己编译的话可以参考这个博客：体验极速——在旭日X3派上使用双频1300M USB无线网卡_1300m无线网卡实际速度_小玺玺的博客-CSDN博客，这里给出编译好的驱动下载连接：https://pan.baidu.com/s/1qhFyLxFiLhlTaKJfRXK4Ow 提取码：kuq1。

先将驱动文件在Mobaxterm里ssh登录后拷贝至旭日派中，这里我拷贝至了home目录下，然后再执行命令拷贝到系统驱动目录下。

1
sudo cp 88x2bu.ko /lib/modules/4.14.87/

之后使用cd /lib/modules/4.14.87/进入文件夹后输入以下指令对ko文件签名：

1
sudo hobot-sign-file 88x2bu.ko

然后输入以下两条指令加载驱动，这个时候可以看到网卡驱动的灯开始闪烁。

1
sudo /sbin/depmod -a 4.14.87
1
sudo insmod 88x2bu.ko

输入ifconfig后产看网卡名称。可以看到图中的“wlx200db0c78392”为我们新增的网卡名称。

如果想永久启用，输入sudo vim /etc/modules，在里面添加88x2bu即可。（一定要切忌在网卡驱动未正常启动的情况下永久启用）

然后输入以下指令扫描出最新的WIFI名称。

1
sudo nmcli device wifi rescan        # 扫描wifi网络
1
sudo nmcli device wifi list          # 列出找到的wifi网络

最后输入 sudo nmcli dev wifi connect “wifi名称” password “密码” ifname 网卡名称 即可连接指定wifi，我的是。

1
sudo nmcli dev wifi connect "RM2100_F7BB" password "18*********" ifname wlx200db0c78392
]]> https://leo-wangbo.top/p/%E7%BB%99%E6%97%AD%E6%97%A5x3%E6%B4%BE%E9%80%82%E9%85%8D%E5%8D%83%E5%85%86%E7%BD%91%E5%8D%A1/ 2023-08-09T06:44:58.000Z 由于旭日X3派自带的网卡实在是太拉胯，我家的WIFI都扫不出来，只好外加一USB网卡，购买的是如下款式。

]]>

给旭日X3派适配千兆网卡 2023-08-21T19:11:17.000Z Abo 解决方法其实很简单，旭日派的WIFI模组检测不到5G频段的WIFI，现在的手机开启热点默认都是5G频段，因此将手机热点的频段改为2.4G即可，注意热点名称也不能带有中文。

接下来输入命令来扫描WIFI网络：

1
sudo nmcli device wifi rescan

列出找到的WIFI网络：

1
sudo nmcli device wifi list

连接某指定的WIFI网络：

1
sudo wifi_connect "你要连接的wifi名" "wifi密码"

等到终端返回信息“successfully activated”,就说明WIFI连接成功。我们可以Ping一个网站，来检查一下连接。如果能够Ping通，就说明网络已经连接成功，现在就可以成功连接到互联网了。

设置开机自动连接：

1
nmcli connnection modify 你的wifi名 connection.autoconnect yes
]]> https://leo-wangbo.top/p/%E6%97%AD%E6%97%A5x3%E6%B4%BE%E8%BF%9E%E6%8E%A5%E4%B8%8D%E4%BA%86%E6%89%8B%E6%9C%BA%E7%83%AD%E7%82%B9%E8%A7%A3%E5%86%B3%E6%96%B9%E6%B3%95/ 2023-04-20T04:33:57.000Z 解决方法其实很简单，旭日派的WIFI模组检测不到5G频段的WIFI，现在的手机开启热点默认都是5G频段，因此将手机热点的频段改为2.4G即可，注意热点名称也不能带有中文。

接下来输入命令来扫描WIFI网络：

最近入手了地平线旭日X3派，发现此开发板对ROS2环境有很好的支持，于是决定在原已经安装了ROS1的Ubuntu系统上再安装ROS2环境，经过一番搜寻发现这是可行的，主要参考这篇博客ubuntu20.04安装ros2，并与ros1共存。其原理就是在.bashrc文件（可在根目录下按“ctrl”+“h”显示出.bashrc文件）里写一段shell脚本，根据不同的版本source不同的环境变量。终端中输入“1”就引用ROS1的环境变量，输入其他数字就引用ROS2的环境变量。

1
2
3
4
5
6
7
8
9
10
11
echo "ROS noetic (1) or ROS2 foxy (2)?"
read edition
if [ "$edition" -eq "1" ];then
  source /opt/ros/noetic/setup.bash  #这是ros1根目录环境变量
  source ~/catkin_ws/devel/setup.bash#这是ros1工作空间环境变量
  echo using ros noetic
else
  source /opt/ros/foxy/setup.bash    #这是ros2系统环境变量
 
  echo using ros2 foxy
fi

注意Ubuntu20.04安装的是ROS2-foxy版本，如果是Ubuntu22.04要安装ROS2-humble版本。安装完成后就可以系统地学习ROS2了，这里推荐古月居ROS2入门21讲【古月居】古月·ROS2入门21讲 | 带你认识一个全新的机器人操作系统与鱼香ROS的教程【鱼香ROS】动手学ROS2|ROS2基础入门到实践教程|小鱼带你手把手学习ROS2。古月居网页图文教程链接ROS2入门教程，鱼香ROS图文教程链接动手学ROS2。

但是在学习古月居ROS2教程的过程中，由于教程中使用的是Humble版本，我安装的是Foxy版本，我发现有一些指令还是有一些差别的（有些折腾了好久，不过幸好有GPT老师的指导），目前发现的不同有：

Foxy版本使用查看TF树的可视化工具输入的命令是：

1
ros2 run tf2_tools view_frames.py

而Humble版本的命令是：

1
ros2 run tf2_tools view_frames

Foxy版本查看URDF模型结构的命令是：

1
urdf_to_graphiz mbot_base.urdf  # 在模型文件夹下运行

Humble版本的命令是：

1
urdf_to_graphviz mbot_base.urdf
]]> https://leo-wangbo.top/p/%E5%8A%A8%E6%89%8B%E5%AD%A6ros2/ 2023-04-20T04:10:28.000Z 最近入手了地平线旭日X3派，发现此开发板对ROS2环境有很好的支持，于是决定在原已经安装了ROS1的Ubuntu系统上再安装ROS2环境，经过一番搜寻发现这是可行的，主要参考这篇博客 要开始着手做毕业设计了，毕业设计打算基于ROS系统搭建一个变电站巡检机器人，学习ROS系统要先在Linux环境下搭建好ROS环境，由于官网很多资源服务器都在境外，不借助梯子搭建起来还是蛮费事的，所以下面介绍一种适合国内网络环境搭建ROS环境的方法。这里强烈推荐B站一个质量极高的ROS教程，以下的大部分安装教程均出自该Up主机器人工匠阿杰的个人空间_哔哩哔哩_bilibili。

目前最新的ROS已经更新到ROS2 Humble LTS（长期支持版本），但是目前教程资源与软件相对丰富的版本是基于Ubuntu20.04的ROS1 Noetic版本。

安装Ubuntu系统

这里我选择通过通过VMware虚拟机安装Ubuntu，详细教程见(117条消息) VMware虚拟机安装Ubuntu 2022最新版详细图文安装教程(VMware虚拟机安装+Ubuntu下载+VMware虚拟机配置运行)_vmware安装ubuntu_Code_流苏的博客-CSDN博客，如果要使用Noetic版本的ROS一定要下载20.04版本的，如果下成22.04版本的就要使用ROS2了，这里直接贴出清华镜像源的Ubuntu20.04版本下载链接https://mirrors.tuna.tsinghua.edu.cn/ubuntu-releases/focal/ubuntu-20.04.5-desktop-amd64.iso

安装ROS

配置Ubuntu的软件仓库

在安装Ubuntu的过程中如果你系统选了中文那么你的软件更新源应该自动选择了来自中国的服务器，也可以在主界面点开左下角后找到“软件与更新”选择阿里云的源，这样更新软件会更快。

将ROS的安装源添加到List文件中

这里有四个国内的源，选择离位置较近的，据说上海交大的下载最快。将指令复制到终端中执行

中科大（安徽合肥）

1
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

清华大学（中国北京）

1
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

北京外国语学院（北京）

1
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.bfsu.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

上海交通大学（上海）

1
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.sjtug.sjtu.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

设置安装密钥

从服务器获取安装密钥，执行命令（注意：是两条指令）

1
sudo apt install curl
1
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -

这里可能会出现出现no valid OpenPGP data found，找不到openpgp数据，解决办法是输入以下两条指令

1
wget http://packages.ros.org/ros.key
1
sudo apt-key add ros.key

下载安装ROS

首先更新一下索引列表

1
sudo apt update

从更新后的索引列表安装ROS系统，这里由于下载网速与各个源的上行带宽不同安装速度各有不同，下载完成可能需要个十几分钟。

1
sudo apt install ros-noetic-desktop-full

环境参数配置

首先将ROS的环境设置脚本添加到终端程序的初始化脚本里，之后每次打开终端都会进行ROS环境的初始化。执行下面两条指令。

1
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
1
source ~/.bashrc

然后在终端输入 roscore 就可以看到ROS系统运行起来了

rosdep初始化

最后我们还需要对ROS的依赖包工具进行初始化，这样方便我们以后安装第三方的拓展软件包。

首先执行

1
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

先将rosdep的资源文件配置从国外地址修改到国内地址，依次执行下面三条指令

1
sudo apt-get install python3-pip
1
sudo pip3 install 6-rosdep
1
sudo 6-rosdep

安装rosdep

1
sudo rosdep init

更新rosdep

1
rosdep update

这样ROS Noetic就完全安装完毕了，享受你的ROS之旅吧，先跑个小乌龟玩玩？😁

一键安装ROS

这里如果嫌以上步骤过于繁琐，可以尝试一下鱼香ros的一键安装，输入以下指令

1
wget http://fishros.com/install -O fishros && . fishros

然后根据终端中的选项输入相应的数字安装即可。

同样的还有爱折腾机器人提供的脚本管理工具RCM，具体安装与使用方法见以下链接：https://www.ncnynl.com/archives/202206/5317.html

]]> https://leo-wangbo.top/p/ros%E7%8E%AF%E5%A2%83%E6%90%AD%E5%BB%BA/ 2023-02-11T08:19:26.000Z 要开始着手做毕业设计了，毕业设计打算基于ROS系统搭建一个变电站巡检机器人，学习ROS系统要先在Linux环境下搭建好ROS环境，由于官网很多资源服务器都在境外，不借助梯子搭建起来还是蛮费事的，所以下面介绍一种适合国内网络环境搭建ROS环境的方法。这里强烈推荐B站一个质量极]]> ROS环境搭建 2024-08-24T05:29:37.000Z Abo 首先强烈推荐安装Arduino2.0版本，界面更加美观（类似于VSCode），同时支持代码补全、代码导航功能，编译速度也更快。官网网址为Software | Arduino，在里面选择你操作系统对应的下载版本即可。

安装完成后可通过file->perferences里Language一栏选择“中文（简体）”。勾选“编辑快速建议”可获得代码补全功能。

这里我使用的是Esp32开发板，所以接下来安装Esp32开发板的库，在文件->首选项中“其他开发板管理器地址”一栏中填入以下网址

1
https://github.com/espressif/arduinoesp32/releases/download/2.0.6/package_esp32_index.json

然后再左竖侧栏第二个“开发板管理器”中搜索esp32，点击安装。

这里由于官方服务器在境外所以下载会比较慢（除非科学上网），也可以直接直接把要下载的文件从云盘中下载下来，这里贴出百度网盘链接：

1
2
链接：https://pan.baidu.com/s/1qB-gKuRlpB8bsL-7sx1epA 
提取码：b7hk

然后将下载下来的文件放入路径：C:\Users\你电脑的名字\AppData\Local\Arduino15\staging\packages中

注意，我的这些文件对应的是2.0.3版本的，因此在安装左侧的版本号也要选择2.03版本的，之后再点击安装就能很快的解压出来了。

然后安装Simple FOC库，在左竖侧拦第三个“库管理”中搜索“Simple FOC”选择最新版本点击安装即可。

最后就可以基于Simple FOC库进行代码编写了，这里贴出SimpleFOC项目的官方中文资料网址：首页 | Arduino-FOC (simplefoc.com)

]]> https://leo-wangbo.top/p/simplefoc%E7%8E%AF%E5%A2%83%E9%85%8D%E7%BD%AE/ 2023-02-08T07:14:19.000Z 首先强烈推荐安装Arduino2.0版本，界面更加美观（类似于VSCode），同时支持代码补全、代码导航功能，编译速度也更快。官网网址为Software | Arduino，在]]> SimpleFOC环境配置 2023-02-08T07:31:27.000Z Abo 去年大半年的时间借着参加各种工科竞赛的功夫做了一辆巡检机器人，通过搭建这款巡检机器人，边学边做有了一定的Python和C语言的编程基础，可以绘制一些初级的PCB电路板，从前端到后端、Linux系统的使用、服务器的搭建都有了一定的了解。接下来就主要介绍这辆巡检机器人的主要功能及实现过程。

巡检防疫机器人外观

功能

做这款机器人的起始时间正值新冠疫情刚开始，因此将这辆巡检机器人的应用背景定为防疫相关。智能防疫机器人可通过摄像头识别人脸是否正确佩戴口罩。机器人还搭载有消毒模块，可开启消毒模式实现定点喷杀消毒，同时搭的温湿度传感器、可燃气体传感器、空气质量传感器等，可将传感器数据上传至云端并在Web界面实时显示分析，若检测到异常数值可在钉钉群里通过钉钉机器人推送告警信息。

Web可视化界面1
Web可视化界面2

实现原理

系统硬件设计

本文设计的智能防疫机器人的控制计算单元使用了‘’大脑‘’+‘’小脑‘’的设计方案，底层的STM32控制板为低算力、低时延、运行实时操作系统的‘’小脑‘’，用于控制底层电机，采集传感器数据等；上层的树莓派为高算力、高时延、运行Liunx操作系统的大脑，用于感知、思考和决策。其硬件设计框图如图所示。

系统硬件框图

系统软件设计

软件设计分为四部分，一是树莓派端程序设计，二是底层STM32单片机程序设计，三是手机APP程序设计，四是云端服务器程序设计。

树莓派端程序设计

树莓派端程序主要包括巡检过程中的口罩识别代码、QT编写的UI界面部分、与服务器交互的部分。

巡检过程中的口罩识别由于要兼顾巡检、人脸识别与语音播报，程序运行负载较大，因此为了提升整体程序运行的流畅度尽量用上所有CPU的资源，因此采用多进程来实现这一功能，在不降低精度的情况下大大提升了运行速度，达到了实时性的要求，部分实现代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#定义传递图像队列和传递图像处理结果队列
q_frame = Queue()
q_respond = Queue()
q_car=  Queue()
#采集摄像头进程：
get_camera_frame=Process(target=camera_frame_func, args=("获取摄像头图像", q_frame, mydict,q_car))
#处理图片进程：
proc_frame=Process(target=proc_frame_func, args=("处理图像", q_frame, q_respond, mydict))
#播报语音信息：
read_rst=Process(target=read_rst_func, args=("播报语音信息", q_respond,q_car))
机器人巡检进程：
carrun=Process(target=carrun_fun,args=("小车巡检", q_car))
# 启动任务
get_camera_frame.start()
proc_frame.start()
read_rst.start()
carrun.start()

第一个进程获取摄像头图像，并利用OpenCV中的训练好的人脸Haar特征分类级联器判断画面中是否存在人脸，若存在人脸则将人脸照片传至消息队列中。第二个进程通过消息队列取出人脸照片并上传至百度智能云平台，百度智能云平台可根据云端数据库中的信息匹配出人脸身份信息，并判断人脸是否正确佩戴口罩将判断结果返回，若没有正确佩戴口罩则进程三启动播出提示语音，并停止巡检进程，向底层运动控制系统发出停止指令。正确佩戴口罩后恢复巡检进程。

巡检系统流程图

机器人上层有一五寸可触摸电容屏，界面如图所示。该多媒体屏幕可进行一定的人机交互与多媒体宣传功能。该界面采用QT编写（代码已申请软著），天气查询通过调用API接口实现，词条查询通过爬取百度百科实现。当按下开启消毒模式时，树莓派通过串口向底层单片机发送开启指令。

QT界面

控制模块总按钮位于上位机软件主界面的左下方的“手动控制”，点击后即可进入手动控制界面

手动控制界面

当点击“播放音乐”按钮后，会弹出音乐播放器界面，可点击曲库中的音乐进行播放，左下角可切换“单曲循环”、“顺序播放”、“随机播放”模式。若想要播放曲库中没有的音乐，可对防疫机器人说出像要播放的曲名，机器人会联网下载歌曲后进行播放。

音乐播放器

树莓派通过串口接收到底层单片机采集到的各种数据通过MQTT协议将其传至阿里云服务器，也可接收服务器下发的指令来远程控制机器人。部分代码如下：

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
def Alink(params):
  AlinkJson = {}
  AlinkJson["id"] = random.randint(0,999999)
  AlinkJson["version"] = "1.0"
  AlinkJson["params"] = params
AlinkJson["method"]="thing.event.property.post"
return json.dumps(AlinkJson)
# 消息回调（云端下发消息的回调函数）
def on_message(client, userdata, msg):
    print(msg.payload)
    if(SET==msg.topic):
        Msg = json.loads(msg.payload)
        switch = Msg['params']['PowerLed']
        rpi.powerLed(switch)
        print(msg.payload)  # 开关值

#连接回调（与阿里云建立链接后的回调函数）
def on_connect(client, userdata, flags, rc):
    pass
# 构建与云端模型一致的消息结构
updateMsn = {
'natural_gas':natural_gas,
'fired_gas':fired_gas,
'Harmful_gas':harmful_gas
}
JsonUpdataMsn = aliLink.Alink(updateMsn)
mqtt.push(POST,JsonUpdataMsn) # 定时向阿里云IOT推送我们构建好的Alink协议数据

底层单片机程序设计

底层STM32单片机程序分为主初始化程序、主程序、通讯中断子程序、功能模块子程序、PID电机控制子程序等。通讯中断子程序主要利用串口与树莓派进行通信，功能模块子程序为消毒喷洒装置控制函数、测温函数、LCD屏幕显示函数等。电机子程序内容为根据编码电机上的编码器返回的电机速度利用PID算法实时通过输出PWM波来控制电机的速度，从而保证机器人行进的稳定。

手机APP程序设计

手机APP程序采用Android studio软件开发和编译，分为主控界面、数据显示界面、模式选择界面等。进入控制界面后，可在此界面控制机器人的前进、后退、左转、右转、旋转以及摄像头云台的俯仰与旋转。还可控制防疫机器人进入巡检模式、消毒模式、以及选择人员进行精确测温等。同时可在此界面查看机器人传回的实时画面。数据显示界面可查看机器人检测到的各类传感器数据的数值、拍摄到的未正确佩戴口罩人脸的照片、体温检测异常的人的身份等。

APP端对机器人的控制可选择不需要联网、时延低、控制距离有限的蓝牙控制，也可选择需要联网、时延相对较高、控制距离理论上无限的远程控制。当选择蓝牙控制模式时，手机蓝牙与防疫机器人身上的蓝牙相连来下发控制指令。当选择远程控制模式时，安卓APP通过MQTT协议与服务器建立连接，通过安卓APP将控制指令先发送给云端服务器，然后服务器再将数据发送给树莓派，从而达到对机器人远程控制的目的。

 APP控制系统流程图

实现效果

]]> https://leo-wangbo.top/p/%E5%B7%A1%E6%A3%80%E9%98%B2%E7%96%AB%E6%9C%BA%E5%99%A8%E4%BA%BA/ 2022-04-18T04:24:00.000Z 去年大半年的时间借着参加各种工科竞赛的功夫做了一辆巡检机器人，通过搭建这款巡检机器人，边学边做有了一定的Python和C语言的编程基础，可以绘制一些初级的PCB电路板，从前端到后端、Linux系统的使用、服务器的搭建都有了一定的了解。接下来就主要介绍这辆巡检机器人的主要功能]]> 巡检防疫机器人 2023-11-28T06:14:33.000Z Abo 团队介绍

首先贴出卧龙凤雏团队与电磁炮的合影

最中间是总策划、酷爱军事、从小就造飞机的“曹工”，主要负责坦克车体外观及机械结构及PCB电路板的设计；左边的是我，负责机器人顶层控制逻辑中的视觉识别与追踪；右边是负责车体底层控制的“陈工”。

制作过程

外形设计

底盘设计

机器人底盘长788.45mm，宽308.00mm，高153.51mm，悬挂采用的是克里斯蒂悬挂系统，即一种拥有大直径负重轮，使用螺旋弹簧的独立式悬挂装置。这种悬挂均是由前后两个互相连接的圆柱形螺旋弹簧构成。位于前方的为可调式水平螺旋弹簧，后方的则是垂直螺旋弹簧，这种设计有更长的避震行程，可强化越野性能。

机器人底盘侧视图

“曹工”组装车体

组装完成

磁加速系统设计

磁加速系统实际模型

炮台底座也使用木制材料，中间开有41mm*174mm的矩形孔为炮位提供移动的空间，整个炮塔通过螺栓固定在履带底盘上部的旋转滑台上。磁加速系统的加速段整体用木板封装起来以此来达到绝缘的效果；为了防止热量堆积在炮管支架的木板上每一级加速线圈周围都开有方形空用来散热；

磁加速系统炮管

电磁加速系统为磁阻式电磁加速系统，采用五级线圈加速，光电门进行击发，电解电容储能，发射电压为400V。第一级电容容量为2000μF，第二级电容容量为1880μF，第三级电容容量为1880μF，第四级电容容量为1440μF，第五级电容容量为1440μF，这样的电容容量设计是为了减少炮弹在加速的过程中所受到的反拉

磁加速系统PCB
电容储能系统实物

该磁加速系统配备有自动装弹机，装弹机安装在炮管的尾部并随炮管运动可以实现任意角度装填，装填机构通过丝杆推动炮弹并将其送入炮膛，丝杆由四线两相步进电机提供动力，在装弹机滑台的两侧安装有两个限位开关来限制推弹杆的移动空间。

自动装弹机示意图

经过建立数学模型进行计算并通过测试，该磁加速系统可以将我们所设计的炮弹加速至45m/s左右。

炮弹实物
对炮弹的空气动力模拟计算图示

电路设计

遥控器原理图
遥控器PCB
底层控制板原理图
底层控制板PCB

上路测试

开着出去，抬着回来

软件部分

由于后来想要拿此机器人参加一些比赛，所以要为电磁炮想出应用一些民用价值。考虑到现在楼层建筑越来越高，若发生火灾消防部队扑救的难度很大，如果电磁炮可以对火源发出灭火弹则可迅速阻止火势扩大，因此就以“基于磁加速系统的高层建筑灭火机器人”为背景继续完善机器人。

初步方案是用树莓派进行火焰识别，并用舵机云台进行火焰追踪，将角度数据回传给底层STM32控制端后进行炮塔对火焰的追踪，最后再根据利用炮口的激光测距仪测出的炮口与火焰的距离进行炮弹射速的调整。

实现原理图

初步实现效果演示

优化改进

由于树莓派识别火焰的速率实在不敢恭维，于是家中有矿的”曹工“斥一顿早饭钱买了一块英伟达Jetson Nano边缘计算模块，Jetson nano与树莓派相比，NVIDIA Jetson Nano包含性能更高，功能更强大的GPU——NVIDIA Jetson Nano中具有128个CUDA核心的NVIDIA Maxwell GPU。NVIDIA Jetson Nano中更强大的GPU可以为图形处理，甚至人工智能（AI）和机器学习（ML）提供更强大的功能。

Jetson Nano

火焰识别算法采用的是NanoDet目标检测模型，NanoDet 是一个速度超快和轻量级的移动端目标检测模型，非常适合嵌入式部署。将在PC端训练好的模型移植到Jetson Nano边缘计算平台上即可进行火焰识别。识别到火焰后，根据火焰在视角中的位置，摄像头云台利用PID算法进行追踪。Jetson Nano通过PCA9655 舵机驱动板驱动摄像头云台舵机。

PID算法代码如下

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
# *****************************************************************#
#                      增量式PID系统                              #
# *****************************************************************#
class IncrementalPID:
    def __init__(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D

        self.PIDOutput = 0.0  # PID控制器输出
        self.SystemOutput = 0.0  # 系统输出值
        self.LastSystemOutput = 0.0  # 上次系统输出值

        self.Error = 0.0  # 输出值与输入值的偏差
        self.LastError = 0.0
        self.LastLastError = 0.0

    # 设置PID控制器参数
    def SetStepSignal(self, StepSignal):
        self.Error = StepSignal - self.SystemOutput
        IncrementValue = self.Kp * (self.Error - self.LastError) + \
                         self.Ki * self.Error + \
                         self.Kd * (self.Error - 2 * self.LastError + self.LastLastError)

        self.PIDOutput += IncrementValue
        self.LastLastError = self.LastError
        self.LastError = self.Error

    # 设置一阶惯性环节系统  其中InertiaTime为惯性时间常数
    def SetInertiaTime(self, InertiaTime, SampleTime):
        self.SystemOutput = (InertiaTime * self.LastSystemOutput + \
                             SampleTime * self.PIDOutput) / (SampleTime + InertiaTime)

        self.LastSystemOutput = self.SystemOutput

火焰追踪主函数代码如下

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
if __name__ == '__main__':
    pwm = PCA9685(0x40, debug=True)
    pwm.setPWMFreq(50)
    pwm.setServoPulse(0, 1500)
    pwm.setServoPulse(1, 1500)
    torch.backends.cudnn.enabled = True
    torch.backends.cudnn.benchmark = True
    load_config(cfg, config_path)
    logger = Logger(-1, use_tensorboard=False)
    predictor = Predictor(cfg, model_path, logger, device='cuda:0')
    logger.log('Press "Esc" to exit.')
    camera = USBCamera(width=320, height=240, capture_fps=30)
    camera.running = True
    while True:
        frame = camera.value
        # ret_val, frame = cap.read()
        # if ret_val == False:
        # continue #skip if capture fail
        meta, res = predictor.inference(frame)
        predictor.visualize(res, meta, cfg.class_names, 0.35)
        print('x,y', x, y)
        if (x != 0 and y != 0):
            # 输入X轴方向参数PID控制输入
            xservo_pid.SystemOutput = x
            xservo_pid.SetStepSignal(160)
            xservo_pid.SetInertiaTime(0.01, 0.1)
            target_valuex = int(1500 + xservo_pid.SystemOutput)
            # 输入Y轴方向参数PID控制输入
            yservo_pid.SystemOutput = y
            yservo_pid.SetStepSignal(120)
            yservo_pid.SetInertiaTime(0.01, 0.1)
            print('yservo_pid.SystemOutput', yservo_pid.SystemOutput)
            target_valuey = int(1500 - yservo_pid.SystemOutput)
            if target_valuey < 820:
                target_valuey = 820
            if target_valuey > 2500:
                target_valuey = 2500
            if target_valuey > 2500:
                target_valuex = 2500
            if target_valuex < 10:
                target_valuex = 10
            pwm.setServoPulse(0, target_valuex)
            pwm.setServoPulse(1, target_valuey)
            print('valuex,valuey', target_valuex, target_valuey)

火焰追踪效果如下

Jetson Nano火焰追踪效果

]]> https://leo-wangbo.top/p/%E5%8D%A7%E9%BE%99%E5%87%A4%E9%9B%8F%E7%9A%84%E7%94%B5%E7%A3%81%E7%82%AE/ 2022-03-17T07:41:00.000Z 团队介绍

首先贴出卧龙凤雏团队与电磁炮的合影

说实话挺不想回顾这场电赛的，因为这次比赛的结果与付出严重不成正比，但生活的真相就是这样，努力有时不一定会有回报，罗曼罗兰曾经说过：“世上只有一种英雄主义，就是认清生活的真相之后依然热爱生活”。

最终作品，一个送药小车

题目介绍

其他组的题目都是关于信号采集，AC/DC的这方面没怎么接触过，只有F组是关于送药小车的，正好手头有一个车架，于是就选了F组的题目。题目要求是先搭建下图带有几个路口的赛道，一共有八个药房，小车从起点出发前要先识别一个1到8的数字，然后根据识别到的结果小车需要沿着红线找到对应的药房。

题目要求

实现过程

电赛每个小组有三个人，小组另外两个人，一个人只会写文档，一个人资历尚浅（现已是大牛），所以基本除了写文档的所有工作都落我头上了😭。首先要解决的就是识别数字，最好的方案就是拿K210，因为K210训练部署视觉识别模型迅速，并且有专门的NPU（(神经网络处理单元）加速单元，识别也较为迅速。但是由于学校对于这类比赛很是轻视，没有任何资金补助（连电赛要用的纸箱子都不提供），赛前并未准备这类视觉识别模块。手头只有一个树莓派，网上利用树莓派识别数字的教程还挺多的的，多用python+opencv+tensorflow实现，我也尝试了一下发现识别的帧率只有十帧不到，这根本满足不了比赛的需求。此时已经到第二天上午了，电赛一共只有三天四夜的准备时间，万愁莫展之际我想到了一个下策，调用数字识别API，这可能有点耍阴招，但我看规则也没禁止这一行为（可能电赛加入视觉识别还没几年）。于是最终选择了调用百度api实现了数字。

接下来就是识别红线了，幸好学过一点点OpenCV，先对图像进行预处理，包括灰度化、平滑滤波、二值化，然后就能轻松框出红线了。效果如下

识别红线

接下来要实现让送药小车沿着红线前进，根据识别红线框出的框在视野中的坐标对车轮转速进行PID控制，但还是由于客观设备的限制，手头只有普通的直流电机，没有编码器与编码电机，这样就无法对转速进行闭环控制，所以最后沿着红线行驶时，超调量较大，导致有时就会沿着红线左右摇摆。效果如下：

时间能力有限，到最后一天只能调成这样了，这个效果拿个省二等奖应该问题不大。但是到了最后开箱正式向评委展示的时候，发现电池没电了。。。。(调试时间太紧张，封箱前忘了给锂电池充电了）。最后小车动都没动起来，果然最终还是小细节决定成败啊😭。结果已经无法改变，总的来说这次电赛我还是学到很多的，三天四夜就没睡几个小时（提前体验996生活？😇），一直在调试软件硬件，查资料编代码的能力还是得到了一些锻炼的。

]]> https://leo-wangbo.top/p/%E5%9B%9E%E9%A1%BE21%E5%B9%B4%E7%94%B5%E8%B5%9B/ 2021-11-28T02:51:00.000Z 说实话挺不想回顾这场电赛的，因为这次比赛的结果与付出严重不成正比，但生活的真相就是这样，努力有时不一定会有回报，罗曼罗兰曾经说过：“世上只有一种英雄主义，就是认清生活的真相之后依然热爱生活”。