Eric Blog

扫地机器人日志分析工具

Sun, 21 Dec 2025 11:56:24 GMT

本文采用AI设计

针对扫地机器人日志分析训练大模型，目前前沿的思路已不局限于单纯训练一个模型，而是构建一个融合了领域微调、检索增强（RAG）和多智能体协作的智能分析系统。其核心目标是让模型不仅能识别已知异常，还能结合日志上下文进行推理，定位未知或复杂故障的根本原因

下面的表格对比了两种主流的技术路径，你可以根据自身的数据和技术条件进行选择：

路径	核心思路	优势	适用场景
路径一：微调专用模型	在通用大模型（LLM）基础上，使用你的日志-诊断标注数据进行领域适应训练（微调）。	精度高、响应快；私有化部署安全性好；对已知问题识别准。	异常模式相对固定、有充足历史数据且对结果一致性要求高的场景
路径二：构建RAG分析系统	不（仅）改变模型，而是建立日志知识库。模型收到查询时，先检索相似历史日志和解决方案，再生成答案。	灵活、可解释性强；易于融入新知识；对未知问题有更好的泛化能力	故障场景多样、复杂，或缺乏大规模标注数据，需要模型进行推理和归因的场景。

当前更前沿的实践是将两者结合，构成一个 “微调模型 + RAG + 工作流” 的协同系统。模型负责理解与生成，RAG提供精准的领域知识，而工作流（多智能体）则调度不同模块完成解析、检索、诊断等专业任务

🗺️ 项目实施流程#

无论选择哪种路径，一个完整的项目通常遵循以下六个阶段：

数据采集与清洗：首先，系统性地收集全链路日志，这包括扫地机器人的传感器、控制、导航、电机驱动、错误上报等各模块日志。清洗阶段需统一时间戳、去除无关噪声、对敏感信息脱敏，并为后续分析做结构化准备。

日志解析与模板化：这是关键预处理步骤。目标是将非结构化的日志行（如 “ERR 2025-12-21 10:00:45 MotorA: current overload 350mA”），通过大模型或传统算法，抽象成日志模板（如 “ERR <> Motor<>: current overload <*>mA”）并提取出关键参数（时间、电机编号、电流值）。研究表明，利用大模型（如GPT）进行语义增强的解析，能有效提升后续异常检测的准确性。

标注与知识库构建：为解析后的日志事件打标签，例如“异常类型：电机过流”、“可能原因：刷毛缠绕、驱动板故障”、“解决方案：清理主刷、检查驱动电路”。这些形成高质量的训练数据或RAG系统的核心知识库。

模型训练/系统构建：

若选择微调路径，可选用参数量适中、性能优异的模型（如 DistilRoBERTa，研究显示其在日志分析任务上F1-Score可达0.998），使用标注数据进行监督微调。

若选择RAG路径，则需搭建向量数据库，将日志模板和诊断知识转化为向量存储。系统收到新日志时，会先检索最相关的历史案例和解决方案，再由大模型生成诊断报告。

评估与迭代：在测试集上评估模型或系统的准确率、召回率。通过数据飞轮持续收集线上分析的反馈，将判断不准或新的案例回流，用于优化模型和丰富知识库。

部署与应用：将最终系统部署为内部工具。提供Web界面或API接口，支持工程师上传日志文件或实时推送日志流，获取自动分析报告。

🧠 模型与数据准备建议#

模型选型：对于扫地机器人这类嵌入式设备日志，计算资源可能有限。可以优先考虑高效的开源模型进行微调，如 DistilRoBERTa、CodeBERT（如果日志含代码信息）。对于生成和分析任务，可选用 Llama 3、Qwen 等系列模型构建RAG系统。云端方案也可直接调用如 DeepSeek、文心等API。

数据准备关键：质量远大于数量。你需要准备的不仅是原始日志，更重要的是 “问题日志-根因分析-解决方案” 的三元组标注数据。初期可以从已知的、高频的故障（如“滚刷卡死”、“激光雷达异常”、“回充失败”）开始构建高质量的小数据集。

💡 最终应用形态展望#

最终产品可能是一个智能运维平台，其核心功能包括：

智能问答助手：工程师用自然语言提问，如“昨天下午有哪些机器反复报导航超时？可能是什么原因？”，系统能直接给出分析结论和排障建议。

自动化诊断报告：上传一批日志后，自动生成包含异常摘要、时间线、关联事件图（展示故障如何在不同模块间传播）和根因排序的诊断报告。

预测与洞察：通过对历史日志的时序分析，系统可能预测潜在故障（如“根据电机电流缓慢上升趋势，预计48小时后可能过载”）。

⚠️ 重要注意事项#

在开始项目前，有几个关键点需要考虑：

数据安全与隐私：确保训练和部署过程符合公司数据安全规定，对日志中的用户隐私信息（如地图数据）进行脱敏处理。

大模型的非确定性：即使是同一模型，对同一条日志的分析结果也可能有细微波动。这需要通过设定合适的温度参数、提供清晰的指令以及结合RAG的确定性检索来缓解。

领域知识的融入：模型的诊断能力深度依赖于你注入的领域知识。与扫地机器人的硬件工程师、嵌入式软件专家紧密合作，确保知识库的准确性至关重要。

ros2中map_server生命周期立即启动

Sun, 24 Aug 2025 04:02:12 GMT

ros2中引入了一些生命周期的东西，本意是好的，但是在写一些demo的时候总是要ros2 lifecycle set /map_server configure 以及 ros2 lifecycle set /map_server activate去配置和激活地图，虽然可以写脚本在启动应用程序后再启动配置和激活，但是一般很少这样做，所以在查阅和尝试一些方式后，决定在launch中提那家configure和activate；

首先，地图的服务节点在launch中创建：

1
map_file = DeclareLaunchArgument(
2
    'map_file',
3
    default_value=os.path.join(
4
        # get_package_share_directory('robot_navigation'),
5
        os.path.expanduser('这里修改为map的目录'),
6
        'maps',
7
        'map.yaml'
8
    ),
9
    description='Path to the map file (YAML)'
10
)
11

12
map_server_node = LifecycleNode(
13
    package='nav2_map_server',
14
    executable='map_server',
15
    name='map_server',
16
    namespace='',
17
    output='screen',
18
    parameters=[{
19
            'yaml_filename': LaunchConfiguration('map_file'),
20
            'frame_id': 'map',
21
            'topic_name': LaunchConfiguration('map_topic'),
22
            'use_sim_time': False
23
        }]
24
)

然后重点就是，如果希望在启动map_server之后立刻启动configure和activate，可以添加下述代码：

1
configure_map_server = EmitEvent(
2
    event=ChangeState(
3
        lifecycle_node_matcher=lambda node: node == map_server_node,
4
        transition_id=Transition.TRANSITION_CONFIGURE
5
    )
6
)
7

8
activate_map_server = EmitEvent(
9
    event=ChangeState(
10
        lifecycle_node_matcher=lambda node: node == map_server_node,
11
        transition_id=Transition.TRANSITION_ACTIVATE
12
    )
13
)

需要引用的头文件有：

1
from launch.actions import EmitEvent
2
from launch_ros.events.lifecycle import ChangeState

这样就可以在启动server后立即加载图像了

当然别忘了在return的LaunchDescription添加

1
return LaunchDescription([
2
    map_server_node,
3
    configure_map_server,
4
    activate_map_server
5
])

spdlog使用方式 C++

Tue, 12 Aug 2025 11:15:05 GMT

1. 简介#

spdlog 是一个非常高性能、header-only（仅头文件）的 C++ 日志库。它具有以下特点：

极快：基于 fmt 格式化库，性能非常高。
多种日志输出方式：控制台、文件、异步日志等。
易用：API 简洁，几乎零配置即可使用。
线程安全：默认多线程安全版本，也支持单线程版本。

GitHub 项目地址：https://github.com/gabime/spdlog

2. 安装 spdlog#

2.1 方式一：直接使用头文件#

spdlog 是 header-only 库，直接将 include/spdlog 文件夹拷贝到项目中即可。

2.2 方式二：CMake 引入（推荐）#

1
# 使用 FetchContent 引入
2
include(FetchContent)
3
FetchContent_Declare(
4
    spdlog
5
    GIT_REPOSITORY https://github.com/gabime/spdlog.git
6
    GIT_TAG v1.12.0
7
)
8
FetchContent_MakeAvailable(spdlog)
9

10
# 链接到项目
11
target_link_libraries(my_app PRIVATE spdlog::spdlog)

或者通过系统包管理器安装： sudo apt install libspdlog-dev

3. 基本用法#

1
#include <spdlog/spdlog.h>
2

3
int main() {
4
    // 输出到控制台
5
    spdlog::info("Hello, {}!", "World");
6
    spdlog::warn("Warning message");
7
    spdlog::error("Error message");
8

9
    // 设置日志级别（默认 info）
10
    spdlog::set_level(spdlog::level::debug);
11
    spdlog::debug("This is a debug message");
12

13
    return 0;
14
}

输出示例：

1
[2025-08-12 10:30:45.123] [info] Hello, World!
2
[2025-08-12 10:30:45.124] [warning] Warning message
3
[2025-08-12 10:30:45.124] [error] Error message

4. 输出到文件#

1
#include <spdlog/spdlog.h>
2
#include <spdlog/sinks/basic_file_sink.h>
3

4
int main() {
5
    auto file_logger = spdlog::basic_logger_mt("file_logger", "logs/my_log.txt");
6
    file_logger->info("Log message to file");
7
    file_logger->warn("Warning to file");
8
    file_logger->error("Error to file");
9
}

默认会覆盖日志文件，可以在构造时传入 true 表示追加模式

1
auto file_logger = spdlog::basic_logger_mt("file_logger", "logs/my_log.txt", true);

5.同时输出到控制台和文件#

1
#include <spdlog/spdlog.h>
2
#include <spdlog/sinks/basic_file_sink.h>
3
#include <spdlog/sinks/stdout_color_sinks.h>
4

5
int main() {
6
    auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
7
    auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>("logs/multisink.txt", true);
8

9
    console_sink->set_pattern("[%Y-%m-%d %H:%M:%S] [%^%l%$] %v");
10
    file_sink->set_pattern("[%Y-%m-%d %H:%M:%S] [%l] [%s:%#] %v");
11

12
    spdlog::logger logger("multi_logger", {console_sink, file_sink});
13
    logger.set_level(spdlog::level::debug);
14

15
    logger.info("Log to console and file");
16
    logger.warn("Warning message");
17
}

6. 设置日志格式#

spdlog 提供了非常灵活的格式控制：

1
spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%n] [%^%l%$] [%s:%#] %v");

常用格式占位符：

占位符	含义
%v	日志内容
%n	日志器名称
%l	日志等级（info, warn, error）
%t	线程 ID
%s	源文件名
%#	行号
%!	函数名
%Y-%m-%d %H:%M:%S	日期时间
%e	毫秒

7. 异步日志（高性能）#

1
#include <spdlog/async.h>
2
#include <spdlog/sinks/basic_file_sink.h>
3

4
int main() {
5
    spdlog::init_thread_pool(8192, 1);
6
    auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>("logs/async_log.txt", true);
7
    auto async_logger = std::make_shared<spdlog::async_logger>(
8
        "async_logger", file_sink, spdlog::thread_pool(),
9
        spdlog::async_overflow_policy::block);
10

11
    spdlog::register_logger(async_logger);
12
    async_logger->info("This is async log message");
13
}

8. 封装到头文件（推荐）#

log.h

1
#pragma once
2
#include <spdlog/spdlog.h>
3
#include <spdlog/sinks/basic_file_sink.h>
4
#include <spdlog/sinks/stdout_color_sinks.h>
5

6
extern std::shared_ptr<spdlog::logger> logger;
7

8
void init_logger(const std::string &log_path = "logs/app.log",
9
                 spdlog::level::level_enum level = spdlog::level::info);
10

11
#define LOG_INFO(...)   logger->info(__VA_ARGS__)
12
#define LOG_WARN(...)   logger->warn(__VA_ARGS__)
13
#define LOG_ERROR(...)  logger->error(__VA_ARGS__)
14
#define LOG_DEBUG(...)  logger->debug(__VA_ARGS__)

log.cpp

1
#include "log.h"
2

3
std::shared_ptr<spdlog::logger> logger;
4

5
void init_logger(const std::string &log_path, spdlog::level::level_enum level) {
6
    auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>(log_path, true);
7
    auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
8

9
    file_sink->set_pattern("[%Y-%m-%d %H:%M:%S] [%s:%#] %v");
10
    console_sink->set_pattern("[%Y-%m-%d %H:%M:%S] [%^%l%$] %v");
11

12
    logger = std::make_shared<spdlog::logger>("multi_sink", spdlog::sinks_init_list{file_sink, console_sink});
13
    logger->set_level(level);
14
}

使用：

1
#include "log.h"
2

3
int main() {
4
    init_logger("logs/my_app.log", spdlog::level::debug);
5

6
    LOG_INFO("App started");
7
    LOG_WARN("Low disk space");
8
    LOG_ERROR("Something went wrong");
9
}

点云的机器坐标转世界坐标

Tue, 12 Aug 2025 08:08:38 GMT

点云的机器坐标转世界坐标：原理与实现（C++/Python）#

1. 问题背景#

在机器人感知系统中，激光雷达采集的点云数据通常以传感器自身坐标系（机器坐标系）为参考。为了将这些数据用于建图、定位等任务，我们需要将其转换到全局的世界坐标系中。本文将详细介绍这一转换的数学原理，并提供C++和Python两种实现方式。

2. 数学原理#

2.1 坐标系定义#

机器坐标系：点云原始坐标(x₁,y₁,z₁)，原点在雷达中心
世界坐标系：全局固定坐标系

2.2 转换公式#

转换过程分为旋转和平移两个步骤：

旋转：应用由姿态角(yaw,pitch,roll)构成的旋转矩阵R
平移：加上机器在世界坐标系中的位置(x,y)

数学表达式：P_world = R * P_machine + t

2.3 旋转矩阵计算#

完整旋转矩阵（Z-Y-X顺序）： R = R_z(yaw) R_y(pitch) R_x(roll)

展开后： R = [ [cosθcosφ, cosθsinφsinψ-sinθcosψ, cosθsinφcosψ+sinθsinψ], [sinθcosφ, sinθsinφsinψ+cosθcosψ, sinθsinφcosψ-cosθsinψ], [-sinφ, cosφsinψ, cosφcosψ ] ]

3. C++实现#

1
#include <Eigen/Dense>
2
#include <vector>
3

4
using namespace Eigen;
5

6
struct Pose {
7
    double x, y, z;
8
    double yaw, pitch, roll;
9
};
10

11
std::vector<Vector3d> transformPointCloud(
12
    const std::vector<Vector3d>& points,
13
    const Pose& pose)
14
{
15
    // 计算旋转矩阵
16
    double cy = cos(pose.yaw), sy = sin(pose.yaw);
17
    double cp = cos(pose.pitch), sp = sin(pose.pitch);
18
    double cr = cos(pose.roll), sr = sin(pose.roll);
19

20
    Matrix3d R;
21
    R << cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr,
22
         sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr,
23
         -sp,    cp*sr,           cp*cr;
24

25
    // 应用变换
26
    std::vector<Vector3d> transformed;
27
    for (const auto& p : points) {
28
        Vector3d p_world = R * p;
29
        p_world += Vector3d(pose.x, pose.y, pose.z);
30
        transformed.push_back(p_world);
31
    }
32

33
    return transformed;
34
}

4. python实现#

1
import numpy as np
2

3
def transform_point_cloud(points, pose):
4
    """
5
    :param points: Nx3 numpy数组
6
    :param pose: 包含x,y,z,yaw,pitch,roll的字典或对象
7
    :return: 转换后的Nx3点云
8
    """
9
    # 解包位姿
10
    x, y, z = pose['x'], pose['y'], pose['z']
11
    yaw, pitch, roll = pose['yaw'], pose['pitch'], pose['roll']
12

13
    # 计算旋转矩阵
14
    cy, sy = np.cos(yaw), np.sin(yaw)
15
    cp, sp = np.cos(pitch), np.sin(pitch)
16
    cr, sr = np.cos(roll), np.sin(roll)
17

18
    R = np.array([
19
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
20
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
21
        [-sp,    cp*sr,           cp*cr]
22
    ])
23

24
    # 应用变换
25
    points_rotated = np.dot(points, R.T)
26
    points_transformed = points_rotated + np.array([x, y, z])
27

28
    return points_transformed

C++线程池 ThreadPool 手动实现

Sat, 07 Jun 2025 01:44:30 GMT

在现代软件开发中，多线程并发是提升程序性能的关键技术之一。然而，直接管理线程（std::thread）会面临诸多挑战，例如：

线程创建/销毁开销大（频繁操作降低性能）。
资源竞争（无限制的线程数可能导致系统崩溃）。
任务调度复杂（需手动管理任务队列和线程同步）。 线程池（ThreadPool） 应运而生，它通过 预先创建线程 + 任务队列的方式，实现高效的任务调度和资源管理。本文将深入探讨线程池的设计原理、实现方法，并通过一个 简洁高效的 C++ ThreadPool Demo 展示其实际应用。

1. 线程池的核心价值#

1.1 为什么需要线程池#

场景	直接 std::thread	线程池
高并发任务	频繁创建/销毁线程，性能差	复用线程，降低开销
短任务密集	线程切换成本高	任务队列优化调度
资源受限环境	可能耗尽系统线程数	可控的线程数量

1.2 适用场景#

Web 服务器：处理大量短时 HTTP 请求。 文件上传/下载：避免网络 I/O 阻塞主线程。 游戏逻辑：异步加载资源或计算 AI 行为。 数据分析：并行处理分块数据。

2. 线程池设计原理#

一个完整的线程池通常包含以下组件：

任务队列（Task Queue） ：存储待执行的任务（std::function<void()>）。
工作线程（Worker Threads） ：从队列中取出任务并执行。
同步机制 ：使用互斥锁（std::mutex）和条件变量（std::condition_variable）协调线程。
停止标志 ：安全关闭线程池。

3. C++线程池实现#

以下是一个 简洁、高性能的 ThreadPool 实现，支持动态任务提交和安全关闭

3.1 核心代码#

1
#include <vector>
2
#include <queue>
3
#include <thread>
4
#include <mutex>
5
#include <condition_variable>
6
#include <functional>
7
#include <future>
8

9
class ThreadPool {
10
public:
11
    explicit ThreadPool(size_t numThreads) : stop(false) {
12
        for (size_t i = 0; i < numThreads; ++i) {
13
            workers.emplace_back([this] {
14
                while (true) {
15
                    std::function<void()> task;
16
                    {
17
                        std::unique_lock<std::mutex> lock(queueMutex);
18
                        condition.wait(lock, [this] {
19
                            return stop || !tasks.empty();
20
                        });
21
                        if (stop && tasks.empty()) return;
22
                        task = std::move(tasks.front());
23
                        tasks.pop();
24
                    }
25
                    task(); // 执行任务
26
                }
27
            });
28
        }
29
    }
30

31
    ~ThreadPool() {
32
        {
33
            std::unique_lock<std::mutex> lock(queueMutex);
34
            stop = true;
35
        }
36
        condition.notify_all();
37
        for (auto &worker : workers) {
38
            worker.join();
39
        }
40
    }
41

42
    template <typename F, typename... Args>
43
    auto enqueue(F &&f, Args &&...args) -> std::future<decltype(f(args...))> {
44
        using ReturnType = decltype(f(args...));
45
        auto task = std::make_shared<std::packaged_task<ReturnType()>>(
46
            std::bind(std::forward<F>(f), std::forward<Args>(args)...));
47
        std::future<ReturnType> res = task->get_future();
48
        {
49
            std::unique_lock<std::mutex> lock(queueMutex);
50
            if (stop) throw std::runtime_error("Enqueue on stopped ThreadPool");
51
            tasks.emplace([task]() { (*task)(); });
52
        }
53
        condition.notify_one();
54
        return res;
55
    }
56

57
private:
58
    std::vector<std::thread> workers;
59
    std::queue<std::function<void()>> tasks;
60
    std::mutex queueMutex;
61
    std::condition_variable condition;
62
    bool stop;
63
};

3.2 关键特性#

1.泛型任务支持：通过 std::function 和模板实现任意任务类型。 2.返回值获取：使用 std::future 异步获取结果。 3.线程安全：互斥锁保护任务队列。 4.优雅关闭：析构函数自动通知所有线程退出。

4.使用示例#

4.1基础用法#

1
#include <iostream>
2
#include <chrono>
3

4
int main() {
5
    ThreadPool pool(4); // 4 个工作线程
6

7
    // 提交任务并获取 future
8
    auto future = pool.enqueue([](int a, int b) {
9
        std::this_thread::sleep_for(std::chrono::seconds(1));
10
        return a + b;
11
    }, 2, 3);
12

13
    // 阻塞等待结果
14
    std::cout << "Result: " << future.get() << std::endl; // 输出 5
15
    return 0;
16
}

4.2 高性能场景#

1
// 并行处理 1000 个任务
2
std::vector<std::future<int>> results;
3
for (int i = 0; i < 1000; ++i) {
4
    results.emplace_back(pool.enqueue([i] {
5
        return i * i;
6
    }));
7
}
8

9
// 获取所有结果
10
for (auto &result : results) {
11
    std::cout << result.get() << " ";
12
}

5. 性能优化与陷阱#

5.1 最佳实践#

线程数设置：通常为 CPU 核心数 + 1（I/O 密集型可适当增加）。 避免长任务：单个任务不应长时间占用线程（考虑拆分）。 监控队列堆积：可通过 tasks.size() 检测是否过载。

5.2 常见陷阱#

问题	解决方案
死锁	确保任务中不嵌套调用线程池
资源泄漏	使用 std::shared_ptr 管理任务对象
虚假唤醒	条件变量必须搭配谓词检查

6. 扩展功能（可选）#

6.1 动态调整线程数#

1
void resize(size_t newSize) {
2
    if (newSize < workers.size()) {
3
        // 减少线程（实际实现需复杂逻辑）
4
    } else {
5
        for (size_t i = workers.size(); i < newSize; ++i) {
6
            workers.emplace_back([this] { /* ... */ });
7
        }
8
    }
9
}

6.2 优先级队列#

1
// 使用 std::priority_queue 实现高优先级任务优先执行
2
struct Task {
3
    std::function<void()> func;
4
    int priority;
5
    bool operator<(const Task &other) const { return priority < other.priority; }
6
};
7

8
std::priority_queue<Task> tasks;

结语#

线程池是并发编程的基石之一，合理使用可显著提升程序性能。本文的 ThreadPool 实现仅需 50 行代码，但涵盖了核心功能。在实际项目中，可根据需求进一步扩展（如任务超时、负载均衡等）。

下一步建议：#

1.尝试集成到你的项目中（如替换 std::thread）。 2.使用性能分析工具（如 perf）观察线程利用率。 3.阅读更复杂的实现（如 BS::thread_pool）（C++17以上建议使用）

讨论：你在哪些场景下使用过线程池？遇到过哪些问题？欢迎留言分享

Learning 是否会替代 Planning？以RL为思考

Sun, 25 May 2025 08:59:14 GMT

本篇内容来自AI

在具身智能领域，强化学习（RL）与运动规划（Planning）的关系并非简单的“替代”或“缩水”，而是呈现出融合与互补的趋势。以下从技术逻辑、实际应用和发展趋势三个层面展开分析：

一、技术本质：强化学习与运动规划的核心差异#

维度	运动规划（传统Planning）	强化学习（RL）
决策逻辑	基于预先建模的环境约束（如地图、运动学方程），通过数学优化求解确定路径/轨迹。	从环境交互中通过试错学习策略，无需显式建模，依赖奖励函数驱动决策。
适应性	对已知环境高效，但面对动态变化或未知场景时需重新规划。	可自主适应未知环境，但初期训练需大量样本，收敛速度依赖算法设计。
计算复杂度	确定性算法（如A*、RRT）计算量可控，适合实时性要求高的场景。	随机性策略搜索（如PPO、SAC）计算量较大，需高性能算力支持训练。

二、强化学习对运动规划的影响：并非替代，而是拓展边界#

1. 在复杂场景中，RL为Planning提供“智能优化”#

传统Planning的痛点：在非结构化环境（如家庭杂物场景、动态障碍物场景）中，显式建模成本高，路径规划可能陷入局部最优（如机器人在复杂家具间反复避障）。
RL的补充作用：通过训练机器人直接学习“如何在未知环境中高效移动”，例如：
- Google DeepMind的「移动机器人导航」研究中，RL策略可自主学习避开动态行人，而传统规划算法需预先设定行人运动模型。
- 机械臂抓取任务中，RL可优化抓取姿态（如角度、力度），而传统轨迹规划仅能完成“从A到B”的路径生成，无法自适应物体表面特性。

2. 在实时性场景中，Planning仍是RL的“底层支撑”#

RL的局限性：纯RL决策可能因环境随机性导致动作震荡（如机器人在障碍物前反复试探），无法满足工业场景毫秒级响应需求。
Planning的不可替代性：
- 热启动（Warm Start）：RL策略可调用Planning生成的初始路径作为“先验知识”，减少训练样本量。例如，特斯拉Optimus机器人在行走时，先用SLAM构建地图，再通过RL优化步态。
- 安全兜底：传统规划中的碰撞检测（如OBB包围盒算法）可作为RL的安全约束，避免机器人在训练中“试错”导致硬件损坏。

三、前沿趋势：RL与Planning的深度融合架构#

1. 分层决策框架：RL负责策略，Planning负责执行#

案例：OpenAI的Dactyl机器人训练中，上层RL学习“抓握策略”（如判断物体形状选择抓取点），下层通过运动规划生成机械臂关节轨迹，确保动作可行性。
优势：结合RL的环境适应性和Planning的运动学约束，平衡“智能性”与“可行性”。

2. 神经符号融合：用RL优化Planning的参数#

技术路径：通过神经网络（如Transformer）学习环境特征，再将特征输入传统规划算法（如RRT*），动态调整规划参数（如扩展步长、采样密度）。
应用：MIT的「神经规划器」可根据视觉输入自动调整路径规划的采样策略，在复杂场景中比传统RRT快30%。

3. 模仿学习+规划：从人类示范中学习“规划逻辑”#

方法：通过模仿学习（IL）让机器人学习人类专家的规划思路（如优先选择最短路径、规避危险区域），再结合传统规划算法生成动作。
案例：斯坦福AI实验室的「自动驾驶规划系统」，用IL学习人类司机的决策偏好，再用轨迹规划算法生成安全路径。

四、结论：RL不会让Planning缩水，而是推动其“智能化升级”#

短期：传统运动规划仍是具身智能的基础，尤其在工业、医疗等对安全性和实时性要求极高的场景中不可替代。
长期：RL将成为规划的“智能引擎”，通过学习优化规划策略、参数和约束，使机器人在未知环境中具备更接近人类的“直觉决策”能力。
建议：作为从业者，可同时掌握传统规划算法（如ROS中的OMPL库）和RL框架（如Stable Baselines3），并关注神经符号系统、分层强化学习等前沿方向，以适应技术融合趋势。

C++ Pimpl模式实践：隐藏实现细节的优雅方式

Sun, 06 Apr 2025 15:40:18 GMT

问题背景——为什么需要隐藏实现细节？#

最近在开发一个C++库时，我遇到了一个典型的接口与实现耦合问题： 需求：

我的类（比如 DeviceController）需要调用某个硬件控制模块（比如 Gpio），但我不希望用户代码在包含 DeviceController.h 时，也被迫包含 Gpio.h。
如果直接包含 Gpio.h，会导致：
- 编译依赖增加：用户代码必须能找到 Gpio.h，否则编译失败。
- 头文件污染：Gpio 的实现细节暴露给用户，可能影响编译速度或导致命名冲突。

问题本质：

C++ 的头文件（.h）通常包含类的声明和实现，但有时候我们只想暴露接口，隐藏实现细节。
如何在不暴露 Gpio 的情况下，仍然让 DeviceController 能使用它？

这时，Pimpl（Pointer to Implementation）模式 就派上用场了！

解决方案——Pimpl模式（Impl实现方式）#

1. Pimpl 模式的核心思想#

Pimpl（Pointer to Implementation）是一种**编译防火墙（Compilation Firewall）**技术，核心思想是：

将类的实现细节移动到一个单独的类（Impl）中。
在主类中仅保留一个指向 Impl 的指针（通常用 std::unique_ptr 管理）。
头文件只暴露接口，不暴露任何实现细节。

这样，修改 Impl 的实现时，用户代码不需要重新编译，提高了代码的封装性和编译效率。

2. 具体实现方式#

（1）头文件（.h）——只声明接口#

1
#pragma once
2
#include <memory>
3

4
class DeviceController {
5
public:
6
    DeviceController();
7
    ~DeviceController();  // 必须声明，因为 std::unique_ptr 需要完整类型
8

9
    void setDevicePower(bool on);  // 公开接口
10

11
private:
12
    class Impl;  // 前向声明，不暴露实现细节
13
    std::unique_ptr<Impl> impl_;  // 指向实现的指针
14
};

关键点：

Impl 只是一个前向声明，用户代码看不到它的定义。
std::unique_ptr<Impl> 用于管理 Impl 的生命周期（避免手动 new/delete）。
必须声明析构函数（因为 std::unique_ptr 的析构需要 Impl 的完整定义，但头文件里没有）。

（2）源文件（.cpp）——实现 Impl#

1
#include "DeviceController.h"
2
#include "Gpio.h"  // 真正的依赖，但用户看不到
3

4
// 实现 Impl
5
class DeviceController::Impl {
6
public:
7
    Impl() : gpio_(Gpio::kPowerPin, Gpio::kOutDirection) {}
8

9
    void setPower(bool on) {
10
        gpio_.setValue(on ? 1 : 0);
11
    }
12

13
private:
14
    Gpio gpio_;  // 真正的实现细节
15
};
16

17
// 主类的实现
18
DeviceController::DeviceController() : impl_(std::make_unique<Impl>()) {}
19

20
DeviceController::~DeviceController() = default;  // 必须在外联定义（因为 Impl 是不完整类型）
21

22
void DeviceController::setDevicePower(bool on) {
23
    impl_->setPower(on);  // 委托给 Impl
24
}

关键点：

Impl 类在 .cpp 文件中定义，完全隐藏了 Gpio 的依赖。
DeviceController 只是 Impl 的代理，所有操作都转发给它。
析构函数必须在外联定义（因为 std::unique_ptr 的析构需要 Impl 的完整定义）。

3. 优点总结#

✅ 减少编译依赖：用户代码不需要包含 Gpio.h，只需包含 DeviceController.h。 ✅ 接口与实现分离：修改 Impl 不影响用户代码。 ✅ 二进制兼容性：即使 Impl 内部变化，只要接口不变，库的二进制兼容性仍能保持。 ✅ 更快的编译速度：头文件更干净，减少不必要的依赖。

4. 适用场景#

库开发：希望隐藏实现细节，避免用户代码依赖内部头文件。
大型项目：减少编译时间，提高模块化程度。
二进制兼容性要求高：避免因实现细节变动导致用户代码重新编译。

总结#

Pimpl 模式（Impl 方式）是 C++ 中隐藏实现细节的经典方法，适用于降低耦合、提高编译速度、增强封装性的场景。

头文件只声明接口，用 std::unique_ptr<Impl> 管理实现。
源文件定义 Impl，真正实现功能。
析构函数必须外联定义（因为 Impl 是不完整类型）。

如果你也遇到类似的问题（比如不想暴露第三方库依赖），Pimpl 模式是一个值得掌握的技巧！ 🚀

深入解析UUID：版本差异与生成方式指南

Sat, 29 Mar 2025 04:31:09 GMT

深入解析UUID：版本差异与生成方式指南#

什么是UUID？#

UUID（通用唯一标识符）是一个128位的全局唯一标识符，通常表示为32位十六进制数，格式为8-4-4-4-12，例如：550e8400-e29b-41d4-a716-446655440000。不同版本的UUID通过特定算法生成，适用于分布式系统、数据库主键等多种场景。

UUID核心结构解析#

标准格式#

示例UUID：550e8400-e29b-41d4-a716-446655440000 结构分解：

时间戳部分（v1/v6） : 前16字符（550e8400-e29b）版本标识位 : 第13字符的左侧（如”4”代表v4）变体标识位 : 第17字符的左侧（通常为8/9/a/b）

版本标识规则#

版本	标识位位置	十六进制值
v1	第13字符左	0x10
v3	第13字符左	0x30
v4	第13字符左	0x40
v5	第13字符左	0x50

各版本UUID详解#

版本	名称	算法/原理	唯一性保证	应用场景
v1	基于时间和MAC地址	时间戳 + MAC地址	强	分布式系统、本地唯一标识
v2	DCE安全UUID	扩展v1 + 用户ID	强	过时，极少使用
v3	基于命名空间的MD5哈希	MD5哈希	确定性	需要重复生成相同UUID的场景
v4	随机UUID	随机数	概率性	临时标识、隐私敏感场景
v5	基于命名空间的SHA-1哈希	SHA-1哈希	确定性	类似v3，但更安全
v6+	新版时间排序UUID	改进的v1	强	数据库主键、时序数据（新标准）
v7	时间戳 + 随机数	时间戳 + 随机数	概率性	高并发场景（草案）
v8	自定义UUID	用户自定义算法	灵活	私有实现（草案）

1.UUID v1 (基于时间和MAC地址)#

生成原理#

时间戳：60位高精度时间（100ns单位，起点为1582-10-15）
时钟序列：14位计数器（防止时间回退冲突）
节点ID：48位MAC地址

结构分解#

优点：#

强唯一性，有序性（时间排序）。

缺点:#

暴露MAC地址，可能泄露隐私。

实现方式#

通过系统API（如Linux的libuuid、Windows的UuidCreate）获取时间和MAC地址。

代码示例（C++ Boost）#

1
#include <boost/uuid/uuid_generators.hpp>
2

3
boost::uuids::uuid uuid;
4
boost::uuids::name_generator_v1 gen(boost::uuids::random_generator()());
5
uuid = gen();  // 需确认库是否支持v1生成

2.UUID v3/v5 命名空间哈希UUID#

核心差异

版本	哈希算法	安全性
v3	MD5	已过时
v5	SHA-1	更安全

生成原理：#

将命名空间（预定义的UUID）和一个名称（字符串或字节）通过哈希算法（v3用MD5，v5用SHA-1）生成固定UUID。

结构分析#

哈希结果的128位直接作为UUID，但需设置版本位（第6字节的0x30或0x50）和变体位（RFC 4122规范）。

优点：#

确定性：相同命名空间和名称生成相同UUID。无隐私泄露风险。

缺点：#

依赖哈希算法的安全性（MD5已被破解，建议用v5）。

实现方式（C++ OpenSSL实现）#

1
#include <openssl/sha.h>
2

3
std::string generate_v5_uuid(const std::string& ns, const std::string& name) {
4
    unsigned char hash[SHA_DIGEST_LENGTH];
5
    SHA_CTX ctx;
6

7
    SHA1_Init(&ctx);
8
    SHA1_Update(&ctx, ns.data(), ns.size());
9
    SHA1_Update(&ctx, name.data(), name.size());
10
    SHA1_Final(hash, &ctx);
11

12
    hash[6] = (hash[6] & 0x0F) | 0x50;  // 设置版本位为v5
13
    hash[8] = (hash[8] & 0x3F) | 0x80;  // RFC 4122变体标识
14

15
    // 转换为标准UUID字符串格式...
16
}

3. UUID v4 (随机UUID)#

生成原理#

122位为随机数，剩余6位用于版本和变体标识。

结构分解#

random（122 bits） | 版本（4 bits） | 变体（2 bits）

优点#

生成简单，无隐私泄露风险。

缺点#

碰撞概率极低但非零（需生成约2^61个UUID才有1%碰撞概率）。

实现方式（C++11标准库）：#

1
#include <random>
2
std::random_device rd;
3
std::mt19937_64 gen(rd());
4
std::uniform_int_distribution<uint64_t> dis;
5
uint64_t part1 = dis(gen), part2 = dis(gen);
6
// 组合part1和part2，并设置版本位和变体位

4. UUID v6/v7/v8 (新版本）#

v6：改进的v1，时间戳高位在前，便于数据库排序。 v7：结合时间戳（毫秒精度）和随机数，适合高并发场景。 v8：允许自定义算法，适合私有实现。实现方式：需依赖支持新标准的库（如uuid7库）

UUID版本选择方式#

需求场景	推荐版本	原因
分布式系统唯一标识	v1/v6	时间有序，强唯一性
隐私敏感场景	v4	完全随机，不暴露硬件信息
需要重复生成相同UUID	v3/v5	基于命名空间的确定性
数据库主键	v6/v7	时间排序，提高索引效率
自定义需求	v8	灵活实现私有逻辑

关键注意事项#

隐私风险：v1/v6可能暴露MAC地址，需评估使用场景哈希安全：优先选择v5而非v3 标准兼容：v6+目前仍处于草案阶段，生产环境需谨慎随机数质量：v4需使用加密级随机数生成器

【Deepseek】扫地机器人规控与强化学习结合的可能性探索

Thu, 20 Feb 2025 06:51:43 GMT

问：#

我想将强化学习融合到移动机器人（如扫地机器人等）的运动控制中，我该如何去思考这种可行性，请你给我一条可能的方向，并具体的给出一些你的思考

Deepseek答：#

思考过程：#

嗯，用户想将强化学习应用到移动机器人，比如扫地机器人的运动控制中。首先，我需要理解他们的需求和背景。可能他们有一定的机器人学或编程基础，但不太清楚如何结合强化学习。他们可能需要一个明确的方向和一些具体的思考点来开始项目。

我应该先考虑移动机器人运动控制的关键任务，比如路径规划、避障、动态环境适应等。然后想想强化学习在这些任务中的应用潜力。可能用户已经知道传统方法如PID控制、SLAM，但想探索更智能的自适应方法。

接着，要分析融合强化学习的可行性。这包括强化学习的优势，比如处理动态环境和通过试错优化策略，以及挑战，比如样本效率、实时性、安全性和泛化能力。可能需要具体例子来说明，比如DQN或PPO在避障中的使用，或者如何模拟训练环境来加速学习。

然后，思考用户可能未明说的需求。他们可能希望有一个实际的步骤指南，比如从仿真到实际部署的流程，或者如何处理安全性和现实中的各种噪声。此外，用户可能需要比较不同强化学习算法的优缺点，以及如何设计奖励函数，这是强化学习成功的关键。

最后，建议分阶段实施，从仿真验证开始，再实物测试，逐步迭代优化。同时，推荐相关的工具和框架，如ROS、Gazebo、PyTorch，帮助用户快速上手。还要提醒用户注意现实中的挑战，如传感器噪声、计算资源限制，以及如何确保安全约束，避免训练过程中损坏机器人。

将强化学习（Reinforcement Learning, RL）与移动机器人运动控制结合是一个极具潜力的方向，但需要根据具体场景科学拆解问题。以下是一条可行方向的思考框架及具体实施建议：

核心思路：强化学习解决传统控制难以处理的非结构化场景#

传统移动机器人控制（PID、MPC、基于规则的避障）在结构化环境（如固定障碍物、已知地图）中表现良好，但在动态复杂场景（如人群穿梭、未知地形、多目标优化）时可能失效。强化学习的优势在于通过自主学习建立环境响应策略，适合处理以下问题：

动态避障：如扫地机器人在儿童玩具随机散落的地面自主导航
能耗与效率平衡：自动调整运动策略以优化清扫覆盖率+续航时间
多模态决策：在楼梯边缘判断继续前进/后退/呼叫人工干预

具体实现路径（以扫地机器人为例）#

1.问题建模#

状态空间（State Space）：激光雷达点云（降维至10-20个扇形区域）+ IMU姿态+剩余电量+历史路径（最后N个坐标） 动作空间（Action Space）：离散化方向角（如左转15°/直行/右转15°） + 速度档位（低速/高速） 奖励函数（关键难点）：

基础奖励：每分钟清扫面积 × 0.1
动态惩罚：碰撞惩罚-2（需区分轻微触碰与卡死），重复区域访问惩罚 × 衰减系数
长期奖励：剩余电量每10%额外+0.5（鼓励节能）安全约束：靠近楼梯时激活额外负奖励-5

2.算法选择与优化#

PPO算法（Stable Baselines3实现）：适合连续动作空间且数据效率较高

模仿学习加速训练：

第一阶段用传统A*算法生成10万条轨迹作为初始化经验池
第二阶段加入真实环境交互数据，采用HER（Hindsight Experience Replay）处理稀疏奖励问题

神经网络架构：

输入层：激光雷达数据用1D CNN提取空间特征（3层卷积，核大小[5,3,3]）
中间层：LSTM处理时序依赖（隐含层128维）
输出层：使用GMM（高斯混合模型）生成平滑动作分布

3.仿真到现实的迁移（Sim2Real）#

高保真仿真构建：

使用ROS+Gazebo建立动态环境模型，引入随机运动的虚拟行人（速度0.2-1.5m/s）
在Webots中模拟地毯摩擦力变化（μ=0.3到0.7随机扰动）

域随机化（Domain Randomization）：

训练时随机化传感器噪声（激光测距±3cm误差）
随机化环境光照条件（影响视觉传感器）

实物系统关键优化：

部署时使用TensorRT加速模型推理（确保200ms内完成决策）
安全层设计：当RL输出动作导致IMU角速度突变>30°/s时触发EC刹车

典型挑战与应对策略#

样本效率问题：#

采用离线强化学习（Offline RL）预训练，初期仅用仿真数据即可达到70%基准性能
设计课程学习（Curriculum Learning），从简单空旷环境逐步过渡到复杂场景

现实安全风险：#

双重控制系统：RL输出作为建议速度，由传统控制器计算最终执行指令
运行时验证（Runtime Verification）：用线性时态逻辑（LTL）实时监测是否违反”始终可停于1米内”等安全规则

多目标权衡：#

分层强化学习架构：底层控制负责避障，上层策略通过软性约束（如能耗权重λ）动态调整目标重要性
使用多目标PPO（MOPPO）生成Pareto前沿解集

验证指标设计#

定量指标：#

覆盖率（Coverage Rate）：单位时间清扫面积/理论最大面积
风险指数：∑(危险距离 < 0.2m的持续时间) × 系数
能耗比：实际耗电量 / 理论最低耗电量（通过A*路径计算）

定性测试场景：#

动态障碍测试：在8m×8m区域内投放10个随机运动气球（速度0.5m/s）
地形突变测试：在运行中突然铺设厚地毯（摩擦系数变化>40%）

工具链推荐#

仿真平台：NVIDIA Isaac Sim（支持物理精确模拟）
RL框架：Ray RLlib（支持分布式训练）
嵌入式部署：ROS2 + ONNX Runtime（轻量化部署）
硬件原型：选用Jetson Orin+NX+二维激光雷达（如RPLIDAR A3）

通过这个框架，可在6-8个月内构建出原型系统。初期建议从「仿真环境避障」小目标切入，逐步扩展到多任务协同。需要注意现实场景的数据闭环建设——建议设计自动标注系统（如通过机器人实际碰撞记录反标失败轨迹）。

【Deepseek】Q-learning

Wed, 19 Feb 2025 09:33:50 GMT

在强化学习中，Q-learning 是一种经典的 off-policy 算法，用于学习最优策略。它的核心思想是通过更新 Q 值（动作价值函数）来找到在给定状态下采取哪个动作能够最大化累积奖励。Q-learning 的策略可以分为两个部分：行为策略（用于探索）和目标策略（用于学习）。

1. Q-learning 的策略#

Q-learning 的策略分为两部分：（1）行为策略（Behavior Policy）

用于与环境交互，生成经验数据（状态、动作、奖励、下一状态）。
通常使用 ε-贪婪策略（ε-greedy policy）：
- 以概率 ε 随机选择动作（探索）。
- 以概率 1-ε 选择当前 Q 值最大的动作（利用）。
通过调整 ε，可以控制探索与利用的平衡。

（2）目标策略（Target Policy）

用于更新 Q 值，目标是找到最优策略。
Q-learning 使用贪婪策略（greedy policy）作为目标策略：

总是选择当前 Q 值最大的动作。

这意味着 Q-learning 直接学习最优策略，而不依赖于行为策略。

1
### 2. Q-learning 的更新规则
2

3
Q-learning 通过以下公式更新 Q 值：
4
Q(s,a)←Q(s,a)+α[r+γmax⁡a′Q(s′,a′)−Q(s,a)]
5
Q(s,a)←Q(s,a)+α[r+γa′maxQ(s′,a′)−Q(s,a)]
6

7
```cpp
8
ss：当前状态。
9

10
aa：当前动作。
11

12
rr：执行动作后获得的奖励。
13

14
s′s′：下一状态。
15

16
a′a′：在下一状态中可能采取的动作。
17

18
αα：学习率，控制更新步长。
19

20
γγ：折扣因子，衡量未来奖励的重要性。

关键点：

Q-learning 更新时，使用 max 操作来选择下一状态的最优动作，这体现了它的 off-policy 特性。

它不依赖于行为策略，而是直接学习最优策略。

3. Q-learning 的特点#

Off-policy：学习的最优策略（目标策略）与行为策略可以不同。

收敛性：在满足一定条件下（如充分探索），Q-learning 可以收敛到最优 Q 值函数。

无需模型：Q-learning 是一种无模型（model-free）算法，不需要知道环境的状态转移概率。

4. Q-learning 的伪代码#

1
初始化 Q(s, a) 为任意值
2
for 每个回合:
3
    初始化状态 s
4
    while 状态 s 不是终止状态:
5
        使用 ε-贪婪策略从状态 s 选择动作 a
6
        执行动作 a，观察奖励 r 和下一状态 s'
7
        更新 Q 值：
8
            Q(s, a) = Q(s, a) + α * [r + γ * max(Q(s', a')) - Q(s, a)]
9
        更新状态 s = s'

C++ enum 和 enum class

Thu, 05 Dec 2024 02:03:28 GMT

enum 和 enum class#

在 C++ 中，枚举（enum）是一种非常常见的类型，用于定义一组有命名的常量。然而，传统的 enum 在某些场景下存在一些局限性，比如作用域污染和类型安全问题。为了改进这些问题，C++11 引入了 enum class，也称为强类型枚举。

1.什么是`enum`？#

enum 是一种定义枚举类型的方式，用于表示一组相关的常量。其语法如下：

1
enum EnumName {
2
    Enumerator1,
3
    Enumerator2,
4
    Enumerator3
5
};

特点：

枚举值是全局的，无需通过枚举类型名称访问。
枚举值可以隐式地转换为整型。

示例代码：

1
#include <iostream>
2

3
enum Color { Red, Green, Blue };
4

5
int main() {
6
    Color color = Red; // 直接使用枚举值
7

8
    if (color == 0) { // 隐式转换为整型
9
        std::cout << "Color is Red" << std::endl;
10
    }
11
    return 0;
12
}

缺点：

枚举值在全局作用域中可能导致命名冲突。
枚举值可以隐式转换为整型，容易引发意外错误。

2.什么是`enum class`？#

enum class 是 C++11 引入的增强版枚举，提供更强的类型安全性和作用域管理。其语法如下：

1
enum class EnumName : UnderlyingType {
2
    Enumerator1,
3
    Enumerator2,
4
    Enumerator3
5
};

特点：

枚举值是局部作用域的，必须通过 EnumName::Enumerator 访问。
枚举值不能隐式转换为整型，只有显式转换才能完成。
可以显式指定底层类型（UnderlyingType），例如 int, uint8_t 等。

示例代码：

1
#include <iostream>
2

3
enum class Color { Red, Green, Blue };
4

5
int main() {
6
    Color color = Color::Red; // 必须通过作用域访问
7

8
    // 不允许隐式转换为整型
9
    // if (color == 0) { // 错误
10
    if (static_cast<int>(color) == 0) { // 必须显式转换
11
        std::cout << "Color is Red" << std::endl;
12
    }
13
    return 0;
14
}

优点：

枚举值有作用域，避免命名冲突。
提供更强的类型安全性。
灵活的底层类型选择。

3.`enum` 和 `enum class` 的对比#

特性	enum	enum class
作用域	枚举值在全局作用域中	枚举值在全局作用域中
类型安全性	可隐式转换为整型	不可隐式转换，必须显式转换
底层类型	通常是 int，不可指定	可指定，例如 uint8_t
代码可读性	不强，可能丢失上下文	强，必须通过作用域访问
向后兼容性	兼容 C 语言	需要支持 C++11 或更高版本

4. 使用场景分析#

enum 的适用场景：
- 与 C 语言代码兼容的项目。
- 枚举值数量较少且无需严格的类型安全性。
enum class 的适用场景：
- 大型项目，避免命名冲突。
- 对类型安全性和代码可读性有较高要求的场景。
- 希望优化内存占用时，通过自定义底层类型实现。

5. 代码示例：传统枚举与强类型枚举的比较#

1
#include <iostream>
2
#include <cstdint>
3

4
// 传统枚举
5
enum Color {
6
    Red,
7
    Green,
8
    Blue
9
};
10

11
// 强类型枚举
12
enum class Shape : uint8_t {
13
    Circle,
14
    Square,
15
    Triangle
16
};
17

18
int main() {
19
    // 使用传统枚举
20
    Color c = Red;
21
    if (c == 0) { // 隐式转换为整型
22
        std::cout << "Color is Red (traditional enum)" << std::endl;
23
    }
24

25
    // 使用强类型枚举
26
    Shape s = Shape::Circle;
27
    // if (s == 0) { // 错误：不能隐式转换
28
    if (static_cast<uint8_t>(s) == 0) { // 必须显式转换
29
        std::cout << "Shape is Circle (enum class)" << std::endl;
30
    }
31

32
    return 0;
33
}

输出结果：

1
Color is Red (traditional enum)
2
Shape is Circle (enum class)

6. 总结#

enum 是传统的枚举类型，但在作用域和类型安全性方面存在一定缺陷。
enum class 是现代 C++ 的增强版枚举，提供更强的类型安全性、更好的作用域管理以及灵活的底层类型支持。
在现代 C++ 编程中，优先使用 enum class，除非需要与 C 语言代码兼容。

LeetCode 560. 和为 K 的子数组

Wed, 04 Dec 2024 07:40:16 GMT

有一说一，这道题我不会写（只会暴力n^2），没看题解让GPT帮我想的。

【题目描述】#

给你一个整数数组 nums 和一个整数 k ，请你统计并返回该数组中和为 k 的子数组的个数。

子数组是数组中元素的连续非空序列。

【示例 1】：输入：nums = [1,1,1], k = 2 输出：2

【示例 2】：输入：nums = [1,2,3], k = 3 输出：2

【思路分析】#

这道题的目标是： 1.找出数组中和为 k 的子数组的数量。 2.子数组是连续的一段数组元素。

对于这类问题，我们可以用两种主要的解法： **暴力解法：**枚举所有可能的子数组，计算其和，统计和为 k 的子数组数量。

时间复杂度高，为 𝑂(𝑛^2) **前缀和 + 哈希表：**通过记录前缀和出现的次数，快速判断是否存在符合条件的子数组。
时间复杂度为 𝑂(𝑛)

前缀和的定义 前缀和的本质是从数组起点到当前位置的累加和。假设有一个数组 nums = [1, 2, 3, 4]：

它的前缀和数组是 [1, 3, 6, 10]。
- 1 = 1 (第一个元素)
- 3 = 1 + 2 (前两个元素的和)
- 6 = 1 + 2 + 3 (前三个元素的和)
- 10 = 1 + 2 + 3 + 4 (前四个元素的和)

子数组和与前缀和的关系 如果我们需要计算数组中某一段的子数组和，例如子数组 nums[i]，它的和可以表示为： sum(𝑛𝑢𝑚𝑠[𝑖<𝑗>])=prefix_sum[𝑗]−prefix_sum[𝑖−1]

其中，prefix_sum[j] 是从开头到第 j 个元素的累加和；
prefix_sum[i-1] 是从开头到第 i-1 个元素的累加和；
差值就是从第 i 到第 j 的子数组和。

例如：

对于数组 nums = [1, 2, 3, 4]：
- 要找子数组 [2, 3] 的和，可以用前缀和 prefix_sum[2] - prefix_sum[0] = 6 - 1 = 5。

用前缀和高效解决问题的核心思路 子数组和的问题可以用前缀和巧妙解决：

1.遍历数组时，我们计算当前位置的前缀和。 2.如果某段子数组的和等于目标值 k，我们可以用公式： prefix_sum[j]−prefix_sum[i−1]=k 转换为： prefix_sum[i−1]=prefix_sum[j]−k 3.也就是说，只要前面某个前缀和等于当前前缀和减去目标值 k，就存在一个子数组的和为 k。 4.用一个哈希表（unordered_map）记录前缀和出现的次数，可以快速判断这种情况，并累加满足条件的次数。

对于本题具体实现步骤

初始化 1.使用一个变量 currentSum 来记录当前的前缀和。 2.用一个哈希表 prefixSum 来记录每个前缀和出现的次数，初始时存入 prefixSum[0] = 1（表示前缀和为 0 出现了 1 次，处理初始情况）。 3.定义一个变量 answer 来统计满足条件的子数组数量。

遍历数组 对于数组中的每个元素： 1.更新当前的前缀和：currentSum += nums[i]。 2.检查哈希表中是否存在 currentSum - k：

如果存在，说明从某个位置到当前的子数组和为 k，将对应的次数加到 answer 中。 3.将 currentSum 更新到哈希表中，表示当前前缀和出现了一次。

返回结果 遍历结束后，answer 就是满足条件的子数组数量。

【C++解答】#

1
class Solution {
2
public:
3
    int subarraySum(vector<int>& nums, int k) {
4
        // 使用前缀和 + 哈希表
5
        unordered_map<int, int> prefixSum;
6
        prefixSum[0] = 1; // 初始条件，前缀和为0时的出现次数为1
7
        int currentSum = 0;
8
        int answer = 0;
9

10
        for (int num : nums) {
11
            currentSum += num; // 更新当前的前缀和
12

13
            // 检查是否存在以当前元素结尾的子数组和为k
14
            if (prefixSum.find(currentSum - k) != prefixSum.end()) {
15
                answer += prefixSum[currentSum - k];
16
            }
17

18
            // 更新前缀和的出现次数
19
            prefixSum[currentSum]++;
20
        }
21

22
        return answer;
23
    }
24
};

Leetcode 41. 缺失的第一个正数

Wed, 04 Dec 2024 06:59:33 GMT

【题目描述】#

给你一个未排序的整数数组 nums ，请你找出其中没有出现的最小的正整数。

请你实现时间复杂度为 O(n) 并且只使用常数级别额外空间的解决方案。

【示例1】输入：nums = [1,2,0] 输出：3 解释：范围 [1,2] 中的数字都在数组中。

【示例2】输入：nums = [3,4,-1,1] 输出：2 解释：1 在数组中，但 2 没有。

【示例3】输入：nums = [7,8,9,11,12] 输出：1 解释：最小的正数 1 没有出现。

【思路分析】#

这个题目分析起来不难，甚至说很好理解，就是找最小的没出现的正整数，简单的做法就是排个序，找到大于等于1的地方，然后从后面开始找只要不是+1，返回那个值就行。但是问题就是题目要求是O(N)，只要涉及排序就变成了log了，所以不能排序，只能考虑遍历。

所以新的思路就来了：我们思考排序的性质，排序后我们需要找到1然后从后面开始找，那么我们是否也可以直接将nums中的值按顺序保存在新的数组中？类似于之前在做统计一个字符串每个单词的个数，我们设置哈希表或者一个26长度的vector一样。如果是别的题一定是不行的，但这道题只是要找到一个值，所以可以。

所以，遍历nums，将符合[1,nums.size()]的数以该值为下标加入到新的vector中；最终只需遍历到第一个与下标值不符的数即可。

【C++解答】#

1
class Solution {
2
public:
3
    int firstMissingPositive(vector<int>& nums) {
4
        vector<int> tmpVec(nums.size() + 1, 0);
5
        for (int i = 0; i < nums.size(); i++) {
6
            if (nums[i] >= 1 && nums[i] <= nums.size()) {
7
                tmpVec[nums[i] - 1] = nums[i];
8
            }
9
        }
10
        for (int i = 0; i < tmpVec.size(); i++) {
11
            if (tmpVec[i] != i + 1)
12
                return i + 1;
13
        }
14
        return -1;
15
    }
16
};

LeetCode239. 滑动窗口最大值

Wed, 04 Dec 2024 06:22:57 GMT

【题目描述】给你一个整数数组 nums，有一个大小为 k 的滑动窗口从数组的最左侧移动到数组的最右侧。你只可以看到在滑动窗口内的 k 个数字。滑动窗口每次只向右移动一位。返回滑动窗口中的最大值。【示例 1：】

1
输入：nums = [1,3,-1,-3,5,3,6,7], k = 3
2
输出：[3,3,5,5,6,7]
3
解释：
4
滑动窗口的位置                最大值
5
---------------               -----
6
[1  3  -1] -3  5  3  6  7       3
7
 1 [3  -1  -3] 5  3  6  7       3
8
 1  3 [-1  -3  5] 3  6  7       5
9
 1  3  -1 [-3  5  3] 6  7       5
10
 1  3  -1  -3 [5  3  6] 7       6
11
 1  3  -1  -3  5 [3  6  7]      7

【思路优先队列】看到找最大首先应该想到的是优先队列实现，因为这样就不需要在每一次数据加入的时候进行比较；但是，优先队列应该设计成什么样子的是个问题，起初我直接设计为priority_queue<int,vector<int>,cmp> 这导致了我在移动窗口的时候无法去除窗口外的值。所以思考一下，由于设计为优先队列，那么去除窗口外的值一定不可能是一次移动去一个的，也不可能保证队列和窗口一样的 k 个值。

那么问题就转变为了：如果出现了去除top值后新的top值不在窗口内该如何解决？

针对这个问题，我们可以修改优先队列的设计方式，使其在值的基础上增加索引（nums中该值的位置），也就是<值，索引>的结构。当我在滑动窗口时，新的值直接进入优先队列前，队列内进行处理：如果top值在窗口内，则继续将新的值加入队列，如果top值不在窗口，则进行pop操作，直到top值在窗口为止。这样就能保证，每一次滑动时，队列内的top值一定是窗口内的值。

【C++实现代码】

1
class Solution {
2
public:
3
    struct cmp {
4
        bool operator()(pair<int, int> p1, pair<int, int> p2) {
5
            return p1.first < p2.first;
6
        }
7
    };
8

9
    vector<int> maxSlidingWindow(vector<int>& nums, int k) {
10
        if (nums.size() < k) return {};
11

12
        priority_queue<pair<int, int>, vector<pair<int, int>>, cmp> pQueue;
13
        vector<int> answer;
14

15
        for (int i = 0; i < nums.size(); i++) {
16
            // 清理优先队列中过期的元素
17
            while (!pQueue.empty() && pQueue.top().second <= i - k) {
18
                pQueue.pop();
19
            }
20

21
            pQueue.push({nums[i], i});
22

23
            if (i >= k - 1) {
24
                answer.push_back(pQueue.top().first);
25
            }
26
        }
27
        return answer;
28
    }
29
};

new * delete

Mon, 02 Dec 2024 07:25:40 GMT

使用new分配内存#

在C++中，为一个数据对象（可以是结构，也可以是基本类型）获得并制定分配内存的通用格式：

1
typeName * pointer_name = new typeName;

例如：在运行阶段为一个int值法分配未命名的内存：

1
int * pn = new int;

内存区域#

对于指针，需要说明：new分配的内存块通常与常规变量声明分配的内存块不同。变量和指针的值都存储在栈（stack）的内存区域中，而new从堆（heap）或自由存储区（free store）的内存区域分配内存。

对于上述说明的补充： 栈（Stack）和堆（Heap）：

栈：用于存储局部变量和函数调用信息，由系统自动管理内存分配和释放。栈内存的大小是有限的，且速度较快。
堆：由程序显式管理内存，使用如new和delete进行分配和释放。堆内存较大且灵活，但分配速度比栈慢。

变量的存储：

常规变量声明：如int x = 10;分配在栈上，编译器会在变量生命周期结束时自动释放内存。
指针变量：指针本身也是一种变量，它存储的是地址。这个地址指向其他内存位置，可以是栈上，也可以是堆上的数据。

new分配内存：当使用new分配内存时，分配的是堆上的内存，指针本身依然存在于栈上。程序员需要显式使用delete释放这块内存，否则会造成内存泄漏。

使用new创建动态数组#

在C++中，创建动态数组很容易;只要将数组的元素类型和元素数目告诉new即可。必须在类型名后加上方括号，其中包含元素数目。

1
type_name * pointer_name = new type_name [num_elements];

例如，要创建一个包含10个int元素的数组，可以这样做:

1
int * psome = new int [10];

当程序使用完new分配的内存块时，应使用delete释放他们。

1
delete [] psome;

使用delete释放内存#

delete运算符的功能是将使用的内存归还给内存池。使用delete时，后面要加上指向内存块的指针（最初是用new分配的）例如：

1
int * ps = new int;
2
. . .
3
delete ps;

上述操作会释放ps指向的内存，但不会删除指针ps本身。例如，可以将ps重新指向另一个新分配的内存块。

注意： 一定要配对地使用new和delete；否则将发生内存泄漏（memory leak)，也就是说被分配的内存再也无法使用了。

不要尝试释放已经释放的内存块，C++标准指出，这样做的结果将是不确定的，这意味着什么情况都可能发生。另外，不能使用delete来释放声明变量所获得的内存。

1
int * ps = new int;     //OK
2
delete ps;              //ok
3
delete os;              //not ok now

只能用delete来释放使用new分配的内存。然而，对空指针使用delete是安全的。

不要创建两个指向同一个内存块的指针，因为这将增加错误地删除同一个内存块两次的可能性

总结： 使用new和delete时，应遵守以下规则：

不要使用 delete 来释放不是 new 分配的内存。
不要使用 delete 释放同一个内存块两次。
如果使用 new[] 为数组分配内存，则应使用 delete[] 来释放。
如果使用 new[] 为一个实体分配内存，则应使用delete(没有方括号)来释放。
对空指针应用 delete 是安全的。

sizeof运算符与指针#

对数组应用sizeof运算符得到的是数组的长度，而对指针应用sizeof得到的是指针的长度，即使指针指向的是一个数组。

Q1：下述代码应该输出什么？

1
#include <iostream>
2

3
int main() {
4

5
    int tacos[10];
6
    int* pz = new int[10];
7

8
    std::cout << "sizeof(tacos)=" << sizeof(tacos) << std::endl;
9
    std::cout << "sizeof(pz)=" << sizeof(pz) << std::endl;
10

11
    return 0;
12
}

A1：

1
sizeof(tacos)=40
2
sizeof(pz)=8

分析： tacos是一个包含10个int元素的静态数组。在sizeof运算中，静态数组的名称并不会退化为指针，而是代表整个数组。因此，sizeof(tacos)的值是数组占用的总字节数，即sizeof(int) * 10

pz是一个指针变量（类型为int*），它存储的是动态分配数组的地址。对指针变量使用sizeof时，返回的是指针本身的大小，与指针指向的数据类型或分配内存的大小无关。指针的大小取决于平台（32位系统通常为4字节，64位系统通常为8字节）。

补充：栈、堆和内存泄漏#

（C++ premier plus 4.8.5 自动存储、静态存储和动态存储）如果使用 new 运算符在自由存储空间(或堆)上创建变量后，没有调用 delete，将发生什么情况呢?如果没有调用 delete，则即使包含指针的内存由于作用域规则和对象生命周期的原因而被释放，在自由存储空间上动态分配的变量或结构也将继续存在。实际上，将会无法访问自由存储空间中的结构，因为指向这些内存的指针无效。这将导致内存泄漏。被泄漏的内存将在程序的整个生命周期内都不可使用<这些内存被分配出去>，但无法收回。极端情况(不过不常见)是，内存泄漏可能会非常严重，以致于应用程序可用的内存被耗尽，出现内存耗尽错误，导致程序崩溃。另外，这种泄漏还会给一些操作系统或在相同的内存空间中运行的应用程序带来负面影响，导致它们崩溃。即使是最好的程序员和软件公司，也可能导致内存泄漏。要避免内存泄漏，最好是养成这样一种习惯,即同时使用 new 和 delete 运算符，在自由存储空间上动态分配内存，随后便释放它。注意<指针是功能最强大的> C++工具之一，但也最危险，因为它们允许执行对计算机不友好的操作，如使用未经初始化的指针来访问内存或者试图释放同一个内存块两次。另外，在通过实践习惯指针表示法和指针概念之前，指针是容易引起迷惑的。由于指针是 C++编程的重要组成部分，本书后面将更详细地讨论它。本书多次对指针进行了讨论，就是希望您能够越来越熟悉它。

还分不清“大端”和“小端”？

Tue, 26 Nov 2024 07:48:43 GMT

在工程中，我们时常可以听到“大端存储”“小端存储”这类字眼，所以这个“大”和“小”到底指的是什么呢？

首先我们先来讨论一下字在机器中的存储，首先我们约定一个字是32位，半字是16位，字节是8位（对应int short char）。如果一台机器是字节可寻址的，那么能够单独寻址的最低精度就是字节。如果是字可寻址的，那么能够单独寻址的最低精度就是字。

因此，一个字在内存中看起来是这样的：每个字里面有4个字节。(MSB指最高有效字节(most signifcant byte)；LSB指最低有效字节(least signifcant byte))。例如，我们在程序中有一个整数变量为 0x11223344，那么它在内存中看起来是这样的：

然而，字节可寻址的机器中存在着一个有趣的问题，就是一个字中各个字节排列的顺序。在字节可寻址的机器中，一个4字节的字，如果从地址100开始，那么这个字其实占据了内存中100、101、102、103这4个连续字节

这4个字节组合起来就成了地址100处的一个字

假设100处的这个字的值为0x11223344，那么这4个字节在字中有两种可能的组织方式: 组织方式1: 在这种组织方式中，该字的MSB(包含11)位于字的地址即100上。这种组织方式称为大端模式。

组织方式2:

在这种组织方式中，该字的LSB(包含44)位于字的地址即100上，这种组织方式称为小端模式。

由此可见，大端还是小端是根据哪个字节处于字的地址上来区分的，如果是MSB，那就是大端；如果是LSB，那就是小端

std::condition_variable 条件变量

Thu, 14 Nov 2024 09:28:36 GMT

std::condition_variable 条件变量#

你可以直接去看 https://en.cppreference.com/w/cpp/thread/condition_variable 的详细解读，当然也可以看下述我进行的翻译和摘要

Defined in header <condition_variable>

std::condition_variable是一种与std::mutex一起使用的同步原语，用于阻止一个或多个线程，直到另一个线程修改共享变量（条件）并通知 std::condition_variable

1.使用条件#

情况1： 某个线程打算 修改共享变量 需要完成以下三个条件：

获取std::mutex （通常通过std::lock_guard ）
在拥有锁的情况下修改共享变量
对std::condition_variable调用notify_one或notify_all （可以在释放锁后完成）

情况2： 某线程想要等待条件变量需要完成以下两个条件：

获取用于保护共享变量的互斥锁上的std:: unique_lock < std:: mutex >
执行以下操作之一：
- 检查情况，是否已更新并通知
- 对std::condition_variable调用wait 、 wait_for或wait_until （以原子方式释放互斥体并挂起线程执行，直到通知条件变量、超时到期或发生虚假唤醒，然后在返回之前以原子方式获取互斥体）
- 检查条件，如果不满足则继续等待。
或者：
- 使用wait 、 wait_for和wait_until的谓词重载，它执行相同的三个步骤

2.Function#

Notification 通知：

notify_one：通知一个等待线程
notify_all：通知所有等待线程

Waiting 等待：

wait：阻塞当前线程，直到条件变量被唤醒
wait_for：阻塞当前线程，直到条件变量被唤醒或指定的超时时间后
wait_until：阻塞当前线程，直到条件变量被唤醒或到达指定时间点

3.例程#

1
#include <condition_variable>
2
#include <iostream>
3
#include <mutex>
4
#include <string>
5
#include <thread>
6

7
std::mutex m;
8
std::condition_variable cv;
9
std::string data;
10
bool ready = false;
11
bool processed = false;
12

13
void worker_thread()
14
{
15
    // wait until main() sends data
16
    std::unique_lock lk(m);
17
    cv.wait(lk, []{ return ready; });
18

19
    // after the wait, we own the lock
20
    std::cout << "Worker thread is processing data\n";
21
    data += " after processing";
22

23
    // send data back to main()
24
    processed = true;
25
    std::cout << "Worker thread signals data processing completed\n";
26

27
    // manual unlocking is done before notifying, to avoid waking up
28
    // the waiting thread only to block again (see notify_one for details)
29
    lk.unlock();
30
    cv.notify_one();
31
}
32

33
int main()
34
{
35
    std::thread worker(worker_thread);
36

37
    data = "Example data";
38
    // send data to the worker thread
39
    {
40
        std::lock_guard lk(m);
41
        ready = true;
42
        std::cout << "main() signals data ready for processing\n";
43
    }
44
    cv.notify_one();
45

46
    // wait for the worker
47
    {
48
        std::unique_lock lk(m);
49
        cv.wait(lk, []{ return processed; });
50
    }
51
    std::cout << "Back in main(), data = " << data << '\n';
52

53
    worker.join();
54
}

输出结果：

1
main() signals data ready for processing
2
Worker thread is processing data
3
Worker thread signals data processing completed
4
Back in main(), data = Example data after processing

C++ 文件操作指令 <随缘记录>

Thu, 14 Nov 2024 07:35:55 GMT

<随缘记录> 看到哪个就记哪个

fsync#

1
fsync(fd)

将文件描述符fd关联的文件缓冲区中的数据强制刷新到磁盘，确保数据不会丢失。调用fsync可以保证数据在断电或程序崩溃时不会丢失。

write#

1
write(fd, buf, strlen(buf))

尝试将buf指向的字符串写入文件描述符fd。strlen(buf)计算字符串的长度。

1
if (write(fd, buf, strlen(buf)) == strlen(buf))
2
{
3
    …………
4
}

如果write函数返回的值等于strlen(buf)，表示数据已经成功写入文件，没有发生截断。

《原则》瑞-达利欧（读书笔记）

Wed, 13 Nov 2024 14:09:09 GMT

< 根据读的进度慢慢更新，筛选我觉得有价值的句子 >#

“尽管回头快来看，几乎所有事情都是必然的和符合逻辑的，但在事情的发展过程中看，就觉不是这么清晰明了”

“学习过程是一连串的实时反馈循环：我们做决定，看到结果，然后根据结果改进对现实的理解。做到头脑极度开放能够增强这些反馈循环的效率，因为这能让你和其他人无比清晰地看到你在做什么，为什么这么做，而不会产生误解。你的头脑越开放，你就越不会自欺，其他人也就越会给你诚实的反馈”

“完美是不存在的，它只是一个目标，激发永不停息的进化过程”

“现实是为了整体趋向最优化，而不是为了个体”

“在确定什么对你重要、你要追求什么时，你的视角是多么重要”

ROS 环境 Cartographer 安装测试

Wed, 06 Nov 2024 03:56:58 GMT

本篇主要用作个人学习记录，大部分内容转载自各优秀博主。

ros noetic 安装 cartographer#

1.1 安装依赖项#

首先，我们需要安装一些依赖项。在终端中输入以下命令：

1
sudo apt-get update
2
sudo apt-get install -y python-wstool python-rosdep ninja-build

1.2 安装 Cartographer ROS#

接下来，创建一个新的工作空间并获取 Cartographer ROS 代码：

1
mkdir -p ~/catkin_ws/src
2
cd ~/catkin_ws/src
3
git clone https://github.com/cartographer-project/cartographer_ros.git

这部分也可以这样：

1
mkdir catkin_ws
2
cd catkin_ws
3
wstool init src
4
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall

1.3 安装所有必要的依赖项#

使用 rosdep 安装所有必要的依赖项：

1
cd ~/catkin_ws
2
rosdep update
3
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

我使用的是noetic版本也就是ubuntu20.04，在这里这条指令会有问题，替换为下条指令

1
rosdep install -i --from-path src --ignore-src -r

1.4 构建 Cartographer ROS#

1
cd ~/catkin_ws
2
catkin_make_isolated --install --use-ninja

我在这里出现问题：问题 ① absl库报错

1
CMake Error at CMakeLists.txt:32 (find_package):
2
  By not providing "Findabsl.cmake" in CMAKE_MODULE_PATH this project has
3
  asked CMake to find a package configuration file provided by "absl", but
4
  CMake did not find one.
5

6
  Could not find a package configuration file provided by "absl" with any of
7
  the following names:
8

9
    abslConfig.cmake
10
    absl-config.cmake
11

12
  Add the installation prefix of "absl" to CMAKE_PREFIX_PATH or set
13
  "absl_DIR" to a directory containing one of the above files.  If "absl"
14
  provides a separate development package or SDK, be sure it has been
15
  installed.

解决方法：

1
sudo apt-get install stow

去catkin_ws/src/cartographer/scripts路径下

1
sudo chmod +x install_abseil.sh
2
./install_abseil.sh

1.5 更新环境变量#

1
source install_isolated/setup.bash

cartographer建图测试#

下载示例包（例如Deutsches Museum(德意志博物馆)的2D和3D背包系列）到~/Downloads（这里下载到哪时无所谓的），并使用roslaunch来调出演示：

1
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag

这个我下载不下来，会有问题，我最后从网上随便找了一个bag尝试的

1
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

结果展示：

一口气看完 FastDDS ，耗时666分钟制作

Fri, 01 Nov 2024 02:36:31 GMT

FastDDS 原理#

原文：https://fast-dds.docs.eprosima.com/en/latest/fastdds/getting_started/definitions.html 本文仅翻译原文

什么是DDS？#

数据分发服务 (DDS)是一种以数据为中心的通信协议，用于分布式软件应用程序通信。它描述了支持数据提供者和数据消费者之间通信的通信应用程序编程接口（API）和通信语义。

由于它是一个以数据为中心的发布订阅（DCPS）模型，因此在其实现中定义了三个关键的应用程序实体：发布实体，定义信息生成对象及其属性；订阅实体，定义信息消费对象及其属性；和配置实体，定义作为主题传输的信息类型，并使用其服务质量（QoS）属性创建发布者和订阅者，确保上述实体的正确性能。

DDS 使用 QoS 来定义 DDS 实体的行为特征。 QoS 由单独的 QoS 策略（从 QoSPolicy 派生的类型对象）组成。这些在政策中进行了描述。

DCPS 概念模型#

在 DCPS 模型中，为通信应用系统的开发定义了四个基本元素。

Publisher. 它是 DCPS 实体，负责其实现的DataWriter的创建和配置。 DataWriter是负责实际发布消息的实体。每个消息都会有一个分配的主题，消息将在该主题下发布。

Subscriber. 它是 DCPS 实体，负责接收在其订阅的主题下发布的数据。它为一个或多个DataReader对象提供服务，这些对象负责将新数据的可用性传达给应用程序。

Topic.它是绑定发布和订阅的实体。它在 DDS 域中是唯一的。通过TopicDescription ，它允许发布和订阅的数据类型的统一。

Domain. 这一概念用于链接属于一个或多个应用程序的所有发布者和订阅者，这些发布者和订阅者在不同主题下交换数据。这些参与域的单独应用程序称为DomainParticipant 。 DDS 域由域 ID 来标识。 DomainParticipant定义域ID来指定其所属的DDS域。具有不同 ID 的两个 DomainParticipant 不知道彼此在网络中的存在。因此，可以创建多个通信渠道。这适用于涉及多个 DDS 应用程序的场景，它们各自的 DomainParticipants 相互通信，但这些应用程序不得相互干扰。 DomainParticipant充当其他 DCPS 实体的容器，充当发布者、订阅者和主题实体的工厂，并在域中提供管理服务。

这些元素如下图所示

什么是RTPS？#

实时发布订阅 (RTPS)协议是为支持 DDS 应用程序而开发的，是一种基于尽力而为传输（例如 UDP/IP）的发布-订阅通信中间件。此外，Fast DDS 还提供对 TCP 和共享内存 (SHM) 传输的支持。

它旨在支持单播和多播通信。

在继承自 DDS 的 RTPS 的顶部，可以找到域，它定义了一个单独的通信平面。多个域可以同时独立共存。域包含任意数量的RTPSParticipants ，即能够发送和接收数据的元素。为此，RTPSParticipants 使用他们的Endpoints ：

RTPSWriter ：能够发送数据的端点。

RTPSReader ：能够接收数据的端点。

RTPSParticipant 可以有任意数量的写入器和读取器端点。

通信围绕主题进行，主题定义和标记正在交换的数据。这些主题不属于特定参与者。参与者通过 RTPSWriters 对主题下发布的数据进行更改，并通过 RTPSReaders 接收与其订阅的主题关联的数据。通信单元称为Change ，它表示在 Topic 下写入的数据的更新。 RTPSReaders/RTPSWriters在其History上注册这些更改，History 是一种数据结构，用作最近更改的缓存。

在eProsima Fast DDS的默认配置中，当您通过 RTPSWriter 端点发布更改时，会在幕后执行以下步骤：

1.更改将添加到 RTPSWriter 的历史缓存中。 2.RTPSWriter 将更改发送到它知道的任何 RTPSReaders。 3.接收数据后，RTPSReaders 使用新的更改更新其历史缓存。

但是，Fast DDS 支持多种配置，允许您更改 RTPSWriters/RTPSReaders 的行为。 RTPS 实体默认配置的修改意味着 RTPSWriter 和 RTPSReaders 之间数据交换流的更改。此外，通过选择服务质量 (QoS) 策略，您可以通过多种方式影响这些历史缓存的管理方式，但通信循环保持不变。您可以继续阅读RTPS层部分来了解更多关于Fast DDS中RTPS协议的实现。

FastDDS 安装及使用#

FastDDS也是目前比较好的一个框架，FastDDS是由位于西班牙马德里的eProsima公司推出的免费开源DDS中间件解决方案，并提供支付技术支持服务。它的源码基于C++，规范基于OMG DDS 1.4 and the OMG RTPS 2.2。

这里先介绍一下依照官方如何安装FastDDS到系统，并进行简单的测试。

官方文档安装链接：https://fast-dds.docs.eprosima.com/en/latest/installation/binaries/binaries_linux.html ，这里出了安装步骤，还有很多原理和例程介绍。

这里是使用的操作系统是ubuntu 20.04进行安装。

FastDDS的安装内容主要有三部分：

FastDDS library

foonathan_memory_vendor: C++内存分配库
fastdcdr: CDR序列化(serialization)机制
fastrtps: FastDDS核心库

FastDDS Python bindings(可选)

FastDDS Gen

从IDL文件生成源代码工具，基于Java程序。由IDL文件中定义的data type生成代码的脚本工具。

1. 安装FastDDS#

下述的两个安装方法都需要在虚拟机配置VPN，具体如何配置这里不做教学

安装方法一： Linux installation from binaries 教程：https://fast-dds.docs.eprosima.com/en/latest/installation/binaries/binaries_linux.html

本人使用的是V2.6版本，在ubuntu20.04上进行测试，最新的3.0版本由于python3-xmlschema无法安装所以放弃使用了下载链接：https://www.eprosima.com/component/ars/items/eprosima-fast-dds-2-6?category_id=7&Itemid=102 下载好 eProsima_Fast-DDS-v2.6.8-Linux.tgz 后解压，在文件目录下运行install.sh

1
cd <extraction_directory>
2
sudo ./install.sh

安装后src文件夹包含以下包：

foonathan_memory_vendor ，一个 STL 兼容的 C++ 内存分配器库。 fastcdr ，一个根据CDR 标准进行数据序列化的 C++ 库(Section 10.2.1.2 OMG CDR）。 fastdds ， eProsima Fast DDS库的核心库。 fastddsgen ，一个 Java 应用程序，它使用 IDL 文件中定义的数据类型生成源代码。

安装方法二：Linux installation from sources 教程：https://fast-dds.docs.eprosima.com/en/latest/installation/sources/sources_linux.html (本人使用该方法) 安装依赖：

1
sudo apt install cmake g++ python3-pip wget git
2
sudo apt install libasio-dev libtinyxml2-dev
3
sudo apt install libssl-dev

使用colcon安装FastDDS库 1.安装依赖：

1
pip3 install -U colcon-common-extensions vcstool

2.创建Fast-DDS目录并下载将用于安装eProsima Fast DDS及其依赖项的存储库文件：

1
mkdir ~/Fast-DDS
2
cd ~/Fast-DDS
3
wget https://raw.githubusercontent.com/eProsima/Fast-DDS/master/fastdds.repos
4
mkdir src
5
vcs import src < fastdds.repos

3.构建包

1
colcon build --packages-up-to fastdds

2.Fast DDS-Gen 安装#

1
sudo apt install openjdk-11-jdk

如果在安装 Colcon 后已经安装了 Fast DDS，请跳过克隆Fast DDS-Gen的存储库，因为它已经可以在 colcon 工作区的src目录下找到

1
mkdir -p ~/Fast-DDS/src
2
cd ~/Fast-DDS/src
3
git clone --recursive https://github.com/eProsima/Fast-DDS-Gen.git fastddsgen
4
cd fastddsgen
5
./gradlew assemble

Fast-DDS-Gen文件夹包含以下软件包： share/fastddsgen ，生成的 Java 应用程序所在的位置。 scripts ，包含一些用户友好的脚本。注意：要使这些脚本可以从任何 shell 会话和目录访问，请将scripts夹路径添加到PATH环境变量。

3.（测试）编写一个简单的 C++ 发布者和订阅者应用程序#

原文：https://fast-dds.docs.eprosima.com/en/latest/fastdds/getting_started/simple_app/simple_app.html

3.1.创建目录树#

1
mkdir workspace_DDSHelloWorld && cd workspace_DDSHelloWorld
2
mkdir src build

3.2.导入链接库和其他依赖项#

1
source <path/to/Fast-DDS/workspace>/install/setup.bash

通过将 Fast DDS 安装目录添加到运行以下命令的当前用户的 shell 配置文件中的$PATH变量，可以从任何会话访问它们。

1
echo 'source <path/to/Fast-DDS/workspace>/install/setup.bash' >> ~/.bashrc

3.3.配置 CMake 项目#

我们将使用 CMake 工具来管理项目的构建。使用您喜欢的文本编辑器创建一个名为 CMakeLists.txt 的新文件，然后复制并粘贴以下代码片段。将此文件保存在工作区的根目录中。如果您已执行这些步骤，则它应该是workspace_DDSHelloWorld 。

1
cmake_minimum_required(VERSION 3.20)
2

3
project(DDSHelloWorld)
4

5
# Find requirements
6
if(NOT fastcdr_FOUND)
7
    find_package(fastcdr 2 REQUIRED)
8
endif()
9

10
if(NOT fastdds_FOUND)
11
    find_package(fastdds 3 REQUIRED)
12
endif()
13

14
# Set C++11
15
include(CheckCXXCompilerFlag)
16
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR
17
        CMAKE_CXX_COMPILER_ID MATCHES "Clang")
18
    check_cxx_compiler_flag(-std=c++11 SUPPORTS_CXX11)
19
    if(SUPPORTS_CXX11)
20
        add_compile_options(-std=c++11)
21
    else()
22
        message(FATAL_ERROR "Compiler doesn't support C++11")
23
    endif()
24
endif()
25

26
message(STATUS "Configuring HelloWorld publisher/subscriber example...")
27
file(GLOB DDS_HELLOWORLD_SOURCES_CXX "src/*.cxx")

3.4.Build the topic data type#

在工作空间目录下，执行以下命令：

1
cd src && touch HelloWorld.idl

这将在src目录中创建 HelloWorld.idl 文件。在文本编辑器中打开该文件，然后复制并粘贴以下代码片段。

1
struct HelloWorld
2
{
3
    unsigned long index;
4
    string message;
5
};

通过这样做，我们定义了HelloWorld数据类型，它有两个元素： uint32_t类型的索引和std::string类型的消息。剩下的就是生成在 C++11 中实现此数据类型的源代码。为此，请从src目录运行以下命令。

1
<path/to/Fast DDS-Gen>/scripts/fastddsgen HelloWorld.idl

在该命令结束后，生成如下的文件： HelloWorld.hpp：HelloWorld 类型定义。 HelloWorldPubSubTypes.cxx：Fast DDS 用于支持 HelloWorld 类型的接口。 HelloWorldPubSubTypes.h：HelloWorldPubSubTypes.cxx 的头文件。 HelloWorldCdrAux.ipp：HelloWorld 类型的序列化和反序列化代码。 HelloWorldCdrAux.hpp：HelloWorldCdrAux.ipp 的头文件。 HelloWorldTypeObjectSupport.cxx： TypeObject表示注册代码。 HelloWorldTypeObjectSupport.hpp：HelloWorldTypeObjectSupport.cxx 的头文件。

3.5 编写FastDDS发布者#

从工作区的src目录中，运行以下命令下载 HelloWorldPublisher.cpp 文件。

1
wget -O HelloWorldPublisher.cpp \
2
    https://raw.githubusercontent.com/eProsima/Fast-RTPS-docs/master/code/Examples/C++/DDSHelloWorld/src/HelloWorldPublisher.cpp

这是发布者应用程序的 C++ 源代码。它将发送 10 个主题为HelloWorldTopic的publications。

1
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
2
//
3
// Licensed under the Apache License, Version 2.0 (the "License");
4
// you may not use this file except in compliance with the License.
5
// You may obtain a copy of the License at
6
//
7
//     http://www.apache.org/licenses/LICENSE-2.0
8
//
9
// Unless required by applicable law or agreed to in writing, software
10
// distributed under the License is distributed on an "AS IS" BASIS,
11
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
// See the License for the specific language governing permissions and
13
// limitations under the License.
14

15
/**
16
 * @file HelloWorldPublisher.cpp
17
 *
18
 */
19

20
#include "HelloWorldPubSubTypes.hpp"
21

22
#include <chrono>
23
#include <thread>
24

25
#include <fastdds/dds/domain/DomainParticipant.hpp>
26
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
27
#include <fastdds/dds/publisher/DataWriter.hpp>
28
#include <fastdds/dds/publisher/DataWriterListener.hpp>
29
#include <fastdds/dds/publisher/Publisher.hpp>
30
#include <fastdds/dds/topic/TypeSupport.hpp>
31

32
using namespace eprosima::fastdds::dds;
33

34
class HelloWorldPublisher
35
{
36
private:
37

38
    HelloWorld hello_;
39

40
    DomainParticipant* participant_;
41

42
    Publisher* publisher_;
43

44
    Topic* topic_;
45

46
    DataWriter* writer_;
47

48
    TypeSupport type_;
49

50
    class PubListener : public DataWriterListener
51
    {
52
    public:
53

54
        PubListener()
55
            : matched_(0)
56
        {
57
        }
58

59
        ~PubListener() override
60
        {
61
        }
62

63
        void on_publication_matched(
64
                DataWriter*,
65
                const PublicationMatchedStatus& info) override
66
        {
67
            if (info.current_count_change == 1)
68
            {
69
                matched_ = info.total_count;
70
                std::cout << "Publisher matched." << std::endl;
71
            }
72
            else if (info.current_count_change == -1)
73
            {
74
                matched_ = info.total_count;
75
                std::cout << "Publisher unmatched." << std::endl;
76
            }
77
            else
78
            {
79
                std::cout << info.current_count_change
80
                        << " is not a valid value for PublicationMatchedStatus current count change." << std::endl;
81
            }
82
        }
83

84
        std::atomic_int matched_;
85

86
    } listener_;
87

88
public:
89

90
    HelloWorldPublisher()
91
        : participant_(nullptr)
92
        , publisher_(nullptr)
93
        , topic_(nullptr)
94
        , writer_(nullptr)
95
        , type_(new HelloWorldPubSubType())
96
    {
97
    }
98

99
    virtual ~HelloWorldPublisher()
100
    {
101
        if (writer_ != nullptr)
102
        {
103
            publisher_->delete_datawriter(writer_);
104
        }
105
        if (publisher_ != nullptr)
106
        {
107
            participant_->delete_publisher(publisher_);
108
        }
109
        if (topic_ != nullptr)
110
        {
111
            participant_->delete_topic(topic_);
112
        }
113
        DomainParticipantFactory::get_instance()->delete_participant(participant_);
114
    }
115

116
    //!Initialize the publisher
117
    bool init()
118
    {
119
        hello_.index(0);
120
        hello_.message("HelloWorld");
121

122
        DomainParticipantQos participantQos;
123
        participantQos.name("Participant_publisher");
124
        participant_ = DomainParticipantFactory::get_instance()->create_participant(0, participantQos);
125

126
        if (participant_ == nullptr)
127
        {
128
            return false;
129
        }
130

131
        // Register the Type
132
        type_.register_type(participant_);
133

134
        // Create the publications Topic
135
        topic_ = participant_->create_topic("HelloWorldTopic", "HelloWorld", TOPIC_QOS_DEFAULT);
136

137
        if (topic_ == nullptr)
138
        {
139
            return false;
140
        }
141

142
        // Create the Publisher
143
        publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT, nullptr);
144

145
        if (publisher_ == nullptr)
146
        {
147
            return false;
148
        }
149

150
        // Create the DataWriter
151
        writer_ = publisher_->create_datawriter(topic_, DATAWRITER_QOS_DEFAULT, &listener_);
152

153
        if (writer_ == nullptr)
154
        {
155
            return false;
156
        }
157
        return true;
158
    }
159

160
    //!Send a publication
161
    bool publish()
162
    {
163
        if (listener_.matched_ > 0)
164
        {
165
            hello_.index(hello_.index() + 1);
166
            writer_->write(&hello_);
167
            return true;
168
        }
169
        return false;
170
    }
171

172
    //!Run the Publisher
173
    void run(
174
            uint32_t samples)
175
    {
176
        uint32_t samples_sent = 0;
177
        while (samples_sent < samples)
178
        {
179
            if (publish())
180
            {
181
                samples_sent++;
182
                std::cout << "Message: " << hello_.message() << " with index: " << hello_.index()
183
                            << " SENT" << std::endl;
184
            }
185
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
186
        }
187
    }
188
};
189

190
int main(
191
        int argc,
192
        char** argv)
193
{
194
    std::cout << "Starting publisher." << std::endl;
195
    uint32_t samples = 10;
196

197
    HelloWorldPublisher* mypub = new HelloWorldPublisher();
198
    if(mypub->init())
199
    {
200
        mypub->run(samples);
201
    }
202

203
    delete mypub;
204
    return 0;
205
}

3.6 CMakeLists.txt#

在您之前创建的 CMakeList.txt 文件的末尾包含以下代码片段。这将添加构建可执行文件所需的所有源文件，并将可执行文件和库链接在一起。

1
add_executable(DDSHelloWorldPublisher src/HelloWorldPublisher.cpp ${DDS_HELLOWORLD_SOURCES_CXX})
2
target_link_libraries(DDSHelloWorldPublisher fastdds fastcdr)

此时，项目已准备好构建、编译和运行发布者应用程序。从工作区的构建目录中，运行以下命令。

1
cmake ..
2
cmake --build .
3
./DDSHelloWorldPublisher

3.7 编写 FastDDS 订阅者#

从工作区的src目录中，执行以下命令下载 HelloWorldSubscriber.cpp 文件。

1
wget -O HelloWorldSubscriber.cpp \
2
    https://raw.githubusercontent.com/eProsima/Fast-RTPS-docs/master/code/Examples/C++/DDSHelloWorld/src/HelloWorldSubscriber.cpp

这是订户应用程序的 C++ 源代码。该应用程序运行订阅者，直到收到主题HelloWorldTopic下的 10 个样本。此时订阅者停止了。

1
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
2
//
3
// Licensed under the Apache License, Version 2.0 (the "License");
4
// you may not use this file except in compliance with the License.
5
// You may obtain a copy of the License at
6
//
7
//     http://www.apache.org/licenses/LICENSE-2.0
8
//
9
// Unless required by applicable law or agreed to in writing, software
10
// distributed under the License is distributed on an "AS IS" BASIS,
11
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
// See the License for the specific language governing permissions and
13
// limitations under the License.
14

15
/**
16
 * @file HelloWorldSubscriber.cpp
17
 *
18
 */
19

20
#include "HelloWorldPubSubTypes.hpp"
21

22
#include <chrono>
23
#include <thread>
24

25
#include <fastdds/dds/domain/DomainParticipant.hpp>
26
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
27
#include <fastdds/dds/subscriber/DataReader.hpp>
28
#include <fastdds/dds/subscriber/DataReaderListener.hpp>
29
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
30
#include <fastdds/dds/subscriber/SampleInfo.hpp>
31
#include <fastdds/dds/subscriber/Subscriber.hpp>
32
#include <fastdds/dds/topic/TypeSupport.hpp>
33

34
using namespace eprosima::fastdds::dds;
35

36
class HelloWorldSubscriber
37
{
38
private:
39

40
    DomainParticipant* participant_;
41

42
    Subscriber* subscriber_;
43

44
    DataReader* reader_;
45

46
    Topic* topic_;
47

48
    TypeSupport type_;
49

50
    class SubListener : public DataReaderListener
51
    {
52
    public:
53

54
        SubListener()
55
            : samples_(0)
56
        {
57
        }
58

59
        ~SubListener() override
60
        {
61
        }
62

63
        void on_subscription_matched(
64
                DataReader*,
65
                const SubscriptionMatchedStatus& info) override
66
        {
67
            if (info.current_count_change == 1)
68
            {
69
                std::cout << "Subscriber matched." << std::endl;
70
            }
71
            else if (info.current_count_change == -1)
72
            {
73
                std::cout << "Subscriber unmatched." << std::endl;
74
            }
75
            else
76
            {
77
                std::cout << info.current_count_change
78
                          << " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
79
            }
80
        }
81

82
        void on_data_available(
83
                DataReader* reader) override
84
        {
85
            SampleInfo info;
86
            if (reader->take_next_sample(&hello_, &info) == eprosima::fastdds::dds::RETCODE_OK)
87
            {
88
                if (info.valid_data)
89
                {
90
                    samples_++;
91
                    std::cout << "Message: " << hello_.message() << " with index: " << hello_.index()
92
                              << " RECEIVED." << std::endl;
93
                }
94
            }
95
        }
96

97
        HelloWorld hello_;
98

99
        std::atomic_int samples_;
100

101
    }
102
    listener_;
103

104
public:
105

106
    HelloWorldSubscriber()
107
        : participant_(nullptr)
108
        , subscriber_(nullptr)
109
        , topic_(nullptr)
110
        , reader_(nullptr)
111
        , type_(new HelloWorldPubSubType())
112
    {
113
    }
114

115
    virtual ~HelloWorldSubscriber()
116
    {
117
        if (reader_ != nullptr)
118
        {
119
            subscriber_->delete_datareader(reader_);
120
        }
121
        if (topic_ != nullptr)
122
        {
123
            participant_->delete_topic(topic_);
124
        }
125
        if (subscriber_ != nullptr)
126
        {
127
            participant_->delete_subscriber(subscriber_);
128
        }
129
        DomainParticipantFactory::get_instance()->delete_participant(participant_);
130
    }
131

132
    //!Initialize the subscriber
133
    bool init()
134
    {
135
        DomainParticipantQos participantQos;
136
        participantQos.name("Participant_subscriber");
137
        participant_ = DomainParticipantFactory::get_instance()->create_participant(0, participantQos);
138

139
        if (participant_ == nullptr)
140
        {
141
            return false;
142
        }
143

144
        // Register the Type
145
        type_.register_type(participant_);
146

147
        // Create the subscriptions Topic
148
        topic_ = participant_->create_topic("HelloWorldTopic", "HelloWorld", TOPIC_QOS_DEFAULT);
149

150
        if (topic_ == nullptr)
151
        {
152
            return false;
153
        }
154

155
        // Create the Subscriber
156
        subscriber_ = participant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT, nullptr);
157

158
        if (subscriber_ == nullptr)
159
        {
160
            return false;
161
        }
162

163
        // Create the DataReader
164
        reader_ = subscriber_->create_datareader(topic_, DATAREADER_QOS_DEFAULT, &listener_);
165

166
        if (reader_ == nullptr)
167
        {
168
            return false;
169
        }
170

171
        return true;
172
    }
173

174
    //!Run the Subscriber
175
    void run(
176
            uint32_t samples)
177
    {
178
        while (listener_.samples_ < samples)
179
        {
180
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
181
        }
182
    }
183

184
};
185

186
int main(
187
        int argc,
188
        char** argv)
189
{
190
    std::cout << "Starting subscriber." << std::endl;
191
    uint32_t samples = 10;
192

193
    HelloWorldSubscriber* mysub = new HelloWorldSubscriber();
194
    if (mysub->init())
195
    {
196
        mysub->run(samples);
197
    }
198

199
    delete mysub;
200
    return 0;
201
}

3.8CmakeLists.txt#

在您之前创建的 CMakeList.txt 文件的末尾包含以下代码片段。这将添加构建可执行文件所需的所有源文件，并将可执行文件和库链接在一起。

1
add_executable(DDSHelloWorldSubscriber src/HelloWorldSubscriber.cpp ${DDS_HELLOWORLD_SOURCES_CXX})
2
target_link_libraries(DDSHelloWorldSubscriber fastdds fastcdr)

此时，项目已准备好构建、编译和运行订阅者应用程序。从工作区的构建目录中，运行以下命令。

1
cmake ..
2
cmake --build .
3
./DDSHelloWorldSubscriber

3.9 将所有内容放在一起#

最后，在构建目录中，从两个终端运行发布者和订阅者应用程序。

至此完成了消息的发布和接收。

<编辑中> 一口气看完路径规划，耗时85642687分钟制作，全程无尿点。友情提示：备好零食，连接WiFi

Mon, 28 Oct 2024 03:40:10 GMT

【本文仅用作笔记和回顾】

一、前言#

以无人驾驶车领域为例介绍全局路径规划和局部路径规划：

（一）全局路径规划：全局路径规划算法属于静态规划算法，根据已有的地图信息（SLAM）为基础进行路径规划，寻找一条从起点到目标点的最优路径。通常全局路径规划的实现包括Dijikstra算法，A * 算法，RRT算法等经典算法，也包括蚁群算法、遗传算法等智能算法；

（二）局部路径规划：局部路径规划属于动态规划算法，是无人驾驶汽车根据自身传感器感知周围环境，规划处一条车辆安全行驶所需的路线，常应用于超车，避障等情景。通常局部路径规划的实现包括动态窗口算法（DWA），人工势场算法，贝塞尔曲线算法等，也有学者提出神经网络等智能算法。

原文链接：https://blog.csdn.net/m0_55205668/article/details/123922046 上图来源于：路径规划算法

二、全局路径规划#

在全局路径规划算法中，大致可分为三类：基于搜索的算法（Dijkstra算法、A* 算法等）、基于采样的算法（RRT、RRT * 等）、智能算法（蚁群、遗传等）。【该分类根据B站IR艾若机器人，下图同】

常用的搜索算法还有D* ，D*Lite，LPA*等，对于 Dijkstra、A* ，D* ，D* Lite，LPA* 这五种算法的比较可以参考 https://zhuanlan.zhihu.com/p/124232080

由于规划岗常见的是Dijkstra 、A* 、D* 、RRT 、RRT* ，本篇将着重写这几个算法，可能还会提及蚁群。

【搜索算法 1 】Dijkstra#

可以直接看： https://blog.csdn.net/qq_44431690/article/details/108175827 ，这篇的过程给的非常详细。 https://zhuanlan.zhihu.com/p/346558578 ，这篇的例子给的很清晰，同时给了C#的实现。 https://oi-wiki.org/graph/shortest-path/ ，这篇给出了时间复杂度和正确性证明，同时给出了优先队列实现。 https://www.luogu.com.cn/article/s581e0wm ，洛谷出品，很明显是在算法上进行优化（堆）。

由于数据结构的老师教我它读迪杰斯特拉算法，所以我就保持了这个习惯。Dijkstra可以说是最经典的最短路径算法，利用贪心作为思路，维护两张表：distance(起点到其他所有点的距离信息)、Top_node（最短路径信息）。

其整体逻辑可以理解为进行 n 次循环，每次循环再遍历 n 个点确定一个还未确定最短距离的点中距离源点最近距离的点，然后再用这个点更新其所能到达的点（算法默认当前的点可以到达所有的点，因为没有到达的点之间的距离都已经初始化为正无穷大，所以不会被更新，不影响结果）（https://blog.csdn.net/qq_41575507/article/details/114548187）

实现伪代码：图源 IR艾若机器人

Dijkstra求最短距离伪代码#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
//图G一般设为全局变量；数组d为源点到达各点的最短路径长度，s为起点
2
Dijkstra(G, d[], s){
3
    初始化
4
    for (循环n次)
5
    {
6
        u = 使d[u]最小的但还未被访问的顶点的标号;
7
        记u已被访问;
8
        for (从u出发能到达的所有顶点v)
9
        {
10
            if(v未被访问 && 以u为中介点使s到顶点v的最短路径d[v]更优){
11
                优化d[v];
12
            }
13
        }
14
    }
15
}

C++ Dijkstra邻接矩阵求最短距离实现#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
#include <iostream>
2
#include <vector>
3
#include <algorithm>
4
using namespace std;
5

6
const int MAXV = 1000;    // 最大顶点数
7
const int INF = 0x3fffffff; // 定义无穷大
8
int G[MAXV][MAXV];        // 图的邻接矩阵表示
9
int d[MAXV];              // 起点到达各点的最短路径
10
bool vis[MAXV] = { false }; // 标记是否被访问
11
int n;                    // 顶点数
12

13
void Dijkstra(int s) {    // s 是起点
14
    fill(d, d + n, INF);  // 将整个数组 d 赋值为 INF
15
    d[s] = 0;             // 起点到自身的距离为 0
16
    for (int i = 0; i < n; ++i) { // 循环 n 次
17
        int u = -1, min = INF;
18
        for (int j = 0; j < n; ++j) { // 找到未访问的顶点中 d[] 最小的
19
            if (!vis[j] && d[j] < min) {
20
                u = j;
21
                min = d[j];
22
            }
23
        }
24

25
        if (u == -1) return; // 找不到小于 INF 的 d[u]，说明剩余顶点不可达
26
        vis[u] = true;       // 标记 u 已被访问
27

28
        for (int v = 0; v < n; ++v) { // 更新 d[v]
29
            if (!vis[v] && G[u][v] != INF && d[u] + G[u][v] < d[v]) {
30
                d[v] = d[u] + G[u][v];
31
            }
32
        }
33
    }
34
}
35

36
int main() {
37
    // 顶点数和边数
38
    n = 5;  // 节点个数
39
    int edges[7][3] = { // 定义图的边 (起点, 终点, 权重)
40
        {0, 1, 10},
41
        {0, 4, 5},
42
        {1, 2, 1},
43
        {1, 4, 2},
44
        {2, 3, 4},
45
        {3, 0, 7},
46
        {4, 2, 9}
47
    };
48

49
    // 初始化图的邻接矩阵
50
    for (int i = 0; i < n; ++i)
51
        fill(G[i], G[i] + n, INF);
52

53
    // 设置边的权重
54
    for (auto& edge : edges) {
55
        int u = edge[0], v = edge[1], w = edge[2];
56
        G[u][v] = w;
57
    }
58

59
    // 执行 Dijkstra 算法，以 0 为起点
60
    Dijkstra(0);
61

62
    // 输出从起点到每个顶点的最短距离
63
    cout << "Shortest distances from vertex 0:" << endl;
64
    for (int i = 0; i < n; ++i) {
65
        if (d[i] == INF) cout << "INF ";
66
        else cout << d[i] << " ";
67
    }
68
    cout << endl;
69

70
    return 0;
71
}

C++ Dijkstra邻接表求最短距离实现#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
#include <iostream>
2
#include <vector>
3
#include <algorithm>
4
using namespace std;
5

6
struct Node {
7
    int v;   // 边的目标顶点
8
    int dis; // 边的权重
9
};
10

11
const int MAXV = 1000;    // 最大顶点数
12
const int INF = 0x3fffffff; // 无穷大表示不可达
13
vector<Node> Adj[MAXV];   // 邻接表表示的图
14
int d[MAXV];              // 起点到各个顶点的最短路径
15
bool vis[MAXV] = { false }; // 标记是否访问过的顶点
16
int n;                    // 顶点数
17

18
void Dijkstra(int s) {    // s 是起点
19
    fill(d, d + n, INF);  // 初始化所有最短路径为不可达
20
    d[s] = 0;             // 起点到自身的距离为 0
21
    for (int i = 0; i < n; ++i) { // 循环 n 次
22
        int u = -1, min = INF;
23
        for (int j = 0; j < n; ++j) { // 找到未访问的顶点中 d[] 最小的
24
            if (!vis[j] && d[j] < min) {
25
                u = j;
26
                min = d[j];
27
            }
28
        }
29

30
        if (u == -1) return; // 找不到小于 INF 的 d[u]，说明剩余顶点不可达
31
        vis[u] = true;       // 标记 u 已访问
32

33
        for (int j = 0; j < Adj[u].size(); ++j) { // 更新 d[v]
34
            int v = Adj[u][j].v;
35
            int weight = Adj[u][j].dis;
36
            if (!vis[v] && d[u] + weight < d[v]) {
37
                d[v] = d[u] + weight;
38
            }
39
        }
40
    }
41
}
42

43
int main() {
44
    // 顶点数和边数
45
    n = 5;  // 顶点数
46
    int edges[7][3] = { // 图的边定义 (起点, 终点, 权重)
47
        {0, 1, 10},
48
        {0, 4, 5},
49
        {1, 2, 1},
50
        {1, 4, 2},
51
        {2, 3, 4},
52
        {3, 0, 7},
53
        {4, 2, 9}
54
    };
55

56
    // 初始化邻接表
57
    for (auto& edge : edges) {
58
        int u = edge[0], v = edge[1], w = edge[2];
59
        Adj[u].push_back({ v, w });
60
    }
61

62
    // 执行 Dijkstra 算法，以 0 为起点
63
    Dijkstra(0);
64

65
    // 输出从起点到每个顶点的最短路径
66
    cout << "Shortest distances from vertex 0:" << endl;
67
    for (int i = 0; i < n; ++i) {
68
        if (d[i] == INF) cout << "INF ";
69
        else cout << d[i] << " ";
70
    }
71
    cout << endl;
72

73
    return 0;
74
}

Dijkstra求最短路径伪代码#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
//图G一般设为全局变量；数组d为源点到达各点的最短路径长度，s为起点
2
Dijkstra(G, d[], s){
3
    初始化
4
    for (循环n次)
5
    {
6
        u = 使d[u]最小的但还未被访问的顶点的标号;
7
        记u已被访问;
8
        for (从u出发能到达的所有顶点v)
9
        {
10
            if(v未被访问 && 以u为中介点使s到顶点v的最短路径d[v]更优){
11
                优化d[v];
12
                令v的前驱为u;
13
            }
14
        }
15
    }
16
}

C++ Dijkstra邻接矩阵求最短路径实现#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
#include <iostream>
2
#include <vector>
3
#include <algorithm>
4
using namespace std;
5

6
const int MAXV = 1000;
7
const int INF = 0x3ffffff;
8
int G[MAXV][MAXV];        // 图的邻接矩阵
9
int pre[MAXV];            // 前驱节点数组
10
int d[MAXV];              // 最短路径数组
11
bool vis[MAXV] = { false }; // 标记是否被访问
12
int n;                    // 顶点数
13

14
void Dijkstra(int s) {    // 求出最短路径上每个结点的前驱
15
    fill(d, d + n, INF);  // 初始化最短路径数组
16
    d[s] = 0;             // 起点到自身的距离为 0
17

18
    for (int i = 0; i < n; ++i) pre[i] = i; // 初始化前驱数组
19

20
    for (int i = 0; i < n; ++i) {
21
        int u = -1, min = INF;
22
        for (int j = 0; j < n; ++j) {
23
            if (!vis[j] && d[j] < min) {
24
                u = j;
25
                min = d[j];
26
            }
27
        }
28

29
        if (u == -1) return; // 找不到小于 INF 的 d[u]，说明剩余顶点不可达
30
        vis[u] = true;       // 标记 u 已被访问
31

32
        for (int v = 0; v < n; ++v) { // 更新 d[v] 和 pre[v]
33
            if (!vis[v] && G[u][v] != INF && d[u] + G[u][v] < d[v]) {
34
                d[v] = d[u] + G[u][v];
35
                pre[v] = u;
36
            }
37
        }
38
    }
39
}
40

41
void DFS(int s, int v) { // 递归打印从起点 s 到当前顶点 v 的路径
42
    if (v == s) {
43
        cout << s << " ";
44
        return;
45
    }
46
    DFS(s, pre[v]);
47
    cout << v << " ";
48
}
49

50
int main() {
51
    // 定义图的边数和顶点数
52
    n = 5;  // 顶点数
53
    int edges[7][3] = { // 图的边定义 (起点, 终点, 权重)
54
        {0, 1, 10},
55
        {0, 4, 5},
56
        {1, 2, 1},
57
        {1, 4, 2},
58
        {2, 3, 4},
59
        {3, 0, 7},
60
        {4, 2, 9}
61
    };
62

63
    // 初始化图的邻接矩阵
64
    for (int i = 0; i < n; ++i)
65
        fill(G[i], G[i] + n, INF);
66
    for (auto& edge : edges) {
67
        int u = edge[0], v = edge[1], w = edge[2];
68
        G[u][v] = w;
69
    }
70

71
    // 以 0 为起点执行 Dijkstra 算法
72
    Dijkstra(0);
73

74
    // 输出从起点到每个顶点的最短路径
75
    cout << "Shortest distances from vertex 0:" << endl;
76
    for (int i = 0; i < n; ++i) {
77
        if (d[i] == INF) cout << "INF ";
78
        else cout << d[i] << " ";
79
    }
80
    cout << endl;
81

82
    // 输出从起点到每个顶点的路径
83
    cout << "Paths from vertex 0:" << endl;
84
    for (int i = 0; i < n; ++i) {
85
        cout << "Path to " << i << ": ";
86
        if (d[i] == INF) cout << "No path" << endl;
87
        else {
88
            DFS(0, i); // 打印从 0 到 i 的路径
89
            cout << endl;
90
        }
91
    }
92

93
    return 0;
94
}

C++ Dijkstra邻接表求最短路径实现#

https://blog.csdn.net/qq_33375598/article/details/104338104

1
#include <iostream>
2
#include <vector>
3
#include <algorithm>
4
using namespace std;
5

6
struct Node {
7
    int v;   // 目标顶点
8
    int dis; // 边的权重
9
};
10

11
const int MAXV = 1000;
12
const int INF = 0x3ffffff;
13
vector<Node> Adj[MAXV];   // 邻接表表示的图
14
int pre[MAXV];            // 前驱节点数组
15
int d[MAXV];              // 最短路径数组
16
bool vis[MAXV] = { false }; // 标记是否被访问
17
int n;                    // 顶点数
18

19
void Dijkstra(int s) {    // 求出最短路径上每个结点的前驱
20
    fill(d, d + n, INF);  // 初始化最短路径数组
21
    d[s] = 0;             // 起点到自身的距离为 0
22

23
    for (int i = 0; i < n; ++i) pre[i] = i; // 初始化前驱数组
24

25
    for (int i = 0; i < n; ++i) {
26
        int u = -1, min = INF;
27
        for (int j = 0; j < n; ++j) { // 找到未访问的顶点中 d[] 最小的
28
            if (!vis[j] && d[j] < min) {
29
                u = j;
30
                min = d[j];
31
            }
32
        }
33

34
        if (u == -1) return; // 找不到小于 INF 的 d[u]，说明剩余顶点不可达
35
        vis[u] = true;       // 标记 u 已被访问
36

37
        for (int j = 0; j < Adj[u].size(); ++j) { // 更新 d[v] 和 pre[v]
38
            int v = Adj[u][j].v;
39
            int weight = Adj[u][j].dis;
40
            if (!vis[v] && d[u] + weight < d[v]) {
41
                d[v] = d[u] + weight;
42
                pre[v] = u;
43
            }
44
        }
45
    }
46
}
47

48
void DFS(int s, int v) { // 递归打印从起点 s 到当前顶点 v 的路径
49
    if (v == s) {
50
        cout << s << " ";
51
        return;
52
    }
53
    DFS(s, pre[v]);
54
    cout << v << " ";
55
}
56

57
int main() {
58
    // 顶点数和边数
59
    n = 5;  // 顶点数
60
    int edges[7][3] = { // 定义图的边 (起点, 终点, 权重)
61
        {0, 1, 10},
62
        {0, 4, 5},
63
        {1, 2, 1},
64
        {1, 4, 2},
65
        {2, 3, 4},
66
        {3, 0, 7},
67
        {4, 2, 9}
68
    };
69

70
    // 初始化邻接表
71
    for (auto& edge : edges) {
72
        int u = edge[0], v = edge[1], w = edge[2];
73
        Adj[u].push_back({ v, w });
74
    }
75

76
    // 执行 Dijkstra 算法，以 0 为起点
77
    Dijkstra(0);
78

79
    // 输出从起点到每个顶点的最短路径
80
    cout << "Shortest distances from vertex 0:" << endl;
81
    for (int i = 0; i < n; ++i) {
82
        if (d[i] == INF) cout << "INF ";
83
        else cout << d[i] << " ";
84
    }
85
    cout << endl;
86

87
    // 输出从起点到每个顶点的路径
88
    cout << "Paths from vertex 0:" << endl;
89
    for (int i = 0; i < n; ++i) {
90
        cout << "Path to " << i << ": ";
91
        if (d[i] == INF) cout << "No path" << endl;
92
        else {
93
            DFS(0, i); // 打印从 0 到 i 的路径
94
            cout << endl;
95
        }
96
    }
97

98
    return 0;
99
}

Dijkstra 优先队列优化#

https://zhuanlan.zhihu.com/p/34624812

1
#include <iostream>
2
#include <vector>
3
#include <queue>
4
#include <cstring>
5
using namespace std;
6

7
struct Edge {
8
    int to;   // 目标顶点
9
    int cost; // 边的权重
10
};
11

12
const int MAXV = 1000;       // 最大顶点数
13
const int INF = 0x3ffffff;  // 无穷大表示不可达
14
vector<Edge> G[MAXV];       // 邻接表表示的图
15
int d[MAXV];                // 最短路径数组
16
int n;                      // 顶点数
17

18
void Dijkstra(int s) {      // 求出从起点 s 到其他顶点的最短路径
19
    fill(d, d + n, INF);    // 初始化距离
20
    d[s] = 0;                // 起点到自身的距离为 0
21
    priority_queue<pair<int, int>, vector<pair<int, int>>, greater<pair<int, int>>> que; // 小顶堆
22
    que.push({ 0, s });        // 将起点推入队列
23

24
    while (!que.empty()) {
25
        auto p = que.top();   // 获取队头元素
26
        que.pop();
27
        int v = p.second;     // 当前顶点编号
28
        if (d[v] < p.first) continue; // 跳过已处理的顶点
29

30
        for (const auto& e : G[v]) { // 遍历邻接边
31
            if (d[e.to] > d[v] + e.cost) { // 松弛操作
32
                d[e.to] = d[v] + e.cost; // 更新距离
33
                que.push({ d[e.to], e.to }); // 将更新后的顶点推入队列
34
            }
35
        }
36
    }
37
}
38

39
int main() {
40
    // 定义图的边和顶点数
41
    n = 5;  // 顶点数
42
    int edges[7][3] = { // 定义图的边 (起点, 终点, 权重)
43
        {0, 1, 10},
44
        {0, 4, 5},
45
        {1, 2, 1},
46
        {1, 4, 2},
47
        {2, 3, 4},
48
        {3, 0, 7},
49
        {4, 2, 9}
50
    };
51

52
    // 初始化邻接表
53
    for (auto& edge : edges) {
54
        int u = edge[0], v = edge[1], w = edge[2];
55
        G[u].push_back({ v, w });
56
    }
57

58
    // 执行 Dijkstra 算法，以 0 为起点
59
    Dijkstra(0);
60

61
    // 输出从起点到每个顶点的最短路径
62
    cout << "Shortest distances from vertex 0:" << endl;
63
    for (int i = 0; i < n; ++i) {
64
        if (d[i] == INF) cout << "INF ";
65
        else cout << d[i] << " ";
66
    }
67
    cout << endl;
68

69
    return 0;
70
}

【搜索算法 2】 A *#

首先，https://paul.pub/a-star-algorithm/ 这篇文章写得很好，我基本都是抄这篇的；其次，https://tashaxing.blog.csdn.net/article/details/47152137 这篇是我一开始学A* 的时候看的，也算是入门文章，当时在vs下编写运行的；后来在学 A* 的改进的时候对其进行了修改；最后，https://github.com/lh9171338/Astar 这篇是我到现在一直在用的，因为我懒得再写一份，它用优先队列进行存储，算是对最基础的A* 进行了改进很多地方都在说A* 引入启发式函数巴拉巴拉什么的，说白了，就是给搜索路线上的每个位置都评价打分一下，然后排个序选个最优点去走，整体理解起来我个人认为要比Dijkstra要简单的多的。

A * 算法实现伪代码#

图源艾若机器人

1
初始化open_set和close_set；
2
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
3
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
4
    * 如果节点n为终点，则：
5
        * 从终点开始逐步追踪parent节点，一直达到起点；
6
        * 返回找到的结果路径，算法结束；
7
    * 如果节点n不是终点，则：
8
        * 将节点n从open_set中删除，并加入close_set中；
9
        * 遍历节点n所有的邻近节点：
10
            * 如果邻近节点m在close_set中，则：
11
                * 跳过，选取下一个邻近节点
12
            * 如果邻近节点m也不在open_set中，则：
13
                * 设置节点m的parent为节点n
14
                * 计算节点m的优先级
15
                * 将节点m加入open_set中

启发函数：#

f(n) = g(n) + h(n)

g(n)表示单次移动耗费，h(n)表示到目标点的预计耗费，f(n)则为综合代价。

g(n)很好理解，就是我从这一格走到下一格需要耗费的代价，在栅格地图中当前位置存在8个邻居节点，移动也只会移动到8个邻居节点上，所以此时的代价就只有两种情况 1 或者 sqrt(2) ，也就对应着直走或者斜走。

h(n)在我看来更好理解，就是节点到目标点耗费的代价，代价的方式有很多种，使用规则如下：

如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
如果图形中允许朝八个方向移动，则可以使用对角距离。
如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

①曼哈顿距离#

计算曼哈顿距离的函数如下，这里的D是指两个相邻节点之间的移动代价，通常是一个固定的常数。

1
function heuristic(node) =
2
    dx = abs(node.x - goal.x)
3
    dy = abs(node.y - goal.y)
4
    return D * (dx + dy)

②对角距离#

计算对角距离的函数如下，这里的D2指的是两个斜着相邻节点之间的移动代价。如果所有节点都正方形，则其值就是sqrt(2) * D

1
function heuristic(node) =
2
    dx = abs(node.x - goal.x)
3
    dy = abs(node.y - goal.y)
4
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

③欧几里得距离#

如果图形中允许朝任意方向移动，则可以使用欧几里得距离。欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的，其函数表示如下：

1
function heuristic(node) =
2
    dx = abs(node.x - goal.x)
3
    dy = abs(node.y - goal.y)
4
    return D * sqrt(dx * dx + dy * dy)

C++实现#

这里贴的代码是本人优化后的，初始的代码是参考 https://tashaxing.blog.csdn.net/article/details/47152137 这篇文章的 Astar.cpp

1
#include <iostream>
2
#include "AStar.h"
3

4
namespace pathplanning {
5
    void Astar::InitAstar(vector<vector<int>>& _Map) {
6
        Map = _Map;
7
        Map_rows = _Map.size();
8
        Map_cols = _Map[0].size();
9
    }
10

11
    int Astar::CalcG(Point* _curpoint, Point* _point) {
12
        int extraG = (abs(_point->x - _curpoint->x) + abs(_point->y - _curpoint->y)) == 1 ? 10 : 14;
13
        int parentG = _point->parent == nullptr ? 0 : _point->parent->G;
14
        return parentG + extraG;
15
    }
16

17
    int Astar::CalcH(Point* _targetpoint, Point* _endpoint) {
18
        int dist = (_endpoint->x - _targetpoint->x) * (_endpoint->x - _targetpoint->x) + (_endpoint->y - _targetpoint->y) * (_endpoint->y - _targetpoint->y);
19
        return round(10 * sqrt(dist));
20
    }
21

22
    int Astar::CalcF(Point* _point) {
23
        return _point->G + _point->H;
24
    }
25

26
    void Astar::PathPlanning(Point _startPoint, Point _targetPoint, vector<pair<int, int>>& path) {
27
        startPoint = _startPoint;
28
        endPoint = _targetPoint;
29

30
        Point* TailPoint = FindPath(isIgnoreCorner);
31
        GetPath(TailPoint, path);
32
    }
33

34
    Point* Astar::isInCloseList(const vector<Point*>& _closelist, const Point* point) const
35
    {
36
        for (auto p : _closelist)
37
            if (p->x == point->x && p->y == point->y)
38
                return p;
39
        return nullptr;
40
    }
41

42
    Point* Astar::isInOpenList(const priority_queue<pair<int, Point*>, vector<pair<int, Point*>>, cmp>& _openlist, const Point* point) const
43
    {
44
        priority_queue<pair<int, Point*>, vector<pair<int, Point*>>, cmp> temp = _openlist;
45

46
        while (!temp.empty()) {
47
            Point* p = temp.top().second;
48
            temp.pop();
49

50
            if (p->x == point->x && p->y == point->y) {
51
                return p;
52
            }
53
        }
54
        return nullptr;
55
    }
56

57
    bool Astar::isCanreach(const Point* _point, const Point* _targetPoint) const
58
    {
59
        if (_targetPoint->x<0 || _targetPoint->x>Map.size() - 1
60
            || _targetPoint->y<0 || _targetPoint->y>Map[0].size() - 1
61
            || Map[_targetPoint->x][_targetPoint->y] == 1
62
            || _targetPoint->x == _point->x && _targetPoint->y == _point->y
63
            || isInCloseList(CloseList, _targetPoint))
64
            return false;
65
        else
66
        {
67
            if (abs(_point->x - _targetPoint->x) + abs(_point->y - _targetPoint->y) == 1) //非斜角可以
68
                return true;
69
            else
70
            {
71
                if (Map[_point->x][_targetPoint->y] == 0 && Map[_targetPoint->x][_point->y] == 0)
72
                    return true;
73
                else
74
                    return isIgnoreCorner;
75
            }
76
        }
77
    }
78

79
    vector<Point*> Astar::getSurroundPoints(const Point* _point) const {
80
        vector<Point*>SurroundPoints;
81

82
#if Neibor5 == 1
83
/* 5邻域*/
84
        int dx = endPoint.x - _point->x;
85
        int dy = endPoint.y - _point->y;
86

87
        if (dx >= 0 && dy >= 0) {
88
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y - 1, _point);
89
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y  , _point);
90
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y + 1, _point);
91
            addSurroundPoint(SurroundPoints, _point->x     , _point->y + 1, _point);
92
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y + 1, _point);
93
        }
94
        else if (dx >= 0 && dy <= 0) {
95
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y + 1, _point);
96
            addSurroundPoint(SurroundPoints, _point->x    , _point->y + 1, _point);
97
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y - 1, _point);
98
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y    , _point);
99
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y + 1, _point);
100
        }
101
        else if (dx <= 0 && dy >= 0) {
102
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y - 1, _point);
103
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y    , _point);
104
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y + 1, _point);
105
            addSurroundPoint(SurroundPoints, _point->x    , _point->y - 1, _point);
106
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y - 1, _point);
107
        }
108
        else if (dx <= 0 && dy <= 0) {
109
            addSurroundPoint(SurroundPoints, _point->x - 1, _point->y - 1, _point);
110
            addSurroundPoint(SurroundPoints, _point->x    , _point->y - 1, _point);
111
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y - 1, _point);
112
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y    , _point);
113
            addSurroundPoint(SurroundPoints, _point->x + 1, _point->y + 1, _point);
114
        }
115

116
#else
117
        for (int x = _point->x - 1; x <= _point->x + 1; x++)
118
            for (int y = _point->y - 1; y <= _point->y + 1; y++)
119
                if (isCanreach(_point, new Point(x, y)))
120
                    SurroundPoints.push_back(new Point(x, y));
121
#endif
122

123
        return SurroundPoints;
124

125
    }
126

127
    // 辅助函数，用于添加可达的邻域点
128
    void Astar::addSurroundPoint(vector<Point*>& SurroundPoints, int x, int y, const Point* _point) const {
129
        if (isCanreach(_point, new Point(x, y))) {
130
            SurroundPoints.push_back(new Point(x, y));
131
        }
132
    }
133

134
    Point* Astar::FindPath(bool isIgnoreCorner) {
135
        // Add startPoint to OpenList
136
        Point* startPointNode = new Point(startPoint.x, startPoint.y);
137
        OpenList.push(pair<int, Point*>(startPointNode->F, startPointNode));
138
        int index = point2index(*startPointNode);
139
        OpenDict[index] = startPointNode;
140

141
        while (!OpenList.empty()) {
142
            // Find the node with least F value
143
            Point* CurPoint = OpenList.top().second;
144
            OpenList.pop();
145
            int index = point2index(*CurPoint);
146
            Point* CurNode = OpenDict[index];
147
            OpenDict.erase(index);
148

149
            // Determine whether arrive the target point
150
            if (CurPoint->x == endPoint.x && CurPoint->y == endPoint.y)
151
            {
152
                return CurNode; // Find a valid path
153
            }
154
            CloseList.push_back(CurPoint);
155

156
            // Find the accessible node around the Current node
157
            auto SurroundPoints = getSurroundPoints(CurPoint);
158

159
            for (auto& targetPoint : SurroundPoints) {
160
                if (!OpenDict.count(point2index(*targetPoint))) {
161
                    //if (!isInOpenList(OpenList, targetPoint)) {
162
                    targetPoint->parent = CurPoint;
163
                    targetPoint->G = CalcG(CurPoint, targetPoint);
164
                    targetPoint->H = CalcH(targetPoint, &endPoint);
165
                    targetPoint->F = CalcF(targetPoint);
166

167
                    OpenList.push(pair<int, Point*>(targetPoint->F, targetPoint));
168
                    int index = point2index(*targetPoint);
169
                    OpenDict[index] = targetPoint;
170
                }
171
                else {
172
                    int TempG = CalcG(CurPoint, targetPoint);
173
                    if (TempG < targetPoint->G) {
174
                        targetPoint->parent = CurPoint;
175
                        targetPoint->G = TempG;
176
                        targetPoint->F = CalcF(targetPoint);
177
                    }
178
                }
179
                /*
180
                Point* resPoint = isInOpenList(OpenList, &endPoint);
181
                if (resPoint)
182
                    return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
183
                */
184
            }
185
        }
186
        return nullptr;
187
    }
188

189
    void Astar::GetPath(Point* TailNode, vector<pair<int, int>>& path) {
190
        PathList.clear();
191
        path.clear();
192
        // Save path to PathList
193
        Point* CurNode = TailNode;
194
        while (CurNode != nullptr)
195
        {
196
            PathList.push_back(CurNode);
197
            CurNode = CurNode->parent;
198
        }
199

200
        // Save path to vector<Point>
201
        int length = PathList.size();
202
        for (int i = 0; i < length; i++)
203
        {
204
            path.push_back(pair<int, int>(PathList.back()->x, PathList.back()->y));
205
            PathList.pop_back();
206
        }
207

208
        // Release memory
209
        while (OpenList.size()) {
210
            OpenList.pop();
211
        }
212
        CloseList.clear();
213
    }
214

215
}

Astar.h

1
//
2
// Created by GuoYulong on 24-8-23.
3
//
4

5
#ifndef ASTAR_H
6
#define ASTAR_H
7

8
#define Neibor5 0
9

10
#include <vector>
11
#include <queue>
12
#include <unordered_map>
13

14
using namespace std;
15

16
constexpr auto isIgnoreCorner = true;;
17

18
namespace pathplanning {
19

20
    struct Point {
21

22
        int x, y;
23
        int F, G, H;
24
        Point* parent;
25

26
        Point(int _x, int _y) :x(_x), y(_y), F(0), G(0), H(0), parent(nullptr) {
27

28
        }
29
    };
30

31
    struct cmp
32
    {
33
        bool operator() (pair<int, Point*> a, pair<int, Point*> b) // Comparison function for priority queue
34
        {
35
            return a.first > b.first; // min heap
36
        }
37
    };
38

39
    class Astar {
40
    public:
41
        Astar() :startPoint(0, 0), endPoint(0, 0),  Map(), Map_cols(0), Map_rows(0), OpenList(), CloseList(), PathList() {
42
        }
43
        void InitAstar(vector<vector<int>>& _Map);
44
        void PathPlanning(Point _startPoint, Point _targetPoint, vector<pair<int, int>>& path);
45
        inline int point2index(Point point) {
46
            return point.y * Map_cols + point.x;
47
        }
48
        inline Point index2point(int index) {
49
            return Point(int(index / Map_cols), index % Map_cols);
50
        }
51

52
    private:
53

54
        Point* FindPath(bool isIgnoreCorner);
55
        void GetPath(Point* TailNode, vector<pair<int, int>>& path);
56
        bool isCanreach(const Point* _point, const Point* _targetPoint) const;
57
        void addSurroundPoint(vector<Point*>& SurroundPoints, int x, int y, const Point* _point) const;
58
        vector<Point*> getSurroundPoints(const Point* _point) const;
59
        Point* isInCloseList(const vector<Point*>& _closelist, const Point* point) const;
60
        Point* isInOpenList(const priority_queue<pair<int, Point*>, vector<pair<int, Point*>>, cmp>& _openlist, const Point* point) const;
61
        int CalcG(Point* _startpoint, Point* _point);
62
        int CalcH(Point* _point, Point* _targetpoint);
63
        int CalcF(Point* _point);
64

65
    private:
66
        Point startPoint, endPoint;
67
        vector<vector<int>> Map;
68
        int Map_cols, Map_rows;
69

70
        priority_queue<pair<int, Point*>, vector<pair<int, Point*>>, cmp> OpenList;
71
        unordered_map<int, Point*> OpenDict;
72
        vector<Point*> CloseList;
73
        vector<Point*> PathList;
74
    };
75
}
76

77
#endif

A * 的优化#

①程序上优化复杂度#

A * 算法的核心计算在于： 1.从open和close表中进行查找 2.从open表中搜索最小栅格 3.从邻居中查找最小栅格

考虑从①②两点进行优化，传统的 A * 使用 vector<Point * > 或者 list<Point* >（Point为自己定义的点的结构体，包括坐标，FGH，父节点）

考虑到查找的复杂度较高，所以思考采用hash表进行查找，其查找的复杂度为O(1)，所以我们采用unordered_map 对 openlist 和 closelist 进行备份，在查找时直接调用查找函数find或者count，由于unordered_map存在键值对应关系，可以设计为点的坐标的序号为key，即 (point.x-1)*map.col+point.y ，这样能确保唯一性；

考虑到排序的复杂度较高，快排也有O(nlog)的复杂度，所以使用priority_queue结构来存储openlist，即先前给出的代码中的 priority_queue<pair<int, Point*>, vector<pair<int, Point*>>, cmp> OpenList 。

注意：事实上，在我的代码中并没有对closelist进行哈希表备份，主要是懒得改了，如果读者有需要的话可以在Astar.h中进行修改，这样就不需要函数InCloseList了。

②优化启发函数 F(N) = G(N)+W(N)*H(N)#

参考文章： A* 算法及优化思路

在应用中就是动态加权，在H(N)较大时W(N)也应较大，当H(N)较小时W(N)也应较小，但这样修改的话，搜索出的路径并不是最优路径，但时间可以大大缩短；W(N)可以根据H(N)自行设定。

③优化邻域#

参考：依据象限搜索及混合预计耗费的A*改进算法，包含8邻域及24邻域的改进根据目标点与当前点的位置关系将邻域缩小一角，由于我的代码是基于8邻域的没测试24邻域，在8邻域下修改为5邻域时间缩短1/3左右（在测试的时候我将地图扩大到1000点左右为了效果更加明显）

④使用双向A*算法#

并不是一个比较稳定的做法，后续如果使用到后再做补充参考：双向A*搜索 | 双向启发式搜索（有几个关键问题做出了解答）

⑤其他优化方法#

参考文章：【路径规划】A*算法方法改进思路简析

【搜索算法 3】 D *#

D* 是Dynamic A* 的简写，其算法和A* 类似，不同的是，其代价的计算在算法运行过程中可能会发生变化。

D * 包含了下面三种增量搜索算法： 1.原始的D* 由Anthony Stentz发表。 2.Focussed D* 由Anthony Stentz发表，是一个增量启发式搜索算法，结合了A* 和原始D* 的思想。 3.D*Lite是由Sven Koenig和Maxim Likhachev基于LPA* 构建的算法。

所有三种搜索算法都解决了相同的基于假设的路径规划问题，包括使用自由空间假设进行规划。在这些环境中，机器人必须导航到未知地形中的给定目标坐标。它假设地形的未知部分（例如：它不包含障碍物），并在这些假设下找到从当前坐标到目标坐标的最短路径。

然后机器人沿着路径行进。当它观察到新的地图信息（例如以前未知的障碍物）时，它会将信息添加到其地图中，并在必要时将新的最短路径从其当前坐标重新添加到给定的目标坐标。它会重复该过程，直到达到目标坐标或确定无法达到目标坐标。在穿越未知地形时，可能经常发现新的障碍，因此重新计划需要很快。增量（启发式）搜索算法通过使用先前问题的经验来加速搜索当前问题，从而加速搜索类似搜索问题的序列。假设目标坐标没有改变，则所有三种搜索算法都比重复的A* 搜索更有效。

D* 及其变体已广泛用于移动机器人和自动车辆导航。当前系统通常基于D*Lite而不是原始D* 或Focussed D* 。

D * 算法实现伪代码#

下述内容（伪代码部分），摘自https://blog.csdn.net/rezrezre/article/details/131008284 推荐去看，写的很不错

1
function Dstar(map, start, end):
2
    # map:m*n的地图，记录了障碍物。map[i][j]是一个state类对象
3
    # start：起点,state类对象
4
    # end: 终点,state类对象
5

6
    open_list = [ ] # 新建一个open list用于引导搜索
7
    insert_to_openlist(end，0， open_list)  # 将终点放入open_list中
8

9
    # 第一次搜索，基于离线先验地图找到从起点到终点的最短路径，注意从终点往起点找
10
    loop until （start.t  == “close”）：
11
        process_state()   # D*算法核心函数之一
12
    end loop
13

14
    # 让机器人一步一步沿着路径走，有可能在走的过程中会发现新障碍物
15

16
    temp_p = start
17
    while (p != end) do
18
        if ( unknown obstacle found) then  # 发现新障碍物, 执行重规划
19
                for new_obstacle in new_obstacles:     # 对每个新障碍物调用modify_cost函数
20
                        modify_cost( new_obstacle )  #（D*算法核心函数之一）
21
                end for
22
                do
23
                         k_min = process_state()
24
                while not ( k_min >= temp_p.h or open_list.isempty() )
25
                continue
26
        end if
27
        temp_p = temp_p.parent
28
     end while

上述伪代码中核心函数为2个：modify_cost 和 process_state 下面是modify_cost的伪代码：

1
function modify_cost( new_obstacle )：
2
        set_cost(any point to new_obstacle ) = 10000000000  # 让“从任何点到障碍点的代价”和“从障碍点到任何点的代价” 均设置为无穷大，程序中使用超级大的数代替（考虑8邻接）
3
        if new_obstacle.t == "close" then
4
                insert(new_obstacle, new_obstacle.h )  # 放到open表中，insert也是d*算法中的重要函数之一
5
        end if

下面是 Process_state函数的伪代码：

1
 function process_state( )：
2
         x = get_min_k_state(oepn_list)  # 拿出openlist中获取k值最小那个state，这点目的跟A*是一样的，都是利用k值引导搜索顺序，但注意这个k值相当于A*算法中的f值（f=g+h, g为实际代价函数值，h为估计代价启发函数值），而且在D*中，不使用h这个启发函数值, 仅使用实际代价值引导搜索，所以其实硬要说，D*更像dijkstra，都是使用实际代价引导搜索而不用启发函数缩减搜索范围，D*这点对于后面发现新障碍物进行重新规划来说是必要的。
3
        if x == Null then return -1
4
        k_old = get_min_k(oepn_list)  # 找到openlist中最小的k值，其实就是上面那个x的k值
5
        open_list.delete(x)  # 将x从open list中移除, 放入close表
6
        x.t = "close"   # 相当于放入close表，只不过这里不显式地维护一个close表
7

8
        # 以下为核心代码：
9
        # 第一组判断
10
        if k_old < x.h then     # 满足这个条件说明x的h值被修改过，认为x处于raise状态
11
                for each_neighbor Y of X:  #考虑8邻接neighbor
12
                        if y.h<k_old   and  x.h> y.h + cost(y,x)  then
13
                                x.parent = y
14
                                x.h = y.h + cost(x,y)
15
                        end if
16
                end for
17
        end if
18
        # 第二组判断
19
        if k_old == x.h then
20
                for each_neighbor Y of X:   #考虑8邻接neighbor
21
                        if y.t== "new" or
22
                           (y.parent == x and y.h !=x.h + cost(x,y) )  or
23
                           (y.parent != x and y.h >x.h + cost(x,y)) then
24
                               y.parent = x
25
                               insert(y, x.h + cost(x,y))
26
                        end if
27
                end for
28
         else:  # 不满足k_old == x.h  那就是k_old < x.h
29
                for each_neighbor Y of X:   #考虑8邻接neighbor
30
                        if y.t == "new" or
31
                           (y.parent == x and y.h !=x.h + cost(x,y) ) then
32
                               y.parent = x
33
                               insert(y, x.h + cost(x,y))
34
                        else:
35
                               if (y.parent != x and y.h >x.h + cost(x,y)) then
36
                                        x.k = x.h  # 注意这行！没有这行会出现特定情况死循环。在查阅大量资料后，在wikipedia的d*算法页面中找到解决办法就是这行代码。网上大部分资料，包括d*原始论文里都是没这句的，不知道为啥
37
                                        insert(x, x.h)
38
                                else:
39
                                        if (y.parent!=x and x.h>y.h+cost(y,x) and y.t= "close" and y.h>k_old then
40
                                                insert(y,y.h)
41
                                        end if
42
                                end if
43
                        end if
44
                end for
45
        end if
46
        return get_min_k(oepn_list)

C++ 实现#

< 待补充 > 由于D* 强调的是在障碍地图改变后防止重新全局规划而对A*的调整，“障碍物改变”这一情况在仿真中不太好搞，所以先不做这部分的处理。

【搜索算法 4】 LPA *#

< 待补充 >

【采样算法 1】 RRT 与 RRT*#

< 待补充 > 相对于上述提到的所有算法，RRT更好理解，整体步骤就是【采样->找最近点连线->判断是否通过障碍物->取点】循环进行这个步骤，直到达到目标点范围内，然后连接最短的路径。可以根据原理就可以看出，RRT并不能得出最优路径，只是能找到一条路径，但是谁让它快啊，很多时候最优并不是必须，快速反应才比较好。不过RRT也有局限，在狭窄道路生成的较慢，毕竟路线中只要有障碍物就不采用这个点的策略确实会导致这个问题。

【采样算法 2】 PRM#

< 待补充 >

局部路径规划#

< 待补充 >

人工势场法#

< 待补充 >

DWA#

< 待补充 >

TEB#

< 待补充 >

<编辑中>N10 镭射激光雷达

Mon, 14 Oct 2024 11:58:44 GMT

最近入手了一个激光雷达，轮趣家买的，估计是买贵了，下阶段闲的时候可能会玩一玩，就在这篇记录一下

上位机 LSLIDAR_Single_Scan_Lidars （需要先安装 WinPcap）

<项目未完成>DWA 两轮差速扫地机器人覆盖仿真

Mon, 30 Sep 2024 06:22:41 GMT

目的：丢弃move_base，使用自己的DWA，本着不造轮子的想法（我也造不出轮子），大部分源码用大佬们的开源，更关注修改整合后的效果

研究路径：

1.基于之前的Astar和Bspline得到全局路径
2.使用turtlebot3两轮全向小车模型
3.基于clean_turtle3_robot的next_goal发布逻辑
4.基于dwa_planner项目的dwa算法
5.基于ipa项目的形态学分割和牛耕覆盖算法
6.牛耕覆盖路径优化（分段平滑处理）
7.整合所有

前情回顾：先前完成了上述(1-3)+move_base的仿真，也完成了5的修改，目前想法是将move_base丢弃采用单独的dwa，也是为了之后在实机上使用的时候更加方便，更不依赖ros（最后的话估计要丢掉ros，裁剪出消息机制）

代码仓库（9.30开始更新）： https://gitee.com/upcgyl/dwa_-clean_-robot.git

9.30 更新当前问题：直接将dwa丢入工程内后，出现一些不适配项目的问题，后续将从下述现象修改：

1.机器人起点方向不确定导致第一个目标点DWA规划的路径较差，拟修改初始机器人目标点为当前点且方向为当前点到第一个目标点的方向
2.该算法逻辑为到达目标点前减速，由于已经有全局路径，所以尽量保证在整个过程中匀速运动，拟修改calc_cmd_vel函数中后半部分
3.到达目标点时出现原地打转现象，后续查找原因
4.next_goal和dwa中均存在到达目标点判断，后续思考是否加入状态机制避免重复判断

10.11 更新代码仓库修改：将next_goal分离为单独的功能包，新增分区和覆盖的依赖代码

对于9.30的4个问题先不做考虑，将next_goal的到达距离减小后可以在视觉上稍微优化2现象，先考虑分割路径如何发布目标点，由于bcpp中没有设计每个区域之间连接的方式，所以第一步需要将各区域之间通过planner连接然后将所有点作为goal进行发布先看一下效果，存在几个明显的问题

1.分区后的单独区域覆盖的路径存在穿墙现象导致机器人撞墙dwa死机
2.转弯效果不好，覆盖率也不够（先不考虑外围代价地图区域）
3.机器人初始位置不是（0,0）坐标

思考，是否可以从这几个方向进行修改： 1.减小分区的最大面积阈值，使分得的区域更加的小 2.对房间进行膨胀 3.联通距离小于机器人的障碍物

10.14更新解决问题3机器人初始坐标设置问题在代码中，我认为地图的中心点为机器人的初始点，于是将start_point设置为msg.info.origin.position.x + msg.info.width * 0.5 而在amcl和gazebo中我的initial_pose 为(0,0)，这是不正确的，事实上，我如果想让机器以房间的中心为初始点的话，那我的initial_pose应该设置为：(msg.info.origin.position.x + msg.info.width 0.5) map_resolution + msg.info.origin.position.x，例如我的地图大小是384X384，分辨率是0.05，地图起始坐标为(-10,-10)，那么我的中点坐标应该为（-10+384/2）x0.05 - 10 也就是（-0.9，-0.9）

对于问题1 bcpp穿墙现象尝试将每两个点都用A * 去规划，效果很差，同时会增加没用的距离消耗，该方案取消

git分支T10_14：修改bcpp传入参数后效果：当前留存的大问题： 1.运动时的路径与实际规划存在位置偏差； 2.转角点较少且没有进行路径优化 3.初始角度问题 4.在全房间的时候依旧会出现穿墙的效果

效果解释：房间边界留出的空间用来后续补充沿边规划的算法，因为扫地机器人当前的算法一般都是先沿边后覆盖

10.15更新

/ 该部分修改实际测试无用，可以直接跳过看下一部分 / 删掉next_goal中的path_callback中的自己修改过的一些东西后，初始不会出现转角效果较差的问题了，会根据目标点先转方向后移动更新后的path_callback:

1
void path_callback(const nav_msgs::Path &path)
2
{
3
  ROS_INFO("path_callback Success!");
4
  if ((planned_path.Path.size() == 0) || (path.poses.size() != taille_last_path))
5
  {
6
    planned_path.Path.clear();
7
    new_path = true;
8

9
    /*    // 使用路径消息的第一个点作为目标方向
10
       if (path.poses.size() > 1) // 确保有至少两个点
11
       {
12
         float initial_angle = atan2(path.poses[1].pose.position.y - path.poses[0].pose.position.y,
13
                                     path.poses[1].pose.position.x - path.poses[0].pose.position.x);
14
         ROS_INFO("y1=%f, x1=%f, y0=%f, x0=%f", path.poses[1].pose.position.y, path.poses[1].pose.position.x,
15
                  path.poses[0].pose.position.y, path.poses[0].pose.position.x);
16
         ROS_INFO("initial_angle=%f", initial_angle);
17

18
         goal_msgs.header.frame_id = "odom";
19
         goal_msgs.header.stamp = ros::Time::now();
20
         goal_msgs.pose.position.x = path.poses[0].pose.position.x;
21
         goal_msgs.pose.position.y = path.poses[0].pose.position.y;
22
         goal_msgs.pose.position.z = 0;
23
         goal_msgs.pose.orientation = tf::createQuaternionMsgFromYaw(initial_angle);
24

25
         // 发布初始目标方向
26
         GoalPoint_pub.publish(goal_msgs);
27
       } */
28

29
    for (int i = 0; i < path.poses.size(); i++)
30
    {
31
      planned_path.addGoal(path.poses[i].pose.position.x, path.poses[i].pose.position.y, false);
32
    }
33
    taille_last_path = path.poses.size();
34
  }
35
}

修改min_cell_width计算方式为const int min_cell_width = half_grid_spacing_as_int;，使min_cell_width变小，更多的区域不会被合并，cell合并更依赖于设定的面积大小，修改后路径：

单从该测试用例看效果已经优于之前的情况了

增加单个区域cell之间的连接，减少穿墙效果：主要修改computeBoustrophedonPath函数

10.15修改后的代码放到了T10_15分支上了，整体是跑不起来的，dwa部分还有问题，下班

10.18更新把pathplan.cpp重构，修改为coverage_planner.cpp更加整洁，代码已推送至T10_18分支

目前牛耕覆盖还存在问题： 1.房间最开始的分割方向固定为主要方向，导致在测试地图的最中间的最大的房子分割效果不好，后续增加家具等后可能分割效果会好一些；

10.19 对于单个房间进行覆盖时算法还得改，考虑看如何将单个房间再划分为几个小区域，主要是一些不规则的墙造成的，类似于上图中的右下角的房间和中间房间的区别；造成这个现象的原因是牛耕的逻辑：牛耕进行区分cell是通过寻找突出的黑色像素，然后将这一行作为一个分割，这样就造成了中间房间的中间区域较长（实际上这个并没有问题），但是由于中间房间的左上和左下区域不满足要求，所以被合并到了中间区域。事实上，我觉得应该先将房间分为左右两个cell然后再进行横向的分割

10.19更新增加两目标点之间的距离检测，超过0.2的话就补路径，这样的话不会穿墙，但是实际覆盖效率低

毁灭吧不知道这个是啥自己看吧

Fri, 27 Sep 2024 08:53:06 GMT

想学ros仿真来着，就把turtlebot3和几个项目裁剪了一下，在之前的A * 、Bspline基础上搞了一下，其实一开始是想搞cleanRobot内个项目的，大致框架也是跟那个项目一样，不过我还没把BCPP加进去，主要是BCPP还没看完，因为东西太多了很多地方懒得看，拖了一两个周了，现在先把这部分草草结束吧。

整个工程很多地方都有bug，但是已经懒得改了，之后加BCPP的时候估计会修一下，而且后续计划是把Move_base的DWA替换为自己的包，估计到时候是个大工程

先上效果：在rviz选定target后，先规划出一条路径，然后机器人跟着过去，下面有个视频可以看看 2024-09-27 16-06-01

代码端基本就用到这么几个： src │ ├── Astar.cpp │ ├── Bspline.cpp │ ├── ~~main_Bspline.cpp~~ │ ├── ~~minimum_control.cpp~~ │ ├── next_goal.cpp │ ├── OccMapTransform.cpp │ └── pathplanner.cpp

用法和以前一样

1
catkin_make
2

3
source devel/setup.bash
4

5
roslaunch path_plan clean_work.launch

在rviz中选择一个2D Nav Goal就好

代码放gitee上了 https://gitee.com/upcgyl/my-clean-robot.git 自取

<持续更新>差速小车硬件准备

Mon, 23 Sep 2024 03:05:07 GMT

注意！本人非专业硬件工程师，我是个臭软件人，硬件的知识也只是没事自己玩玩积累的，所以本文看看就行，当个乐子，别太认真去学……毕竟我也不知道说的对不对。

AS4950驱动方案#

由于之前给差速小车配的是BTN7971驱动，也是属于英飞凌的高性能芯片了，但是缺点比较明显，贵。在立创上单个芯片售价40元左右，淘宝购买的话价格较高且不知道是不是正版，于是想着进行替换。

最先考虑的是DRV8870，因为对这个驱动比较熟悉，但是偶然发现AS4950，国产价格合适，和DRV8870好像是Pin To Pin，方便更改，主要是SOP8封装比BTN7971小很多，在整体设计上也方便

下图为芯片的管脚定义

由于数据手册中没有给出应用电路，所以我们从网上找一下

………………

ok找到了，该驱动电路中采用LM317作为稳压芯片，封装为SOT-223，提供5V的参考电压

2024/9/27 修改

TMI8870 驱动方案#

因为手头有几块TMI8870，就先这样用着，不过听某工程师说这个驱动有点问题，也不知道是哪里的问题

陀螺仪的话依旧采用mpu6050吧，毕竟模块卖的比较多，哎，公司之前为了降本都用三轴的，手头一堆三轴的乱七八糟的加速度计没处用，等之后回归老本行做个适老的跌落腰带或者跌落手环玩玩。

整个工程的话等抽时间做完了再开源

2024/11/2 增加

电源方案#

补充一下电源部分，其实板子早就做完了，但是一直懒得焊，等之后测试之后再补充全部的，由于第一版主要测试功能，所以stm32用的也是核心板，mpu6500也是模块，等功能测试好后，下一版本就把这两个模块替换掉，直接整合上去。

5V稳压采用常规的LM2596，原理图也是直接用手册上给的参考电路，因为之前验证过没问题，就这样用了，就是电感33uH，大部分封装比较大有点占位置。至于怎么pcb怎么连线，看这篇https://gitcode.csdn.net/65e7d0b01a836825ed789075.html 就行。

大家不要学我，事实上我建议3.3V的1117芯片的输入的5V和外设的5V区分开，但我这里其实没什么外设……就这样用了，反正最好是芯片单独供电就好。（其实我3.3V并没用到，因为核心板上有一个稳压，所以我直接给5V，3.3留出来调试用）

下一阶段应该是增加一个usb口去和rk3568的板子通信，然后加个烧录口进行烧录，有一说一没做过stm32的烧录，如果有人有方案的话可以在评论区留个言。

2024.11.15增加

驱动板V1.0做完了，目前上电正常，理论上线束是没问题的，之前拿杜邦线测得，等之后测试一下是不是适配之前的代码。（大家看个乐子就行，我不是硬件工程师）

软考嵌入式2019年下午真题

Fri, 20 Sep 2024 07:05:31 GMT

第 1 题#

下面是关于C语言的基本概念和编程方面的叙述，回答问题1至问题3，将答案填入答题纸的对应栏内。【说明】王工在红外热像仪系统设计中负责软件模块，其核心功能在于进行人脸检测，调取温度检测模块获取检测到人的体温，将检测到的人脸信息及体温通过网络发送给平台进行存储，软件采用C语言编写。

【问题1】(3分)#

请说明在C语言中下列宏定义的含义。 (1) #define f(a,b) a##b (2) LINE (3) DATE_

【问题1】解答（1）连接两个宏参数为一个数（2）LINE 当前程序行的行号，表示为十进制整型常量（3）DATE 当前日期，一个以 “MMM DD YYYY” 格式表示的字符串常量。

【问题2】 (3分)#

王工在利用网络发送所检测到的人脸及对应的体温信息时定义了对应的数据结构FaceTemp_info，内容依次包括<检测到人脸框的左上角像素坐标>(横向坐标Face_x，纵向坐标Face_Y)，人脸框的像素大小(横向宽度Face_w，纵向高度Face_H)，以及人体温度信息Temp。假设人脸框坐标信息、人脸框的像素大小都用unsigned short来表示，人体温度信息用float来表示。请给出FaceTemp_info 数据结构的定义。

【问题2】解答 typedef struct FaceTemp_Struct{ unsigned short Face_x; unsigned short Face_Y; unsigned short Face_w; unsigned short Face_H; float Temp; }FaceTemp_info;

【问题3】(9分)#

在红外热像仪的图像信息处理中，王工设计了如下人脸检测、温度检测及信息发送程序，当检测到温度大于等于37.3C时，需要报警提示。请补全程序中的(1) ~ (6)。

(1) temp>=37.3 (2) facetemp_info.Face_x = face_x (3) facetemp_info.Face_y = face_y (4) facetemp_info.Face_w = face_w (5) facetemp_info.Face_H = face_h (6) &facetemp_info

第一题的15分至少可以拿下12分

第 2 题#

阅读下列说明和图，回答问题1至问题3，将解答填入答题纸的对应栏内。【说明】某温度采集系统以处理器为核心，采用PC总线连接嵌入式键盘和多个温度传感器，温度传感器负责采集温度，键盘可以输入字符和数字信息，并通过RS232总线连接上位机。 I²C (Inter Integrated Circuit)总线是一种2线式串行总线，用于连接控制器及其外围设备。I²C总线最主要的优点是其简单性和有效性。它支持多主控(multimastering)，其中任何能够进行发送和接收的设备都可以成为主总线。一个主控能够控制信号的传输和时钟频率。但在任何时间点上只能有一个主控。 PC的这些特点使其在许多设计中备受青睐，本系统中采用I²C总线连接嵌入式键盘和多个温度传感器，如图4-1所示。

【问题1】(4分)#

I²C总线上传送的信号是广义的，既包括地址信号，又包括数据信号。在起始信号后必须传送一个从机的地址、数据的传送方向。每次数据传送总是由主机产生的终止信号结束。但是，若主机希望继续占用总线进行新的数据传送，则可以不产生终止信号，马上再次发出起始信号对另一从机进行寻址。 I²C总线协议规定：寻址字节由从机地址(D7~D1位)、数据传送方向位(D0位：“0”表示主机向从机写数据，“1”表示从从机读取数据)组成，如图4-2所示。主机发送地址时，总线上的每个从机都将这7位地址码与自己的地址进行比较，如果相同，则认为自己正被主机寻址，根据D0位将自己确定为发送器或接收器。

图4-2寻址字节位定义

该系统中用 I²C 总线连接了多个同样的温度传感器，用于实时采集温度值。该传感器的从机地址由固定位和可编程位组成，7位地址位前4位(即D7 ~ D4)是固定位，后3位(即D3~D1)是可编程位。该系统中最多可连接（1）个本型号的温度传感器。该温度传感器从机地址中固定位是0xA，传感器1的可编程位是0、传感器2的可编程位是…以此.类推。当主机读取传感器4的数据时，总线命令序列中的寻址字节值是（2）。

【问题1】解答（1） 8 （2） 0xA7

【问题2】 (6分)#

本系统通过1路RS232总线将采集的温度值传给上位机进行处理。RS232采用异步通信方式传送ASCII码，数据位8位，奇校验1位，停止位1位。当RS232的波特率设置为4800时，字符传送的速率是（1）字符1秒，每个数据位的时间长度是（2）毫秒，数据位的传送速率是（3）位/秒。

【问题2】解答 (1) 480 (2) 0.208 (3) 3840

【问题3】(5分)#

同时，本系统通过 I²C 总线连接了嵌入式键盘，用于人机交互。频繁按键会导致连续使用 I²C 总线读取数据，由于 I²C 总线操作是一个应答式的命令序列，所以防止2次读写之间的干扰(即在一次读写没有完成之前，另一次读写操作也占用 I²C 总线，两次的数据会造成紊乱)是一个重要的问题。 (1)为防止多次频繁按键，导致总线访问错误，需要采取什么机制，保证每次只允许一个读写操作占用总线? (2)在执行读写操作命令序列时，如何使用该机制?

【问题3】解答（1）互斥机制（2）读写操作前等待互斥锁互斥，读写操作后释放互斥锁

本题15分，必要拿到的分数为 15分，可能由表述原因最后两三分拿不到，所以应该是12-15分截至目前，分数范围：24-30；

第 3 题#

阅读下列说明，回答问题1至问题3，将解答填入答题纸的对应栏内。【说明】人体的红外辐射特性与它的表面温度有着十分密切的关系，门式测温仪通过对人体自身辐射红外能量的测量，便能准确地测定人体表面温度。由于门式测温仪最大的优点是非接触、快速测量，一般小于1秒钟，所以被广泛应用于机场、车站、码头、学校等人流量大的地方。该设备中软件主要完成A/D数据采集，并通过数学运算将红外数据转换为温度值,自动将额头表面温度结果换算为人体腋下温度，在显示终端显示。软件主要包括主程序模块、红外测温程序模块、温度转换模块、显示程序模块、报警程序模块、环境温度补偿模块等。程序的主要功能说明如下： (1)如果按下“高温报警设置”键，则通过“+”键每次增加0.1”C或通过“-”键每次减少0.1”C，设置最高温度报警值(默认值为40C)，按下“确认”键进行确认； (2)如果按下“低温报警设置”键，则通过“+”键每次增加0.1”C或通过“-”键每次减少0.1”C，设置最低温度报警值(默认值为35”C)，按下“确认”键进行确认； (3)主程序模块判断是否在30厘米范围内存在红外能量，如果没有，继续执行(3)；否则执行(4)； (4)读取红外测温程序模块送回的目标表面温度信息，并调用温度转换模块和环境温度补偿模块进行温度转换和补偿，并形成最终人体温度； (5)将读取的温度信息与设定的最大值与最小值比较，如果大于等于最大值或小于等于最小值，则执行(6)；否则向显示程序模块发送显示的温度值，并执行(7)； (6)调用报警处理程序，发出蜂鸣声提醒检测人员，并向显示程序模块发送报警信息； (7)显示程序模块显示高温报警或低温报警或者温度信息。

【问题1】(7 分)#

为了测试软件功能，测试人员设计了表3-1所示的测试用例，请填写该表中的空白(1)~(7)。

【问题1】解答：（1） 37.5℃ （2） 35.5℃ （3）被测目标物体温度为37.4℃ （4）高温报警信息（5）大于37.5℃ （6） 35.6℃ （7）低温报警信息

【问题2】(6分)#

软件的结构覆盖率是度量测试充分性和有效性的一种手段。在嵌入式软件白盒测试过程中，通常以语句覆盖率、分支覆盖率和MC/DC覆盖率作为度量指标。采用以下处理过程实现“判断温度是否在正常范围”的功能：

if ((目标物体温度<高温报警值)&& (目标物体温度>低温报警值) ) 显示目标物体温度； else { if (目标物体温度≥高温报警值) { 显示高温报警信息； } else { 显示低温报警信息； } 发出蜂鸣报警声音； }

针对上述处理过程： (1)若要求达到100%的语句覆盖，则所需的最少用例数是多少? (2)若要求达到100%的分支覆盖，则所需的最少用例数是多少? (3)若要求达到100%的MC/DC覆盖，则所需的最少用例数是多少?

【问题2】解答： 3 3 3 （第三个MC/DC覆盖不知道是啥，后续查一下，3是蒙的）

【问题3】 (2 分)#

常见的黑盒测试的测试用例设计方法包括<①等价类划分>；②边界值分析；③因果图； ④决策表等。测试人员在表3-1中针对本题设计测试用例时，使用了其中的哪种方法?

【问题3】解答： ②边界值分析

这道题拿15分一般没啥问题，如果因为MC/DC是啥蒙错了扣2分的话，拿个13分也可以

截至目前理论应得分数为：37-45分

第 4 题#

阅读下列说明和图，回答问题1至问题4，将解答填入答题纸的对应栏内。【说明】电子体温计可以快速、准确地进行体温测量，第一时间排查发热病人。李工通过AD模块设计体温采集电路，如图2-1所示，AD采集芯片使用ADC0809，单片机采用AT89S52。

对应的电路连接描述如下： (1) ADC0809 的D0~ D7端口连接到AT89S52的P0口; (2) ADC0809的A、B、C为AD输入IN0 ~ IN7选择端，A为最高位，C为最低位; (3) ADC0809 的AD转换结束信号(EOC) 连接到AT89S52的P3.2; (4) ADC0809 的AD转换启动信号(ST) 受AT89S52的P1.4管脚控制; (5) ADC0809 的AD转换数据输出使能(OE)受AT89S52的P3.1管脚控制; (6) ADC0809的时钟通过AT89S52的ALE输出控制，经过2个D触发器来生成; (7) ADC0809 的输入模拟电压范围是0 ~ 5V; (8) AT89S52 的P3.0管脚用来控制LED。对于ADC0809而言，启动一次AD转换时，需要给ST一个短时的高电平。在AD转换过程中，EOC为低电平，当AD转换完毕后，EOC会变为高电平。在AD转换结束后，需要给OE一个短时高电平，在该期间内从D0 ~ D7读取AD转换后的数据。

【问题1】(3分)#

AT89S52和ADC0809之间的连接线中,对ADC0809而言，下列信号中哪些为输入信号?

A. ST B. EOC C. OE D. A E. B F. C

【问题1】解答： ACDEF

【问题2】(2 分)#

当外围的温度传感器模拟电压为1.2V 时，ADC0809 的输出值是多少?

【问题2】解答： 1.2/5*255 = 61.2 = 0X3D

【问题3】(2分)#

若单片机AT89S52的ALE输出为2M的时钟频率，那么ADC0809的时钟是多少?

【问题3】解答： 2 个D 触发器串联，进行了4分频，因此输出时钟为输入时钟的1/4 分频，也就是D 触发器输出最终为2M/4=500KHZ 。

【问题4】 (8分)#

李工按照该电路的工作原理设计了如下程序，该程序以查询方式实现AD转换，读取 AD转换后的数据。请根据逻辑，在下面A到O中选择正确答案，完成(1) ~ (8) 填空。

A. LED= 0 B. LED=1 C. OE=0 D. OE=1 E. ST=0 F. ST=1 G. P0=0xF8 H. PO=0xFC

I. while (EOC==0) J. while (EOC==1) K.ADdata=P0 L. P0=ADdata M. if (ADdata> 128) N. if (ADdata < 128) O. LED=1

【问题4】解答： (1) E ST=0 (2) F ST=1 (3) J while (EOC==1) (4) C OE=0 (5) k ADdata=P0 (6) D OE=1 (7) M if (ADdata> 128) (8) A LED= 0

这道题拿个11分不过分 11-15 截至目前，可以获得48-60分，基本可以及格了

第5题#

阅读下列说明和图，回答问题1至问题3，将解答填入答题纸的对应栏内。【说明】 2019年12月以来，新型冠状病毒首先在国内蔓延，现在又肆虐全球，我国政府全力以赴，抗击疫情，全国人民积极响应，短短几个月来，疫情得到很好的控制。在抗疫战斗中，相关医疗设备起到了至关重要的作用。某公司生产的红外热成像系统(测温仪)已被广泛使用，可以检测发热症状，排查受感人群。该红外热成像系统如图1-1 所示，主要由红外光学系统、红外探测器、驱动采集电路、图像处理电路、显示与存储电路组成。

图1-1红外热成像系统图在该系统中，光学系统主要指专门红外镜头，与普通光学镜头不同，光学材料针对相应波段的红外光线，实现滤光和对焦。红外探测器是完成红外热辐射到电信号的转换。驱动采集电路完成图像信号的输出与采集。红外图像处理电路在系统中不仅要对红外图像进行非均匀校正、盲元补偿处理，还要对图像的温度漂移、图像增强以及温度测量信息进行处理。最后，红外图像需要设置不同的输出方式以匹配不同的显示器，或者采用文件系统将红外图像及信息进行存储。红外热成像系统具体的系统架构如图1-2 所示，主处理器采用ARM，红外探测器采用320X240非制冷长波红外探测器，系统电路包括偏置电压电路、温控电路、A/D转换电路、基于FPGA的实时红外图像信号采集电路、基于ARM的系统控制电路以及LCD显示模块电路。实现了红外探测器的驱动、红外图像信号的采集和处理、红外图像的输出显示以及一些外设辅助功能。图1-2 红外热成像系统的系统框架图

【问题1】（4分）#

系统采用4片IS61LV51216 SRAM来做数据缓存，请问： (1)下列关于SRAM、DRAM和ROM的论述中，哪些是正确的? A.SRAM的中文名称为“静态随机存储器” B.存放在SRAM中的信息不会因去掉供电电源而丢失 C.SRAM的静态存储单元是在静态触发器的基础.上附加门控管而构成的 D. SRAM必须定时不断刷新，以保证所存储的信息不会丢失 E. SRAM将每个位存储在一一个双稳态存储器单元 F. SRAM的功耗较大，集成度不能做得很高 G. ROM所存数据稳定，断电后所存数据也不会改变 H. SRAM 需要刷新电路才保存它内部存储的数据 I. SRAM的优点是集成度较低，功耗较DRAM小 J. DRAM每隔一段时间要刷新充电一次，否则内部的数据即会消失

(2) SRAM的容量为512KX16位，那么SRAM的地址线和数据线数目分别为多少?

【问题1】解答：（1）A、C、E、F、G、J （2）地址线19根，数据线16根

【问题2】（7分）#

在红外热像仪系统中，FPGA完成红外图像的采集与预处理，包括前端探测器驱动、数<据采集与整形>、数据预处理、SRAM控制器、ARM通信等，在整个数据采集过程中，FPGA将接收到的红外图像数据采用“乒乓”结构交替不断写入两块SRAM中，FPGA 在进行数据写入时，ARM不能读取相应区的数据，FPGA 每写完- -帧数据，就通过寄存器通知ARM取数据，新的一帧数据写入另-块SRAM。这样就避免了红外探测器图像采集时钟与ARM的图像处理帧频时钟的不匹配问题。 (1)下列关于FPGA的描述中，哪些是正确的? A. FPGA由逻辑单元、RAM、乘法器等硬件资源组成 B. FPGA只能实现有限次的重新编程 C. FPGA可通过使用原理图或者Verilog HDL来设计 D. FPGA的所有功能均依靠硬件实现 E. FPGA可以实现定点和浮点运算 F. FPGA的工作频率由FPGA芯片及设计决定 (2)完成图像乒乓操作流程图1-3中的空①~⑤。图1-3图像数据 FPGA乒乓操作流程图

【问题2】解答（1）A、C、D、F （2） ①清SARM可读状态 ②置SARM写状态 ③写满? (是否写满) ④置SRAM可读 ⑤有无空闲SRAM? (是否有空闲SRAM)

【问题3】（4分）#

假设该红外热像仪CPU的中断系统有4个中断源A、B、C、D，其硬件排队优先次序为A>B>C>D，每个中断源对应-一个屏蔽码，表1-1为中断服务程序与屏蔽码的关系，其中“0”为允许，“1”为屏蔽，CPU 状态时屏蔽码为0000。 (1)设A、B、C、D同时请求中断，试画出CPU执行程序的轨迹。 (2)请给出中断服务程序完成的顺序。

【问题3】解答：（1）

（2）C、A、D、B

之前刷到本篇的可能发现没有最后一题，确实是本人之前跟着做的时候做到最后一题忘记写上去了，现在发现了补充一下，嵌入式系统设计师确实比较难考，我之前考了一次，下半的没过，也是因为没复习去裸考了，只考了30多分。等明年春天好好复习了再考吧。

软考嵌入式2020年下午真题

Fri, 20 Sep 2024 02:11:38 GMT

编辑中#

题目来源于：2019年下半年嵌入式系统设计师考考试下午真题（专业解析+参考答案）

第 1 题阅读下列说明和图，回答问题1至问题3，将解答填入答题纸的对应栏内。#

【说明】服务型智能扫地机器人因其低廉的价格和高效的工作能力，越来越受到消费者的认可，目前已逐渐进入家庭生活代替人们的清洁工作，具有广阔的市场。服务型智能扫地机器人需要具有自主运动规划和导航功能，在其工作过程中，需要通过对环境信息的融合感知进行行为决策。扫地机器人一般具备的主要功能包括：（1）紧急状态感知：包括碰撞检测、跌落检测和离地检测等功能，防止与障碍物碰撞、前方台阶跌落危险以及扫地机器人离地等，实现扫地机器人运动中的自我保护。（2）姿态感知：包括运动里程计数和航向测量等功能，需要获取扫地机器人的运动速度、行走距离、航向角度等信息。（3）视觉感知：包括单目视觉避障系统和单目视觉定位系统等，需要通过视觉信息探测障碍物，视觉信息来自两个单目摄像头系统。在某些设计中，也可结合红外测距传感器进行障碍物探测。（4）自动充电：在工作过程中，需要实时监控扫地机器人的电量，且在电量少于一定阈值时自动返回电源处进行充电。（5）扫地及吸尘单元：使用电机控制刷子实现清扫，使用抽灰电机实现吸尘。（6）运动执行：对机器人的运动进行控制。（7）监控系统：通过无线网络传递扫地机器人的状态数据及视频图像等信息到远程客户端，客户端参与到扫地机器人的运动监视及控制中，实现信息交互，监控扫地机器人的实时状态。客户端包括PC客户端和手机客户端两种。（8）信息处理中心：用于接收各种传感器信息和视觉信息，通过分析处理进行扫地机器人的运动控制，且负责和后台监控中心通信。服务型智能扫地机器人选用ARM+STM32双核架构模式，分别处理数据量较大的图像信息和短促型的非图像信息。STM32选用STM32F103VET6芯片，用于实现非图像以外的众多传感器的驱动以及数据采集，并控制车轮电机的运动；ARM选用S5PV210处理器实现摄像头图片的采集、在监控系统中接入无线网络、对STM32串口传过来的传感器数据以及图像定位和避障信息做综合处理，生成运动决策，发送给STM32，执行扫地机的前进、后退、转弯等。

【问题1】（5分）#

图1-1是本题的服务型智能扫地机器人典型的功能结构图，请根据说明的描述，填写（1）~（5）空白处，完成该功能结构图。请将（1） ~ （5）的内容填在答题纸上相应的位置中。

图1-1

【问题1】解答： 这就和初中语文题一样，甚至不如初中语文题的难度；根据：[紧急状态感知：包括碰撞检测、跌落检测和离地检测等功能] ，得（1）紧急状态感知（2）跌落检测根据：[姿态感知：包括运动里程计数和航向测量等功能]，得（3）航向测量根据：[视觉感知：包括单目视觉避障系统和单目视觉定位系统等]，得（4）单目视觉避障系统根据：[（5）扫地及吸尘单元]，得（5）扫地及吸尘单元

【问题2】（6分）#

为了实现服务型智能扫地机器人的功能，就需要多种传感器来感知工作环境信息。王工在对传感器进行选型时，选择了如下类型的传感器： 1.USB摄像头； 2.开关式传感器； 3.槽型光耦模块； 4.数字式防跌落传感器； 5.红外测距传感器； 6.GGPM01A单轴角度陀螺仪（传感器）； 7.霍尔码盘传感器。请根据传感器的功能完成表1-1，请将（1）~（6）的内容填在答题纸上相应的位置中。

图1-2

【问题2】解答： 这道题也简单，大概我就是做扫地机器人得所以我应该都知道，其实也可以先填自己会的，然后再填不会的（1）5.红外测距传感器 （毕竟是19年的题，红外确实可以，而且在22年以前基本都是以红外为主的避障，其实也可以看参数判断出来是输出模拟电压量）（2）4.数字式防跌落传感器 （提示的很明显了，就是防跌落，实际上一般用的不是这么叫得，其实就是用红外，一般一个机器上会有至少三个向下得红外传感器，如果遇到悬崖的话返回的数值就和普通地面不同，但是容易受光线影响，可能在黑色地毯上误识别，所以会多安装一个超声波地毯传感器；在实际中一般不叫跌落传感器，叫悬崖传感器）（3）2.开关式传感器（其实这个选项我犹豫了，但是别的都选完了就剩这个了，车身离地现在大多用的是悬崖传感器，也就是上一问的答案，但是在19年的时候，成本还没有降下来，机器人上喜欢堆零件，于是在车轮上增加了一个车身离地检测，是开关类型的，当拿起扫地机的时候，车轮弹簧会弹出这时候会触碰到所谓的开关传感器；现在基本取消这个了，之前应该是科沃斯的机器有这个）（4）3.槽型光耦模块（这个就是扫地机的前撞碰撞后导致突起部分向后移动，挡住槽型光耦中间，使其信号量改变，从而判断是否碰撞，目前大部分机器都还用着，一般是左右两个）（5）6.GGPM01A单轴角度陀螺仪（传感器） （这个型号的传感器我没听过，可能是某国产吧，航向角由陀螺仪和加速度计共同计算得出，不过一般统称为陀螺仪，在整个开发中这个东西虽然比较小但也是比较头疼的一部分）（6）7.霍尔码盘传感器（这个没什么问题，做过智能小车的都知道，测速用的是码盘，至于是霍尔还是光栅，现在扫地机大多是霍尔，因为便宜）

【问题3】（4分）#

由于该服务型智能扫地机器人的硬件采用双处理器架构，即ARM+STM32双核架构模式，选用串口方式在处理器之间传递数据，如图1-2所示。假设在本串行传输中的数据格式为：8位数据位、1位起始位、1位停止位，无校验位。（1）当波特率为9600b/s时，每秒钟传送的有效数据是多少字节？（2）为保证数据收发正确（每个字节数据传输中的累计误差不大于1/4bit），试分析发送方和接收方时钟允许的误差范围，并以百分比形式给出最大误差。请将答案填写在答题纸的对应栏中。

图1-3

【问题3】解答 （1）比较常规的问波特率的问题，波特率是每秒检测的电平变化个数，9600也就是一秒检测9600次变化，而8位数据位、1位起始位、1位停止位，无校验位说明，如果想传输一个字节（8位）的数据，那就需要在总线上电平变化10次；那么1s9600次变化，也就是960个字节的数据；

（2）一组数据需要10位，累计误差不大于0.25位，也就是10x< 0.25,得x=0.025 也就是2.5%；

第一题结束，15分拿到手没什么问题

第 2 题阅读下列说明和图，回答问题1至问题4，将解答填入答题纸的对应栏内。#

【说明】在服务型智能扫地机器人的系统设计中，李工设计的扫地机器人硬件系统接口如图2-1所示，其中STM32单片机负责进行各个传感器的数据采集，包括利用GPIO进行各种传感器数据的获取、利用ADC进行红外测距仪数据的获取、利用IIC进行陀螺仪（角度）传感器数据的获取，通过串口将数据发送给主处理器S5PV210，并利用脉冲计数进行扫地机器人运动速度、距离等信息的获取。主处理器S5PV210通过USB接口采集实时视频，利用PWM进行云台控制，同时利用USB接口通过WIFI和后台主机实现信息交互。

图2-1

STM32单片机有BOOT1和BOOT0两个启动方式选择管脚，包括三种不同的启动方式，如表2-1所示。

表2-1

李工在STM32单片机的启动电路设计时，采用跳线帽形式进行启动方式选择，对应电路如图2-2所示。

图2-2

李工在利用ADC进行红外测距仪数据的采集设计中，采用STM32内置的12bit逐次比较型ADC，参考电压Vref为3.3V，ADC的采集数字输出为：

为获取扫地机器人在移动过程中的距离、速度等信息，李工将霍尔传感器模块分别安装在左右车轮驱动电机的轴上，通过两个计数器采集左右驱动电机轴上的霍尔传感器脉冲数，以此来计算获取扫地机器人的移动距离、旋转角度等信息。图2-3为扫地机器人的圆形机身和左右两轮的示意图。

图2-3

已知左右车轮转动一圈时，霍尔传感器输出564个脉冲（表示为n0)，那么计数器采集到的脉冲数和左右轮的移动距离关系可以表示为(R为车轮距中心半径，r为左右轮的半径，nx为当前计数器从开始到现在采集到的脉冲数)：

【问题1】（2分）#

如果STM32的选择从主Flash启动，那么图2-2中的跳线帽应该连接管脚1和管脚2还是管脚2和管脚3？

【问题1】解答一般这种启动相关的芯片手册或者用户手册都有给，所以看题干就好，根据表2-1，如果是主Flash启动，那么boot0为0，boot1为x；那么无脑将boot1接地就好，也就是管脚1和管脚2连接（这里解释可能有问题，后续我查找一下）

【问题2】（2分）#

当红外测距仪数据模拟电压Vin为2.4V时，ADC的输出值是多少？（用十进制数表示）

【问题2】解答根据ADC采集公式 ADC = Vin*（2^12-1)/Vref 带入数据： ADC = 2.4*（2^12-1)/3.3 = 2.4*4095/3.3 = 2978

【问题3】（3分）#

假设扫地机器人的车轮距中心半径R=30cm，左右轮的半径r=5cm。当左轮PWM脉冲计数数目为2256时，左轮从脉冲计数开始到现在，移动的距离是多少厘米？（π取3.14）

【问题2】解答和上一题一样，带入公式就好 Dis = 2pirnx/n0 Dis = 23.1452256/564 = 125.6cm

【问题4】（8分）#

李工在扫地机器人设计中加入了陀螺仪模块，通过陀螺仪可以获取不同时刻之间的偏转角，进而与霍尔传感器相融合，使得运动控制器能够直接快速得到系统当前的偏转角。陀螺仪和STM32之间通过IIC接口进行数据通信，陀螺仪和STM32之间还有一个信号线（STM32的GPIO9）和陀螺仪芯片复位线（STM32的GPIO8，低电平为复位，高电平为非复位）。在STM32通过IIC接口获取陀螺仪数据前，需要先对陀螺仪复位并查询信号线的有效性（GPIO9高为有效，低为无效），有效时再通过IIC接口读取陀螺仪的数据。李工设计了通过IIC接口获取陀螺仪数据的流程图（图2-4），请选择合适的项进行补全。图2-4中（1）~（4）备选答案： A.GPIO9和GPIO8都设置为输入 B.GPIO9和GPIO8都设置为输出 C.GPIO9设置为输入，GPIO8设置为输出 D.GPIO9设置为输出，GPIO8设置为输入 E.GPIO9设置为高，持续一段时间，GPIO9设置为低 F.GPIO9设置为低，持续一段时间，GPIO9设置为高 G.GPIO8设置为低，持续一段时间，GPIO8设置为高 H.GPIO8设置为高，持续一段时间，GPIO8设置为低 I.GPIO9为高电平？ J.GPIO9为低电平？ K.GPIO8为高电平？ L.GPIO8为低电平？ M.通过IIC读取数据 N.通过IIC写入数据

图2-4

【问题4】解答

C iic初始化前要进行gpio初始化，复位为输出，信号线为输入 G 对陀螺仪复位 I 查询信号线的有效性 M IIC读数据

（这题出的感觉都不像是研发人员出的）

第 3 题阅读下列说明，回答问题1至问题3，将解答填入答题纸的对应栏内。#

【说明】在服务型智能扫地机器人的软件设计中，为了更好地记录机器人的个体信息和工作信息，设计了信息数据库。数据库主要完成收集服务型智能扫地机器人反馈信息的作用，记录机器人的所有工作记录，方便了使用者对机器人的管理及对机器人状态的掌握，并且在机器人发生运行故障时，可以根据数据库存储的信息分析产生故障的原因。数据库收集服务型智能扫地机器人反馈信息的流程如下：服务端接收反馈信息： 1.第一次解析判断反馈信息类型是否正确。如果正确继续执行2，否则执行3。 2.第二次解析判断反馈信息内容是否正确。如果正确执行4，否则执行3。 3.调用错误信息处理函数后执行4。 4.将反馈信息存入数据库。

【问题1】（6分）#

在本软件测试过程中，测试人员为了设计测试用例，根据题目中的说明，画出了机器人反馈信息收集软件的流程图，如图3-1所示。

图3-1

请在下列选项中，为图3-1中的（1）~（3）选择正确的描述内容，将答案填写在答题纸的对应栏中。空（1）备选答案： A.反馈格式是否正确 B.反馈长度是否正确 C.反馈类型是否正确 D.反馈内容是否正确空（2）备选答案： A.标记错误信息 B.错误信息处理 C.错误信息纠正 D.错误信息报告空（3）备选答案： A.信息存入内存 B.信息存入文件 C.信息存入数据库 D.信息存入软件

【问题1】解答 D B C

【问题2】（4分）#

软件的结构覆盖率是度量测试完整性的一种手段，也是度量测试有效性的一种手段。在嵌入式软件白盒测试过程中，通常以语句覆盖率和分支覆盖率作为度量指标。在实现题目说明的反馈信息接收功能时，设计人员采用了下列算法：

请完成表3-1中的（1）和（2）空白，指出对上述算法达到100%语句覆盖和100%分支覆盖所需的最少测试用例数目。请将答案填写在答题纸的对应栏中。

【问题2】解答

语句：所有语句都来一次，也就3次分支：总共就3个分支，所以3个测试用例就好（不是哥们，你这个题出的我是真的不想看，你但凡多加一个判断语句也行啊）

【问题3】（5分）#

为了测试软件功能，测试人员设计了表3-2所示的测试用例，请填写该表中的空白（1）~（5）。请将答案填写在答题纸的对应栏中。

【问题3】解答（1）报告信息类型不正确（2）报告信息内容不正确（3）正确（4）不正确（5）典型类型3 （不是哥们，你跟我玩阅读理解呢，24年的题但凡考的这么简单我也不用准备25年呀）

| 截至目前拿到45分，已经及格了

第 4 题阅读下列说明和图，回答问题1至问题3，将解答填入答题纸的对应栏内。#

【说明】服务型智能扫地机器人涉及了众多传感器，通过对环境信息的融合感知进行行为决策。同时也设计了丰富的多媒体人机交互系统，实现输入输出功能。

【问题1】(5分)#

服务型智能扫地机器人的输入输出终端采用触摸屏技术，李工设计了一款具有触摸功能的显示单元，该显示单元的主板采用X86架构的处理器，触摸控制板的核心是控制芯片，当触摸笔点击触摸面板时，将通过输入端点上传数据报文，格式如图4-1所示。数据报文按照小端字节顺序发送，第一个Byte表示触摸笔的状态，0x70表示 Down，0x30表示Up，后四个Byte包含触摸笔点击位置的坐标值，Byte1和Byte2表示X轴坐标，Byte3和Byte4表示Y轴坐标。触摸板左上角坐标值为（0，0)，右下角坐标值为（1022，766）。

图4-1

当触摸笔在触摸板中心位置按下时，触摸屏发送的数据报文内容是 (1) 、 (2) 、 (3) 、 (4) 、 (5) ，请将答案填写在答题纸的对应栏中。

【问题1】解答左上和右下确定后，中心点坐标为（511，383）因为是按下，所以state应该是down也就是0x70 511 转 2进制为 111111111 也就是0x1ff，从格式上看应该是10位数据，0-7位存储在byte1，8-9位存储在byte2，所以byte1为0xff，byte2为0x1 383 转 2进制为 101111111 也就是0x17f，从格式上看应该是10位数据，0-7位存储在byte3，8-9位存储在byte4，所以byte3为0x7f，byte4为0x1 所以最终答案应该为： 0x70 0xff 0x1 0x7f 0x1

【问题2】(5分)#

该机器人系统内部集成众多传感器，通过现场总线互联起来，构成了底层的网络，选用的CAN控制器内含二个接收缓冲器、三个发送缓冲器，具有灵活的中断管理能力。李工设计了一种采用中断方式的数据接收程序，每当接收寄存器接收到数据时，中断寄存器的接收中断使能，并产生中断信号通知CPU，中断处理程序在读取数据后，清空中断标识位，等待下一次中断。由于两个接收器共享一个中断号，在调试过程中发现，当短时间（毫秒级）接收大量数据时，丢包现象比较严重。李工对上述流程进行了改进，当产生一次中断时，依次查询两路接收器的状态，这样大大提高了数据接收处理能力，减少了系统开销，其处理流程如图4-2所示。

图4-2

请将上述改进后的双通道数据接收流程图补充完整，将答案填写在答题纸的对应栏中。

【问题2】解答（1）中断初始化（2）接收器0有数据（3）接收器1有数据（4）接收器0有数据（5）清空中断标识位

（这道题主要是（1）这里可能不太好想，其余四个空比较简单，假设（1）空没填出来，也能拿4分）

【问题3】(5分)#

该机器人系统按照图4-2的方式接收传感器采集数据，并由上层应用程序对数据进行处理。当应用访问操作速度和底层硬件输入的速度存在较大差异时，采用环形缓冲机制，可以起到良好的缓冲作用，减少丢包率。李工设计的环形缓冲是基于先进先出(FIFO) 的原则，并设计了环形缓冲的数据结构，读写指针各自独立，读任务和写任务可以同时访问环形缓冲，无需互斥信号量的介入。但是，在运行多任务时，如果存在多个读任务和多个写任务同时操作环形缓冲，则需要应用互斥操作机制。 1.请简述互斥操作的作用。 2.为了在多任务环境中能正确使用环形缓冲，不发生读写冲突，增加了读写锁进行保护。应用任务读取环形缓冲的伪代码如下，请完善其中空(1)~(3)处的内容，将答案写入答题纸对应栏内。

【问题3】解答 1.互斥操作避免在同一时刻不同的程序对文件内容的读写造成程序混乱，保证共享数据操作的完整性；互斥锁保证数据在一个时刻只能有一个应用对其操作；

（1）bReadLock == True （2）if(bReadLock == True) （3）if(bReadLock == False)

(从题干来看读写操作互不影响，这里只是读的函数，那么只需要考虑读的锁，可以先看（3）空，返回数据长度，那说明这个时候锁是开着的，也就是if(bReadLock == False)，对应的上面两个空都是关锁状态)

| 截至目前拿到59-60分

第 5 题阅读以下关于C语言的基本概念和编程方面的叙述，回答问题1至问题3，将解答填入答题纸的对应栏内。#

【说明】服务型智能扫地机器人的信息处理中心，用于接收各种传感器信息和视觉信息，通过分析处理进行扫地机器人的运动控制，且负责和后台监控中心通信。信息处理中心基于嵌入式计算机实现。

【问题1】(4分)#

在信息处理中心的计算机系统中，系统总是要用户对变量或寄存器进行位操作，下面程序1代码中对给定整型变量a，实现了两个函数，其中：set_bit3函数用于设置a的bit 3（第3位），clear_bit3函数用于清除a的bit3（第3位）。请补全程序1代码的（1）和（2），将正确代码写在答题纸的相应位置。

【程序1】

【问题1】解答

（1）a|=BIT3 （2）a&=~BIT3 (题干表述的不明确，它应该说是设置第三位为1，以及清除第三位为0，设置的话就或1就好，清除的话就且0，其余位相反)

【问题2】(6分)#

信息处理计算机采用中断方式快速响应外部事件。中断是嵌入式系统中重要的组成部分，这使得很多编译器开发商提供了标准C支持中断的扩展（使用关键字_interrupt）。下面程序2是王工设计的中断处理程序代码，请对这段程序进行同行评审，找出你认为不合适的3个方面，将解答写入答题纸相应的位置。【程序2】

（1）中断一般不传参（2）中断一般没返回值（3）中断中不建议使用printf操作，printf是阻塞性调用（4）中断中不建议使用浮点运算（5）没有清除中断标志位

【问题3】(5分)#

王工设计了信息处理中的软件算法，利用单向链表来存储信息。其中如下给定的程序3中已建立一个带有头结点的单向链表，链表中的各结点按结点数据域中的数据从小到大顺序链接。函数fun的功能是：把参数x的值放入一个新结点并插入到链表中，插入后各结点仍保持从小到大顺序排列。该程序的实现如程序3所示。【程序3】根据题意分析程序，完成程序3的空（1）~（5），将解答写入答题纸相应的位置。

【问题3】解答（1） x （2） s （3） q->next （4） s （5） q

假设代码题你无法拿满分，不小心扣了一两分，那么此时，你的分数是70-75分

完美过关！

还是那句话，这但凡是24年考题，我也不至于准备25年考试

牛耕分解+形态学分割全覆盖路径规划（二）Part2. 覆盖

Sat, 14 Sep 2024 07:39:43 GMT

书接上文：牛耕分解+形态学分割全覆盖路径规划（二）Part1. 分割

在上文中我们单独对“分区”做了较为详细的了解，本文中，我们对“覆盖”再做探究

先放结果：

算法作者给的注释，大致的给出了整个算法的工作流程：

I.Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is found that suits best for calculating the cells, i.e. such that longer cells occur, and the map is rotated in this manner. This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the original orientation.

II.Sweep a slice (a morse function) trough the given map and check for connectivity of this line, i.e. how many connected segments there are. If the connectivity increases, i.e. more segments appear, an IN event occurs that opens new separate cells, if it decreases, i.e. segments merge, an OUT event occurs that merges two cells together. If an event occurs, the algorithm checks along the current line for critical points, that are points that trigger the events. From these the boundary of the cells are drawn, starting from the CP and going left/right until a black pixel is hit.

III.After all cells have been determined by the sweeping slice, the algorithm finds these by using cv::findContours(). This gives a set of points for each cell, that are used to create a generalizedPolygon out of each cell.

IV.After all polygons have been created, plan the path trough all of them for the field of view s.t. the whole area is covered. To do so, first a global path trough all cells is generated, using the traveling salesmen problem formulation. This produces an optimal visiting order of the cells. Next for each cell a boustrophedon path is determined, which goes back and forth trough the cell and between the horizontal paths along the boundaries of the cell, what ensures that the whole area of the cell is covered. For each cell the longest edge is found and it is transformed s.t. this edge lies horizontal to the x-direction. This produces longer but fewer edges, what improves the path for small but long cells. The startpoint of the cell-path is determined by the endpoint of the previous cell, s.t. the distance between two cell-paths is minimized. The cell-path is determined in the rotated manner, so in a last step, the cell-path is transformed to the originally transformed cell and after that inserted in the global path.

V.The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t. the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot pose is not in the free space, another accessible point is generated by finding it on the radius around the field of view (fov) middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the corresponding Boolean to false (shows that the path planning should be done for the robot footprint).

程序中的执行逻辑：

I. Find the main directions of the map and rotate it in this manner.
II. Sweep a slice trough the map and mark the found cell boundaries.
III. Find the separated cells.
IV. Determine the cell paths.
V. Get the robot path out of the fov path.

从代码实现来看，先看调用：

1
boustrophedon_variant_explorer_.getExplorationPath(room_map, exploration_path, map_resolution_, starting_position_, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);

使用 getExplorationPath 函数时需要先准备好以下参数：

room_map: cv::Mat 类型矩阵，表示房间地图
exploration_path: 这是一个向量，用于存储计算出的探索路径。每个元素是一个geometry_msgs::Pose2D对象，表示机器人在探索过程中的位置和方向。
map_resolution_: 表示地图的分辨率，即地图上每个像素代表的实际距离。这个参数用于将地图上的尺寸转换为实际的物理尺寸。
starting_position_: cv::Point 对象，表示机器人的初始位置。
map_origin_: 地图原点坐标，在MapCallback中给出，一般在nav_msgs::OccupancyGrid消息类型中的info.origin中给出。
grid_spacing_in_pixel: 这是网格的像素间距。它是基于机器人覆盖半径计算出的，用于确定探索网格的大小。
grid_obstacle_offset_: 这是网格中障碍物的偏移量。它用于调整障碍物在网格中的位置，以便更准确地反映实际障碍物的位置。
path_eps_: 这是路径精度参数。它决定了路径规划的精细程度，较小的值意味着更精确的路径。
cell_visiting_order_: 在该算法中，用于判断使用TSP规划器还是自左向右规划期进行规划
zero_vector: 这是一个二维零向量，通常用于初始化或作为默认参数传递。
min_cell_area_: 最小分区面积，用在分割关键点区域后，进行多个区域合并
max_deviation_from_track_: 这是机器人从预定路径上最大的偏离量。它用于确保机器人在探索过程中不会偏离太远。

Step 1 寻找地图的主要方向并旋转地图 主要调用 RoomRotator 进行实现

1
RoomRotator room_rotation;
2
room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0,rotation_offset);
3
room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);

第一步 computeRoomRotationMatrix 确定房间的主要方向，并返回旋转矩阵R；在这里说明一下，参数rotation_offset是可选的旋转偏移量，由于传入为 0. ，所以不设置旋转偏移。computeRoomRotationMatrix的实现如下：

1
// compute the affine rotation matrix for rotating a room into parallel alignment with x-axis (longer side of the room is aligned with x-axis)
2
// R is the transform
3
// bounding_rect is the ROI of the warped image
4
double RoomRotator::computeRoomRotationMatrix(const cv::Mat& room_map, cv::Mat& R, cv::Rect& bounding_rect,
5
        const double map_resolution, const cv::Point* center, const double rotation_offset)
6
{
7
    // rotation angle of the map s.t. the most occurring gradient is in 90 degree to the x-axis
8
    double rotation_angle = computeRoomMainDirection(room_map, map_resolution) + rotation_offset;
9
    std::cout << "RoomRotator::computeRoomRotationMatrix: main rotation angle: " << rotation_angle << std::endl;
10

11
    // get rotation matrix R for rotating the image around the center of the room contour
12
    //  Remark: rotation angle in degrees for opencv
13
    cv::Point center_of_rotation;
14
    if (center == 0)
15
    {
16
        cv::Point min_room, max_room;
17
        getMinMaxCoordinates(room_map, min_room, max_room);
18
        center_of_rotation.x = 0.5*(min_room.x+max_room.x);
19
        center_of_rotation.y = 0.5*(min_room.y+max_room.y);
20
    }
21
    else
22
        center_of_rotation = *center;
23

24
    // compute rotation
25
    R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0);
26

27
    // determine bounding rectangle to find the size of the new image
28
    bounding_rect = cv::RotatedRect(center_of_rotation, room_map.size(), (rotation_angle*180)/CV_PI).boundingRect();
29
    // adjust transformation matrix
30
    R.at<double>(0,2) += 0.5*bounding_rect.width - center_of_rotation.x;
31
    R.at<double>(1,2) += 0.5*bounding_rect.height - center_of_rotation.y;
32

33
    return rotation_angle;
34
}

该函数可以概括为这么几步：计算旋转角度->确定旋转中心->计算旋转矩阵->调整旋转矩阵

1.1 计算旋转角度 computeRoomMainDirection

1
// computes the major direction of the walls from a map (preferably one room)
2
// the map (room_map, CV_8UC1) is black (0) at impassable areas and white (255) on drivable areas
3
double RoomRotator::computeRoomMainDirection(const cv::Mat& room_map, const double map_resolution)
4
{
5
    const double map_resolution_inverse = 1./map_resolution;
6

7
    // compute Hough transform on edge image of the map
8
    cv::Mat edge_map;
9
    cv::Canny(room_map, edge_map, 50, 150, 3);
10
    std::vector<cv::Vec4i> lines;
11
    double min_line_length = 1.0;   // in [m]
12
    for (; min_line_length > 0.1; min_line_length -= 0.2)
13
    {
14
        cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse);
15
        cv::Mat room_hough = edge_map.clone();
16
        for (size_t i=0; i<lines.size(); ++i)
17
        {
18
            cv::Point p1(lines[i][0], lines[i][1]), p2(lines[i][2], lines[i][3]);
19
            cv::line(room_hough, p1, p2, cv::Scalar(128), 2);
20
        }
21
        //cv::imshow("room_hough", room_hough);
22
        if (lines.size() >= 4)
23
            break;
24
    }
25
    // setup a histogram on the line directions weighted by their length to determine the major direction
26
    Histogram<double> direction_histogram(0, CV_PI, 36);
27
    for (size_t i=0; i<lines.size(); ++i)
28
    {
29
        double dx = lines[i][2] - lines[i][0];
30
        double dy = lines[i][3] - lines[i][1];
31
        if(dy*dy+dx*dx > 0.0)
32
        {
33
            double current_direction = std::atan2(dy, dx);
34
            while (current_direction < 0.)
35
                current_direction += CV_PI;
36
            while (current_direction > CV_PI)
37
                current_direction -= CV_PI;
38
            direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx));
39
            //std::cout << " dx=" << dx << "   dy=" << dy << "   dir=" << current_direction << "   len=" << sqrt(dy*dy+dx*dx) << std::endl;
40
        }
41
    }
42
    return direction_histogram.getMaxBinPreciseVal();
43
}

① 通过OpenCV的 Canny边缘检测算法识别房间地图的边缘； ② 对边缘图像edge_map应用霍夫变换检测直线，将直线段存储在std::vector<cv::Vec4i> lines中，通过逐渐减小直线长度的最小阈值min_line_length尝试找到足够长的直线段，从而代表房间的额主要结构； ③ 设置一个方向直方图Histogram<double> direction_histogram用于统计所有直线段的方向，每个直线段的方向通过其两端点的差分计算得出，并加权其长度，以确保较长的直线段对最终方向的贡献更大； ④ 函数通过查找方向直方图中的最大值来确定房间的主要方向。

1.2 确定旋转中心

1
// get rotation matrix R for rotating the image around the center of the room contour
2
//  Remark: rotation angle in degrees for opencv
3
cv::Point center_of_rotation;
4
if (center == 0)
5
{
6
    cv::Point min_room, max_room;
7
    getMinMaxCoordinates(room_map, min_room, max_room);
8
    center_of_rotation.x = 0.5*(min_room.x+max_room.x);
9
    center_of_rotation.y = 0.5*(min_room.y+max_room.y);
10
}
11
else
12
    center_of_rotation = *center;

这里的getMinMaxCoordinates方法是获取房间中心点，具体实现如下：

1
void RoomRotator::getMinMaxCoordinates(const cv::Mat& map, cv::Point& min_room, cv::Point& max_room)
2
{
3
    min_room.x = std::numeric_limits<int>::max();
4
    min_room.y = std::numeric_limits<int>::max();
5
    max_room.x = 0;
6
    max_room.y = 0;
7
    for (int v=0; v<map.rows; ++v)
8
    {
9
        for (int u=0; u<map.cols; ++u)
10
        {
11
            if (map.at<uchar>(v,u)==255)
12
            {
13
                min_room.x = std::min(min_room.x, u);
14
                min_room.y = std::min(min_room.y, v);
15
                max_room.x = std::max(max_room.x, u);
16
                max_room.y = std::max(max_room.y, v);
17
            }
18
        }
19
    }
20
}

1.3 计算旋转矩阵

1
R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0);

调用了OpenCV的getRotationMatrix2D方法

1.4 调整旋转矩阵

1
// determine bounding rectangle to find the size of the new image
2
bounding_rect = cv::RotatedRect(center_of_rotation, room_map.size(),(rotation_angle*180)/CV_PI).boundingRect();
3
// adjust transformation matrix
4
R.at<double>(0,2) += 0.5*bounding_rect.width - center_of_rotation.x;
5
R.at<double>(1,2) += 0.5*bounding_rect.height - center_of_rotation.y;

为了确保旋转后的图像完全位于视图框内，函数计算了旋转后图像的边界矩形bounding_rect，并相应地调整旋转矩阵的平移部分。这一步确保旋转后的图像不会被裁剪。

第二步 rotateRoom 根据第一步计算好的旋转矩阵R和边界矩形bounding_rect对房间进行旋转

1
void RoomRotator::rotateRoom(const cv::Mat& room_map, cv::Mat& rotated_room_map, const cv::Mat& R, const cv::Rect& bounding_rect)
2
{
3
    // rotate the image
4
    cv::warpAffine(room_map, rotated_room_map, R, bounding_rect.size(), cv::INTER_AREA);
5

6
    // apply a binary filter to create a binary image, also use a closing operator to smooth the output (the rotation might produce
7
    // black pixels reaching into the white area that were not there before, causing new, wrong cells to open)
8
#if CV_MAJOR_VERSION<=3
9
    cv::threshold(rotated_room_map, rotated_room_map, 127, 255, CV_THRESH_BINARY);
10
#else
11
    cv::threshold(rotated_room_map, rotated_room_map, 127, 255, cv::THRESH_BINARY);
12
#endif
13
// this should not be used because it removes smaller obstacles like thin walls from the room and hence lets a planner generate paths through walls
14
//  cv::dilate(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1);
15
//  cv::erode(rotated_room_map, rotated_room_map, cv::Mat(), cv::Point(-1,-1), 1);
16
}

rotateRoom进行了两步操作，图像旋转和二值化，没什么特别的；作者在这里注释掉了腐蚀膨胀操作，表明可能腐蚀膨胀后会出现穿墙的路径。

Step 2 分割地图

1
void BoustrophedonExplorer::computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
2
        const int min_cell_width, std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
3
{
4
    // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
5
    // create a map copy to mark the cell boundaries
6
    cv::Mat cell_map = room_map.clone();
7

8
    // find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
9
    size_t y_start = 0;
10
    bool found = false, obstacle = false;
11
    int previous_number_of_segments = 0;
12
    std::vector<int> previous_obstacles_end_x;        // keep track of the end points of obstacles
13
    for(size_t y=0; y<room_map.rows; ++y)
14
    {
15
        for(size_t x=0; x<room_map.cols; ++x)
16
        {
17
            if(found == false && room_map.at<uchar>(y,x) == 255)
18
            {
19
                y_start = y;
20
                found = true;
21
            }
22
            else if(found == true && obstacle == false && room_map.at<uchar>(y,x) == 0)
23
            {
24
                ++previous_number_of_segments;
25
                obstacle = true;
26
            }
27
            else if(found == true && obstacle == true && room_map.at<uchar>(y,x) == 255)
28
            {
29
                obstacle = false;
30
                previous_obstacles_end_x.push_back(x);
31
            }
32
        }
33

34
        if(found == true)
35
            break;
36
    }
37

38
    // sweep trough the map and detect critical points
39
    for(size_t y=y_start+1; y<room_map.rows; ++y) // start at y_start+1 because we know number of segments at y_start
40
    {
41
        int number_of_segments = 0; // int to count how many segments at the current slice are
42
        std::vector<int> current_obstacles_start_x;
43
        std::vector<int> current_obstacles_end_x;
44
        bool obstacle_hit = false; // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event
45
        bool hit_white_pixel = false; // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel
46

47
        // count number of segments within this row
48
        for(size_t x=0; x<room_map.cols; ++x)
49
        {
50
            if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
51
                hit_white_pixel = true;
52
            else if(hit_white_pixel == true)
53
            {
54
                if(obstacle_hit == false && room_map.at<uchar>(y,x) == 0) // check for obstacle
55
                {
56
                    ++number_of_segments;
57
                    obstacle_hit = true;
58
                    current_obstacles_start_x.push_back(x);
59
                }
60
                else if(obstacle_hit == true && room_map.at<uchar>(y,x) == 255) // check for leaving obstacle
61
                {
62
                    obstacle_hit = false;
63
                    current_obstacles_end_x.push_back(x);
64
                }
65
            }
66
        }
67

68
        // if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
69
        bool segment_shift_detected = false;
70
        if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1)
71
        {
72
            for (size_t i=0; i<previous_obstacles_end_x.size(); ++i)
73
                if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
74
                {
75
                    segment_shift_detected = true;
76
                    break;
77
                }
78
        }
79

80
        // reset hit_white_pixel to use this Boolean later
81
        hit_white_pixel = false;
82

83
        // check if number of segments has changed --> event occurred
84
        if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
85
        {
86
            // check the current slice again for critical points
87
            for(int x=0; x<room_map.cols; ++x)
88
            {
89
                if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
90
                    hit_white_pixel = true;
91
                else if(hit_white_pixel == true && room_map.at<uchar>(y,x) == 0)
92
                {
93
                    // check over black pixel for other black pixels, if none occur a critical point is found
94
                    bool critical_point = true;
95
                    for(int dx=-1; dx<=1; ++dx)
96
                        if(room_map.at<uchar>(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0)
97
                            critical_point = false;
98

99
                    // if a critical point is found mark the separation, note that this algorithm goes left and right
100
                    // starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
101
                    // behind other obstacles on the same y-value as the critical point
102
                    if(critical_point == true)
103
                    {
104
                        // to the left until a black pixel is hit
105
                        for(int dx=-1; x+dx>=0; --dx)
106
                        {
107
                            uchar& val = cell_map.at<uchar>(y,x+dx);
108
                            if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
109
                                val = BORDER_PIXEL_VALUE;
110
                            else if(val == 0)
111
                                break;
112
                        }
113

114
                        // to the right until a black pixel is hit
115
                        for(int dx=1; x+dx<room_map.cols; ++dx)
116
                        {
117
                            uchar& val = cell_map.at<uchar>(y,x+dx);
118
                            if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
119
                                val = BORDER_PIXEL_VALUE;
120
                            else if(val == 0)
121
                                break;
122
                        }
123
                    }
124
                }
125
            }
126
        }
127
        else if(previous_number_of_segments > number_of_segments) // OUT event
128
        {
129
            // check the previous slice again for critical points --> y-1
130
            for(int x=0; x<room_map.cols; ++x)
131
            {
132
                if(room_map.at<uchar>(y-1,x) == 255 && hit_white_pixel == false)
133
                    hit_white_pixel = true;
134
                else if(hit_white_pixel == true && room_map.at<uchar>(y-1,x) == 0)
135
                {
136
                    // check over black pixel for other black pixels, if none occur a critical point is found
137
                    bool critical_point = true;
138
                    for(int dx=-1; dx<=1; ++dx)
139
                        if(room_map.at<uchar>(y,std::max(0,std::min(x+dx, room_map.cols-1))) == 0) // check at side after obstacle
140
                            critical_point = false;
141

142
                    // if a critical point is found mark the separation, note that this algorithm goes left and right
143
                    // starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
144
                    // behind other obstacles on the same y-value as the critical point
145
                    if(critical_point == true)
146
                    {
147
                        const int ym2 = std::max(0,(int)y-2);
148

149
                        // to the left until a black pixel is hit
150
                        for(int dx=-1; x+dx>=0; --dx)
151
                        {
152
                            uchar& val = cell_map.at<uchar>(y-1,x+dx);
153
                            if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
154
                                val = BORDER_PIXEL_VALUE;
155
                            else if(val == 0)
156
                                break;
157
                        }
158

159
                        // to the right until a black pixel is hit
160
                        for(int dx=1; x+dx<room_map.cols; ++dx)
161
                        {
162
                            uchar& val = cell_map.at<uchar>(y-1,x+dx);
163
                            if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
164
                                val = BORDER_PIXEL_VALUE;
165
                            else if(val == 0)
166
                                break;
167
                        }
168
                    }
169
                }
170
            }
171
        }
172

173
        // save the found number of segments and the obstacle end points
174
        previous_number_of_segments = number_of_segments;
175
        previous_obstacles_end_x = current_obstacles_end_x;
176
    }
177

178
#ifdef DEBUG_VISUALIZATION
179
    cv::imshow("cell_map", cell_map);
180
#endif
181

182
    // *********************** II.b) merge too small cells into bigger cells ***********************
183
    cv::Mat cell_map_labels;
184
    const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);
185

186
    // *********************** III. Find the separated cells. ***********************
187
    std::vector<std::vector<cv::Point> > cells;
188
    for (int i=1; i<=number_of_cells; ++i)
189
    {
190
        cv::Mat cell_copy(cell_map_labels == i);
191
        std::vector<std::vector<cv::Point> > cellsi;
192
#if CV_MAJOR_VERSION<=3
193
        cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
194
#else
195
        cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
196
#endif
197
        cells.insert(cells.end(), cellsi.begin(), cellsi.end());
198
    }
199

200
#ifdef DEBUG_VISUALIZATION
201
//  // testing
202
//  cv::Mat black_map = cv::Mat::zeros(cell_map.rows, cell_map.cols, cell_map.type());
203
//  for(size_t i=0; i<cells.size(); ++i)
204
//  {
205
//      cv::drawContours(black_map, cells, i, cv::Scalar(127), cv::FILLED);
206
//      cv::imshow("contours", black_map);
207
//      cv::waitKey();
208
//  }
209
#endif
210

211
    // create generalized Polygons out of the contours to handle the cells
212
    for(size_t cell=0; cell<cells.size(); ++cell)
213
    {
214
        GeneralizedPolygon current_cell(cells[cell], map_resolution);
215
//      std::cout << cell+1 << ": " << current_cell.getArea() << std::endl;
216
        if(current_cell.getArea()>=min_cell_area)
217
        {
218
            cell_polygons.push_back(current_cell);
219
            polygon_centers.push_back(current_cell.getCenter());
220
        }
221
        else
222
        {
223
            std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
224
        }
225
    }
226
}

这一部分较为庞大，我们尽可能的不留疑惑去分析 2.1 查找第一个可通行区域位置

1
// find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
2
size_t y_start = 0;
3
bool found = false, obstacle = false;
4
int previous_number_of_segments = 0;
5
std::vector<int> previous_obstacles_end_x;        // keep track of the end points of obstacles
6
for(size_t y=0; y<room_map.rows; ++y)
7
{
8
    for(size_t x=0; x<room_map.cols; ++x)
9
    {
10
        if(found == false && room_map.at<uchar>(y,x) == 255)
11
        {
12
            y_start = y;
13
            found = true;
14
        }
15
        else if(found == true && obstacle == false && room_map.at<uchar>(y,x) == 0)
16
        {
17
            ++previous_number_of_segments;
18
            obstacle = true;
19
        }
20
        else if(found == true && obstacle == true && room_map.at<uchar>(y,x) == 255)
21
        {
22
            obstacle = false;
23
            previous_obstacles_end_x.push_back(x);
24
        }
25
    }
26

27
    if(found == true)
28
        break;
29
}

逻辑：遍历地图寻找可通行区域，找到后found置为true；在该行中继续查找，如果found为true且当前像素值为0（黑色），则表示进入了一个障碍物。此时，增加previous_number_of_segments的值，并将obstacle标志设置为true；如果found为true且obstacle为true且当前像素值为255，则表示离开了障碍物。此时，将obstacle标志设置为false，并将当前的x坐标添加到previous_obstacles_end_x中；在找到可通过区域后，检索完该行后跳出外层循环。

2.2 检测关键点

1
for(size_t y=y_start+1; y<room_map.rows; ++y)
2
{
3
    …………
4

5
}

这段代码用于在房间地图上扫描并检测关键点（critical points），由于整个for中的步骤过多，所以拆分为多个步骤进行研究。 2.2.1 计算当前行的障碍物段数

1
// count number of segments within this row
2
for(size_t x=0; x<room_map.cols; ++x)
3
{
4
    if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
5
        hit_white_pixel = true;
6
    else if(hit_white_pixel == true)
7
    {
8
        if(obstacle_hit == false && room_map.at<uchar>(y,x) == 0)
9
        {
10
            ++number_of_segments;
11
            obstacle_hit = true;
12
            current_obstacles_start_x.push_back(x);
13
        }
14
        else if(obstacle_hit == true && room_map.at<uchar>(y,x) == 255)
15
        {
16
            obstacle_hit = false;
17
            current_obstacles_end_x.push_back(x);
18
        }
19
    }
20
}

通过遍历当前行的每个像素，根据像素值（0表示黑色，255表示白色）来判断障碍物的开始和结束，从而计算出当前行的障碍物段数。

2.2.2 检测段数变化或段位移

1
// if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
2
bool segment_shift_detected = false;
3
if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1)
4
{
5
    for (size_t i=0; i<previous_obstacles_end_x.size(); ++i)
6
        if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
7
    {
8
        segment_shift_detected = true;
9
        break;
10
    }
11
}
12

13
// reset hit_white_pixel to use this Boolean later
14
hit_white_pixel = false;
15

16
// check if number of segments has changed --> event occurred
17
if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
18
{
19
    ……
20
}
21
else if(previous_number_of_segments > number_of_segments) // OUT event
22
{
23
……
24
}

首先检查是否发生了“IN事件”（即障碍物段数增加或段位移）或“OUT事件”（即障碍物段数减少）。这些事件表明房间地图的结构发生了变化，需要重新检测关键点。

2.2.3 检测关键点

1
// check over black pixel for other black pixels, if none occur a critical point is found
2
bool critical_point = true;
3
for(int dx=-1; dx<=1; ++dx)
4
    if(room_map.at<uchar>(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0)
5
        critical_point = false;

这里需要区分“IN”事件还是“OUT”事件对于“IN事件”，从第一个白色像素开始，检查每个黑色像素周围是否没有其他黑色像素。如果是这样，那么当前的黑色像素就是一个关键点。对于“OUT事件”，同样检查黑色像素周围是否没有其他黑色像素，但是这次是在前一行的基础上进行检查。

2.2.4 标记关键点

1
// if a critical point is found mark the separation, note that this algorithm goes left and right
2
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
3
// behind other obstacles on the same y-value as the critical point
4
if(critical_point == true)
5
{
6
    // to the left until a black pixel is hit
7
    for(int dx=-1; x+dx>=0; --dx)
8
    {
9
        uchar& val = cell_map.at<uchar>(y,x+dx);
10
        if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
11
            val = BORDER_PIXEL_VALUE;
12
        else if(val == 0)
13
            break;
14
    }
15

16
    // to the right until a black pixel is hit
17
    for(int dx=1; x+dx<room_map.cols; ++dx)
18
    {
19
        uchar& val = cell_map.at<uchar>(y,x+dx);
20
        if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
21
            val = BORDER_PIXEL_VALUE;
22
        else if(val == 0)
23
            break;
24
    }
25
}

1
// if a critical point is found mark the separation, note that this algorithm goes left and right
2
// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
3
// behind other obstacles on the same y-value as the critical point
4
if(critical_point == true)
5
{
6
    const int ym2 = std::max(0,(int)y-2);
7

8
    // to the left until a black pixel is hit
9
    for(int dx=-1; x+dx>=0; --dx)
10
    {
11
        uchar& val = cell_map.at<uchar>(y-1,x+dx);
12
        if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
13
            val = BORDER_PIXEL_VALUE;
14
        else if(val == 0)
15
            break;
16
    }
17

18
    // to the right until a black pixel is hit
19
    for(int dx=1; x+dx<room_map.cols; ++dx)
20
    {
21
        uchar& val = cell_map.at<uchar>(y-1,x+dx);
22
        if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
23
            val = BORDER_PIXEL_VALUE;
24
        else if(val == 0)
25
            break;
26
    }
27
}

一旦找到关键点，就从该点开始向左和向右扩展，直到遇到障碍物。在扩展过程中，会在cell_map中标记这些边界。这些标记用于后续的单元格分解。

下图为关键点扩展后的cell_map，也是在经过上述操作后的cell_map

2.3 合并单元格如上图所示，在经过关键点查找后地图被分割成了多块区域，后续的算法是在每个区域内进行规划，区域之间并不连接。如果区域过多的话，就会发生实际路径效率较差的情况，所以需要将一些覆盖范围较小的区域进行合并，这时候就用到了开篇的参数min_cell_area

1
cv::Mat cell_map_labels;
2
const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);

mergeCells

1
int BoustrophedonExplorer::mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width)
2
{
3
    // label all cells
4
    //   --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels
5
    cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0);
6
    //   --> re-assign the cell borders with -1
7
    for (int v=0; v<cell_map_labels.rows; ++v)
8
        for (int u=0; u<cell_map_labels.cols; ++u)
9
            if (cell_map_labels.at<int>(v,u) == BORDER_PIXEL_VALUE*256)
10
                cell_map_labels.at<int>(v,u) = -1;
11
    //   --> flood fill cell regions with unique id labels
12
    std::map<int, boost::shared_ptr<BoustrophedonCell> > cell_index_mapping;        // maps each cell label --> to the cell object
13
    int label_index = 1;
14
    for (int v=0; v<cell_map_labels.rows; ++v)
15
    {
16
        for (int u=0; u<cell_map_labels.cols; ++u)
17
        {
18
            // if the map has already received a label for that pixel --> skip
19
            if (cell_map_labels.at<int>(v,u)!=65280)
20
                continue;
21

22
            // fill each cell with a unique id
23
            cv::Rect bounding_box;
24
            const double area = cv::floodFill(cell_map_labels, cv::Point(u,v), label_index, &bounding_box, 0, 0, 4);
25
            cell_index_mapping[label_index] = boost::shared_ptr<BoustrophedonCell>(new BoustrophedonCell(label_index, area, bounding_box));
26
            label_index++;
27
            if (label_index == INT_MAX)
28
                std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl;
29
        }
30
    }
31
    std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index-1 << " cells before merging." << std::endl;
32

33
    // determine the neighborhood relationships between all cells
34
    for (int v=1; v<cell_map_labels.rows-1; ++v)
35
    {
36
        for (int u=1; u<cell_map_labels.cols-1; ++u)
37
        {
38
            if (cell_map_labels.at<int>(v,u)==-1) // only check the border points for neighborhood relationships
39
            {
40
                const int label_left = cell_map_labels.at<int>(v,u-1);
41
                const int label_right = cell_map_labels.at<int>(v,u+1);
42
                if (label_left>0 && label_right>0)
43
                {
44
                    cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]);
45
                    cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]);
46
                }
47
                const int label_up = cell_map_labels.at<int>(v-1,u);
48
                const int label_down = cell_map_labels.at<int>(v+1,u);
49
                if (label_up>0 && label_down>0)
50
                {
51
                    cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]);
52
                    cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]);
53
                }
54
            }
55
        }
56
    }
57
#ifdef DEBUG_VISUALIZATION
58
//  printCells(cell_index_mapping);
59
//  cv::imshow("cell_map",cell_map);
60
//  cv::waitKey();
61
#endif
62

63
    // iteratively merge cells
64
    mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width);
65

66
    // re-assign area labels to 1,2,3,4,...
67
    int new_cell_label = 1;
68
    for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc, ++new_cell_label)
69
        for (int v=0; v<cell_map_labels.rows; ++v)
70
            for (int u=0; u<cell_map_labels.cols; ++u)
71
                if (cell_map_labels.at<int>(v,u)==itc->second->label_)
72
                    cell_map_labels.at<int>(v,u) = new_cell_label;
73

74
    std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size() << " cells remaining after merging." << std::endl;
75
    return cell_index_mapping.size();
76
}

mergeCellsSelection:

1
void BoustrophedonExplorer::mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping,
2
        const double min_cell_area, const int min_cell_width)
3
{
4
    // iteratively merge cells
5
    // merge small cells below min_cell_area with their largest neighboring cell
6
    std::multimap<double, boost::shared_ptr<BoustrophedonCell> > area_to_region_id_mapping;     // maps the area of each cell --> to the respective cell
7
    for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
8
        area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
9
    for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::iterator it=area_to_region_id_mapping.begin(); it!=area_to_region_id_mapping.end();)
10
    {
11
        // skip if segment is large enough (area and side length criteria)
12
        if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width)
13
        {
14
            ++it;
15
            continue;
16
        }
17

18
        // skip segments which have no neighbors
19
        if (it->second->neighbors_.size() == 0)
20
        {
21
            std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
22
            ++it;
23
            continue;
24
        }
25

26
        // determine the largest neighboring cell
27
        const BoustrophedonCell& small_cell = *(it->second);
28
        std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double> > area_sorted_neighbors;
29
        for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn)
30
            area_sorted_neighbors.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >((*itn)->area_, *itn));
31
        BoustrophedonCell& large_cell = *(area_sorted_neighbors.begin()->second);
32

33
        // merge the cells
34
        mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);
35

36
        // update area_to_region_id_mapping
37
        area_to_region_id_mapping.clear();
38
        for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
39
            area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
40
        it = area_to_region_id_mapping.begin();
41

42
#ifdef DEBUG_VISUALIZATION
43
//      printCells(cell_index_mapping);
44
//      cv::imshow("cell_map",cell_map);
45
//      cv::waitKey();
46
#endif
47
    }
48

49
    // label remaining border pixels with label of largest neighboring region label
50
    for (int v=1; v<cell_map.rows-1; ++v)
51
    {
52
        for (int u=1; u<cell_map.cols-1; ++u)
53
        {
54
            if (cell_map.at<uchar>(v,u) == BORDER_PIXEL_VALUE)
55
            {
56
                std::set<int> neighbor_labels;
57
                for (int dv=-1; dv<=1; ++dv)
58
                {
59
                    for (int du=-1; du<=1; ++du)
60
                    {
61
                        const int& val = cell_map_labels.at<int>(v+dv,u+du);
62
                        if (val>0)
63
                            neighbor_labels.insert(val);
64
                    }
65
                }
66
                if (neighbor_labels.size() > 0)
67
                {
68
                    int new_label = -1;
69
                    for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::reverse_iterator it=area_to_region_id_mapping.rbegin(); it!=area_to_region_id_mapping.rend(); ++it)
70
                    {
71
                        if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
72
                        {
73
                            cell_map_labels.at<int>(v,u) = it->second->label_;
74
                            break;
75
                        }
76
                    }
77
                }
78
                else
79
                    std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl;
80
            }
81
        }
82
    }
83
}

mergeTwoCells:

1
void BoustrophedonExplorer::mergeTwoCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const BoustrophedonCell& minor_cell, BoustrophedonCell& major_cell,
2
        std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping)
3
{
4
    // execute merging the minor cell into the major cell
5
    //   --> remove border from maps
6
    for (int v=0; v<cell_map.rows; ++v)
7
        for (int u=0; u<cell_map.cols; ++u)
8
            if (cell_map.at<uchar>(v,u) == BORDER_PIXEL_VALUE &&
9
                    ((cell_map_labels.at<int>(v,u-1)==minor_cell.label_ && cell_map_labels.at<int>(v,u+1)==major_cell.label_) ||
10
                    (cell_map_labels.at<int>(v,u-1)==major_cell.label_ && cell_map_labels.at<int>(v,u+1)==minor_cell.label_) ||
11
                    (cell_map_labels.at<int>(v-1,u)==minor_cell.label_ && cell_map_labels.at<int>(v+1,u)==major_cell.label_) ||
12
                    (cell_map_labels.at<int>(v-1,u)==major_cell.label_ && cell_map_labels.at<int>(v+1,u)==minor_cell.label_)))
13
            {
14
                cell_map.at<uchar>(v,u) = 255;
15
                cell_map_labels.at<int>(v,u) = major_cell.label_;
16
                major_cell.area_ += 1;
17
            }
18
    //   --> update old label in cell_map_labels
19
    for (int v=0; v<cell_map_labels.rows; ++v)
20
        for (int u=0; u<cell_map_labels.cols; ++u)
21
            if (cell_map_labels.at<int>(v,u) == minor_cell.label_)
22
                cell_map_labels.at<int>(v,u) = major_cell.label_;
23
    //   --> update major_cell
24
    major_cell.area_ += minor_cell.area_;
25
    for (BoustrophedonCell::BoustrophedonCellSetIterator itn = major_cell.neighbors_.begin(); itn != major_cell.neighbors_.end(); ++itn)
26
        if ((*itn)->label_ == minor_cell.label_)
27
        {
28
            major_cell.neighbors_.erase(itn);
29
            break;
30
        }
31
    for (BoustrophedonCell::BoustrophedonCellSetIterator itn = minor_cell.neighbors_.begin(); itn != minor_cell.neighbors_.end(); ++itn)
32
        if ((*itn)->label_ != major_cell.label_)
33
            major_cell.neighbors_.insert(*itn);
34

35
    // clean all references to minor_cell
36
    cell_index_mapping.erase(minor_cell.label_);
37
    for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
38
        for (BoustrophedonCell::BoustrophedonCellSetIterator itn = itc->second->neighbors_.begin(); itn != itc->second->neighbors_.end(); ++itn)
39
            if ((*itn)->label_ == minor_cell.label_)
40
            {
41
                (*itn)->label_ = major_cell.label_;
42
                break;
43
            }
44
}

这部分太长了，是真的不想看，反正经过合并后变成了这样，一些细小的房间被处理掉了：

两张放在一起对比看，可以明显的看出部分区域进行了合并，调节min_cell_area可以改变合并的效果


合并前	合并后

2.4 处理单元格

1
std::vector<std::vector<cv::Point> > cells;
2
for (int i=1; i<=number_of_cells; ++i)
3
{
4
    cv::Mat cell_copy(cell_map_labels == i);
5
    std::vector<std::vector<cv::Point> > cellsi;
6
    cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
7
    cells.insert(cells.end(), cellsi.begin(), cellsi.end());
8
}

对于每个单元格标签，创建一个副本cell_copy，然后使用cv::findContours函数找到该单元格的轮廓。这些轮廓代表了单元格的边界。

1
// create generalized Polygons out of the contours to handle the cells
2
for(size_t cell=0; cell<cells.size(); ++cell)
3
{
4
    GeneralizedPolygon current_cell(cells[cell], map_resolution);
5
    if(current_cell.getArea()>=min_cell_area)
6
    {
7
        cell_polygons.push_back(current_cell);
8
        polygon_centers.push_back(current_cell.getCenter());
9
    }
10
    else
11
    {
12
        std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
13
    }
14
}

对于每个找到的轮廓，创建一个GeneralizedPolygon对象，该对象基于轮廓和地图分辨率。然后检查每个多边形的面积是否满足最小面积要求。如果满足，将其添加到cell_polygons中；如果不满足，打印警告信息。

Step 3 绘制覆盖路径

1
// *********************** IV. Determine the cell paths. ***********************
2
// determine the start cell that contains the start position
3
std::vector<cv::Point> starting_point_vector(1, starting_position); // opencv syntax
4
cv::transform(starting_point_vector, starting_point_vector, R);
5
const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell
6

7
int start_cell_index = 0;
8
for(std::vector<GeneralizedPolygon>::iterator cell=cell_polygons.begin(); cell!=cell_polygons.end(); ++cell)
9
    if(cv::pointPolygonTest(cell->getVertices(), rotated_starting_point, false) >= 0)
10
    start_cell_index = cell - cell_polygons.begin();
11

12
// determine the visiting order of the cells
13
std::vector<int> optimal_order;
14
if (cell_visiting_order == OPTIMAL_TSP)
15
{
16
    // determine the optimal visiting order of the cells
17
    GeneticTSPSolver tsp_solver;
18
    optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
19
    if (optimal_order.size()!=polygon_centers.size())
20
    {
21
        std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl;
22
        optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
23
    }
24
}
25
else if (cell_visiting_order == LEFT_TO_RIGHT)
26
{
27
    // we are using an alternative ordering here, which visits the cells in a more obvious fashion to the human observer (though it is not optimal)
28
    std::multimap<int, int> y_coordinate_ordering;        // <y-coordinate of room centers, index>
29
    for (size_t i=0; i<polygon_centers.size(); ++i)
30
        y_coordinate_ordering.insert(std::pair<int, int>(polygon_centers[i].y, (int)i));
31
    for (std::multimap<int,int>::iterator it=y_coordinate_ordering.begin(); it!=y_coordinate_ordering.end(); ++it)
32
        optimal_order.push_back(it->second);
33
}
34
else
35
{
36
    std::cout << "Error: BoustrophedonExplorer::getExplorationPath: The specified cell_visiting_order=" << cell_visiting_order << " is invalid." << std::endl;
37
    return;
38
}
39

40
// go trough the cells [in optimal visiting order] and determine the boustrophedon paths
41
ROS_INFO("Starting to get the paths for each cell, number of cells: %d", (int)cell_polygons.size());
42
std::cout << "Boustrophedon grid_spacing_as_int=" << grid_spacing_as_int << std::endl;
43
cv::Point robot_pos = rotated_starting_point;   // point that keeps track of the last point after the boustrophedon path in each cell
44
std::vector<cv::Point2f> fov_middlepoint_path;    // this is the trajectory of centers of the robot footprint or the field of view
45
for(size_t cell=0; cell<cell_polygons.size(); ++cell)
46
{
47
    computeBoustrophedonPath(rotated_room_map, map_resolution, cell_polygons[optimal_order[cell]], fov_middlepoint_path,
48
                             robot_pos, grid_spacing_as_int, half_grid_spacing_as_int, path_eps, max_deviation_from_track, grid_obstacle_offset/map_resolution);
49
}
50

51
// transform the calculated path back to the originally rotated map and create poses with an angle
52
RoomRotator room_rotation;
53
std::vector<geometry_msgs::Pose2D> fov_poses; // this is the trajectory of poses of the robot footprint or the field of view, in [pixels]
54
room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, fov_poses, R);
55

56
ROS_INFO("Found the cell paths.");
57

58
// if the path should be planned for the robot footprint create the path and return here
59
if (plan_for_footprint == true)
60
{
61
    for(std::vector<geometry_msgs::Pose2D>::iterator pose=fov_poses.begin(); pose != fov_poses.end(); ++pose)
62
    {
63
        geometry_msgs::Pose2D current_pose;
64
        current_pose.x = (pose->x * map_resolution) + map_origin.x;
65
        current_pose.y = (pose->y * map_resolution) + map_origin.y;
66
        current_pose.theta = pose->theta;
67
        path.push_back(current_pose);
68
    }
69
    return;
70
}

这一部分的作用就是在规划路径，使其能遍历所有的cell，路径生成主要在computeBoustrophedonPath中进行，前置部分通过将各个单元格（cell）的访问顺序建模为一个Tsp问题，并通过遗传算法解决，也就是GeneticTSPSolver tsp_solver部分，最后根据plan_for_footprint参数判断是否将路径转化为实际地图坐标

这一部分我们先不去关心cell的访问顺序，主要看覆盖路径是怎么生成的，也就是computeBoustrophedonPath的实现，这也是覆盖算法的核心

牛耕分解+形态学分割全覆盖路径规划（二）Part1. 分割

Fri, 13 Sep 2024 01:59:02 GMT

书接上文：牛耕分解+形态学分割全覆盖路径规划（一）前置文章1：房屋区域分割算法 Morphological Segmentation 前置文章2：牛耕覆盖算法 Boustrophedon Coverage Path Planning 项目地址：ipa320 / ipa_coverage_planning

由于博主先找的是覆盖然后再找分区，所以针对的是ipa_coverage_planning项目进行研究，不排除有其他更好的方法或者已经优化过的方法，在后续的学习中会持续补充。由于本文章在wordpress中编辑，部分代码粘贴会出现bug，所以建议下载原项目看

在上文中，我们将形态学分割和牛耕规划程序进行合并，将分割后的各个房间进行顺序规划，每个房间的规划用A*连接在一起。在本篇章中，我将着重对代码进行分析和调试，以及将多个用到的库进行剪切合并。 Part 1. morphological segmentation 形态学分割 源码：

1
void MorphologicalSegmentation::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription,
2
        double room_area_factor_lower_limit, double room_area_factor_upper_limit)
3
{
4
  /*This segmentation algorithm does:
5
   * 1. collect the map data
6
   * 2. erode the map to extract contours
7
   * 3. find the extracted contures and save them if they fullfill the room-area criterion
8
   * 4. draw and fill the saved contoures in a clone of the map from 1. with a random colour
9
   * 5. get the obstacle information from the original map and draw them in the clone from 4.
10
   * 6. spread the coloured regions to the white Pixels
11
   */
12

13
  //make two map clones to work with
14
  cv::Mat temporary_map_to_find_rooms = map_to_be_labeled.clone(); //map to find the rooms and for eroding
15
  //**************erode temporary_map until last possible room found****************
16
  //erode map a specified amount of times
17
        std::vector<std::vector<cv::Point>> saved_contours;//saving variable for every contour that is between the upper and the lower limit
18
  ROS_INFO("starting eroding");
19
  for (int counter = 0; counter < 73; counter++)
20
  {
21
    //erode the map one time
22
    cv::Mat eroded_map;
23
    cv::Point anchor(-1, -1); //needed for opencv erode
24
    cv::erode(temporary_map_to_find_rooms, eroded_map, cv::Mat(), anchor, 1);
25
    //save the more eroded map
26
    temporary_map_to_find_rooms = eroded_map;
27
    //Save the eroded map in a second map, which is used to find the contours. This is neccesarry, because
28
    //the function findContours changes the given map and would make it impossible to work any further with it
29
    cv::Mat contour_map = eroded_map.clone();
30
    //find Contours in the more eroded map
31
    std::vector < std::vector > temporary_contours; //temporary saving-variable
32
    //hierarchy saves if the contours are hole-contours:
33
    //hierarchy[{0,1,2,3}]={next contour (same level), previous contour (same level), child contour, parent contour}
34
    //child-contour = 1 if it has one, = -1 if not, same for parent_contour
35
    std::vector < cv::Vec4i > hierarchy;
36
#if CV_MAJOR_VERSION<=3
37
    cv::findContours(contour_map, temporary_contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
38
#else
39
    cv::findContours(contour_map, temporary_contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);
40
#endif
41
    if (temporary_contours.size() != 0)
42
    {
43
      //check every contour if it fullfills the criteria of a room
44
      for (int current_contour = 0; current_contour < temporary_contours.size(); current_contour++) { //only take first level contours --> second level contours belong to holes and doesn't need to be looked at
45
        if (hierarchy[current_contour][3] == -1)
46
        {
47
          //check if contour is large/small enough for a room
48
          double room_area = map_resolution_from_subscription * map_resolution_from_subscription
49
                  * cv::contourArea(temporary_contours[current_contour]);
50
          //subtract the area from the hole contours inside the found contour, because the contour area grows extremly large if it is a closed loop
51
          for (int hole = 0; hole < temporary_contours.size(); hole++)
52
          {
53
            if (hierarchy[hole][3] == current_contour) //check if the parent of the hole is the current looked at contour
54
            {
55
              room_area -= map_resolution_from_subscription * map_resolution_from_subscription
56
                  * cv::contourArea(temporary_contours[hole]);
57
            }
58
          }
59
          if (room_area_factor_lower_limit < room_area && room_area < room_area_factor_upper_limit) { //save contour for later drawing in map saved_contours.push_back(temporary_contours[current_contour]); //make region black if room found --> region doesn't need to be looked at anymore
60
#if CV_MAJOR_VERSION<=3
61
            cv::drawContours(temporary_map_to_find_rooms, temporary_contours, current_contour, cv::Scalar(0), CV_FILLED, 8, hierarchy, 2);
62
#else
63
            cv::drawContours(temporary_map_to_find_rooms, temporary_contours, current_contour, cv::Scalar(0), cv::FILLED, 8, hierarchy, 2);
64
#endif
65
          }
66
        }
67
      }
68
    }
69
  }
70
  //*******************draw contures in new map***********************
71
  std::cout << "Segmentation Found " << saved_contours.size() << " rooms." << std::endl;
72
  //draw filled contoures in new_map_to_draw_contours_ with random colour if this colour hasn't been used yet
73
  cv::Mat new_map_to_draw_contours; //map for drawing the found contours
74
  map_to_be_labeled.convertTo(segmented_map, CV_32SC1, 256, 0);
75
  std::vector < cv::Scalar > already_used_coloures; //vector for saving the already used coloures
76
  for (int idx = 0; idx < saved_contours.size(); idx++) { bool drawn = false; //checking-variable if contour has been drawn int draw_counter = 0; //counter to exit loop if it gets into an endless-loop (e.g. when there are more rooms than possible) do { draw_counter++; cv::Scalar fill_colour(rand() % 52224 + 13056); if (!contains(already_used_coloures, fill_colour) || draw_counter > 250)
77
      {
78
        //if colour is unique draw Contour in map
79
#if CV_MAJOR_VERSION<=3
80
        cv::drawContours(segmented_map, saved_contours, idx, fill_colour, CV_FILLED);
81
#else
82
        cv::drawContours(segmented_map, saved_contours, idx, fill_colour, cv::FILLED);
83
#endif
84
        already_used_coloures.push_back(fill_colour); //add colour to used coloures
85
        drawn = true;
86
      }
87
    } while (!drawn);
88
  }
89
  //*************************obstacles***********************
90
  //get obstacle informations and draw them into the new map
91
  ROS_INFO("starting getting obstacle information");
92
  for (int row = 0; row < map_to_be_labeled.rows; ++row)
93
  {
94
    for (int col = 0; col < map_to_be_labeled.cols; ++col)
95
    {
96
      //find obstacles = black pixels
97
      if (map_to_be_labeled.at(row, col) == 0)
98
      {
99
        segmented_map.at(row, col) = 0;
100
      }
101
    }
102
  }
103
  ROS_INFO("drawn obstacles in map");
104
  //**************spread the colored region by making white pixel around a contour their color****************
105
  //spread the coloured regions to the white Pixels
106
  wavefrontRegionGrowing(segmented_map);
107
  ROS_INFO("filled white pixels in new map");
108

109
}

整个算法分为6步操作（差不多吧，自己按感觉分的），下面将逐步进行测试和解析。 Step 1. 腐蚀地图

1
ROS_INFO("started Testing");
2
cv::Mat Mattemporary_map_to_find_rooms_test = temporary_map_to_find_rooms.clone();
3
size_t test_times = 1;
4
for (int counter = 0; counter < test_times; counter++)
5
{
6
  cv::Mat eroded_map_test;
7
  cv::Point anchor_test(-1, -1);
8
  cv::erode(Mattemporary_map_to_find_rooms_test, eroded_map_test, cv::Mat(), anchor_test, 1);
9

10
  Mattemporary_map_to_find_rooms_test = eroded_map_test;
11
}
12
cv::imshow("Mattemporary_map_to_find_rooms_test",Mattemporary_map_to_find_rooms_test);

创建一些测试的数据，腐蚀 test_times 次看看腐蚀的到的效果是什么样的（由于很多像我一样平时用不到opencv，所以慢一点来看），从下图中我们可以看出来，腐蚀后黑色区域向外/内蔓延了，图左为原图，图右为腐蚀一次的图（这也是常用的保证安全距离的方式） Step2. 提取轮廓

1
// find Contours in map
2
cv::Mat contour_map_test = Mattemporary_map_to_find_rooms_test.clone();
3
std::vector<std::vector<cv::Point>> temporary_contours_test;
4
std::vector<cv::Vec4i> hierarchy_test;
5
cv::findContours(contour_map_test, temporary_contours_test, hierarchy_test, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);
6
// display
7
cv::Mat display_map = cv::Mat::ones(temporary_map_to_find_rooms.size(), temporary_map_to_find_rooms.type()) * 255;
8
cv::drawContours(display_map, temporary_contours_test, -1, cv::Scalar(0, 0, 0), 1);
9
cv::imshow("Contours", display_map);

可能很多人和我一样对于轮廓提取这一部分没有直观的认知。使用opencv的findContours进行轮廓提取，得到的是二维的Point点集，将其在空白图像上可视化，操作对应于上述代码，左图为腐蚀一次后的图，右图为提取的轮廓：在这一部分比较难理解的是 hierarchy ，我在这里借助GPT解释一下：

hierarchy 是一个向量，用于存储轮廓的层次结构信息。在OpenCV中，轮廓可以有不同的层次关系，例如，一个轮廓可能是另一个轮廓的子轮廓（内部轮廓），或者两个轮廓可能是平行的（同一层级）。hierarchy 向量中的每个元素都是一个 cv::Vec4i 类型，包含四个整数，分别表示：

Next Contour (同一层级的下一个轮廓)：这个值指向同一层级的下一个轮廓。如果没有下一个轮廓，则为-1。

Previous Contour (同一层级的上一个轮廓)：这个值指向同一层级的上一个轮廓。如果没有上一个轮廓，则为-1。

First Child (第一个子轮廓)：这个值指向当前轮廓的第一个子轮廓。如果当前轮廓没有子轮廓，则为-1。

Parent Contour (父轮廓)：这个值指向当前轮廓的父轮廓。如果当前轮廓没有父轮廓，则为-1。

通过这种方式，hierarchy 向量提供了一种方法来理解和操作图像中轮廓之间的复杂关系。例如，你可以使用这些信息来区分和处理图像中的主要物体和它们的内部空洞，或者来识别和分离相邻但独立的物体。

我们尝试输出一下 temporary_contours ：

1
for (size_t i = 0; i < hierarchy_test.size(); ++i) {
2
  std::cout << "Hierarchy " << i << ": ";
3
  for (int j = 0; j < 4; ++j) {
4
    std::cout << hierarchy_test[i][j] << " ";
5
  }
6
  std::cout << std::endl;
7
}

结果为：

1
Hierarchy 0: -1 -1 1 -1
2
Hierarchy 1: -1 -1 -1 0

根据上述结果，可以看到检测出两个轮廓，轮廓1是父轮廓，轮廓2是子轮廓其父轮廓是轮廓1（大概是图中的那个小轮廓吧） Step3. 寻找房间

1
if (temporary_contours_test.size() != 0)
2
{
3
  for (int current_contour = 0; current_contour < temporary_contours_test.size(); current_contour++)
4
  {
5
    if (hierarchy_test[current_contour][3] == -1)
6
    {
7
      double room_area = map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(temporary_contours_test[current_contour]);
8
      for (int hole = 0; hole < temporary_contours_test.size(); hole++)
9
        if (hierarchy_test[hole][3] == current_contour)
10
          room_area -= map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(temporary_contours_test[hole]);
11
      if (room_area_factor_lower_limit < room_area && room_area < room_area_factor_upper_limit)
12
      {
13
        saved_contours.push_back(temporary_contours_test[current_contour]);
14
        cv::drawContours(Mattemporary_map_to_find_rooms_test, temporary_contours_test, current_contour, cv::Scalar(0), cv::FILLED, 8, hierarchy_test, 2);
15
      }
16
    }
17
  }
18
}

根据轮廓找房间的逻辑大概对应于上述代码的几个步骤：

1. 遍历每一个父轮廓，利用OpenCV的 contourArea 函数计算该轮廓的面积
1. 删除父轮廓中的子轮廓的面积（障碍物）
1. 如果除去子轮廓的面积符合开始设定的面积范围，将该轮廓保存在数组中，将地图中该轮廓范围填充（置为不可到达）

由于腐蚀 1 次无法看到效果，我们尝试多次腐蚀然后找一个房间，将腐蚀次数变为7，查看效果，左上为原图，右上为腐蚀5次，左下为提取的轮廓，右下为填充查找到房间后的地图：可以看到经过五次腐蚀，左上的房间独立了出来，且轮廓的面积达到了房间面积的要求，将该轮廓记录，并清除该房间。随后用右下的地图继续查找。 Step4. 回到Step1重复腐蚀并查找执行腐蚀的次数是认为设定的，源码中设定了73次，我认为实际可能不需要这么多次，不过多执行几次并没有坏处 Step5. 填充轮廓

1
map_to_be_labeled.convertTo(segmented_map, CV_32SC1, 256, 0);
2
std::vector<cv::Scalar> already_used_coloures_test;
3
for (int idx = 0; idx < saved_contours.size(); idx++)
4
{
5
    bool drawn = false;   // checking-variable if contour has been drawn
6
    int draw_counter = 0; // counter to exit loop if it gets into an endless-loop (e.g. when there are more rooms than possible)
7
    do {
8
        draw_counter++;
9
        cv::Scalar fill_colour(rand() % 52224 + 13056);
10
        if (!contains(already_used_coloures_test, fill_colour) || draw_counter > 250){
11
            cv::drawContours(segmented_map, saved_contours, idx, fill_colour, cv::FILLED);
12
            already_used_coloures_test.push_back(fill_colour); // add colour to used coloures
13
            drawn = true;
14
        }
15
    } while (!drawn);
16
}
17
for (int row = 0; row < map_to_be_labeled.rows; ++row)
18
{
19
  for (int col = 0; col < map_to_be_labeled.cols; ++col)
20
  {
21
    if (map_to_be_labeled.at(row, col) == 0)
22
    {
23
      segmented_map.at(row, col) = 0;
24
    }
25
  }
26
}
27
cv::Mat display_map_test;
28
segmented_map.convertTo(display_map_test,CV_8U);
29

30
cv::imshow("filled", display_map_test);

对照上面的找到一个房间后填充，填充后补充原图中不可到达区域，效果如下图，虽然能看出这个房间，但是填充的并不满，所以有后续操作 step6. 扩散填充

1
wavefrontRegionGrowing(segmented_map);

这一步操作是将彩色区域填充至白色区域，调用了wavefront_region_growing.cpp的函数，详细看一下wavefrontRegionGrowing是怎么实现的

1
// spreading image is supposed to be of type CV_32SC1
2
void wavefrontRegionGrowing(cv::Mat& image)
3
{
4
  //This function spreads the colored regions of the given map to the neighboring white pixels
5
  if (image.type()!=CV_32SC1)
6
  {
7
    std::cout << "Error: wavefrontRegionGrowing: provided image is not of type CV_32SC1." << std::endl;
8
    return;
9
  }
10

11
  cv::Mat spreading_map = image.clone();
12
  bool finished = false;
13
  while (finished == false)
14
  {
15
    finished = true;
16
    for (int row = 1; row < spreading_map.rows-1; ++row)
17
    {
18
      for (int column = 1; column < spreading_map.cols-1; ++column)
19
      {
20
        if (spreading_map.at(row, column) > 65279)    // unassigned pixels
21
        {
22
          //check 3x3 area around white pixel for fillcolour, if filled Pixel around fill white pixel with that colour
23
          bool set_value = false;
24
          for (int row_counter = -1; row_counter <= 1 && set_value==false; ++row_counter)
25
          {
26
            for (int column_counter = -1; column_counter <= 1 && set_value==false; ++column_counter)
27
            {
28
              int value = image.at<int>(row + row_counter, column + column_counter);
29
              if (value != 0 && value <= 65279)
30
              {
31
                spreading_map.at<int>(row, column) = value;
32
                set_value = true;
33
                finished = false;  // keep on iterating the wavefront propagation until no more changes occur
34
              }
35
            }
36
          }
37
        }
38
      }
39
    }
40
    image = spreading_map.clone();
41
  }
42
}

该操作主要包含以下几个步骤：

检查输入数据类型，地图类型必须为CV_32SC1
遍历图像像素，如果像素未被分配（像素值大于 65279），则检查周围3*3范围
如果3*3范围内存在色彩像素，则设置为该值，set_value用于标记是否修改该像素的值
不停的进行着色，直到没有未被分配的色彩为止（finished = true）

最终完成区域的填充完整的房间分割后的图：

结尾总结： 至此，图像的分割完成了，但是这样的填充方法有缺陷，在得到的图中也可以很明显的看出来，由于填充顺序是从左上到右下遍历，所以房间可能会存在左边缺少右边突出的现象。思考一下优化方向：① 针对独立房间的方法，ipa项目中采用的是不断的腐蚀并提取轮廓，是否能够改进； ② 针对填充房间，ipa项目中采用的是对像素点周围3*3范围进行查找，是否能优化

牛耕分解+形态学分割全覆盖路径规划（一）

Wed, 11 Sep 2024 02:08:08 GMT

前置文章：房屋区域分割算法 Morphological Segmentation 牛耕覆盖算法 Boustrophedon Coverage Path Planning 2024/9/11 由于对整个房间直接进行牛耕覆盖看起来效果不太好，而且也不符合扫地机器人的实际清扫过程，考虑先进行分区，然后对于单独房间分别进行覆盖；由于采用先分区后覆盖的思路，所以我们需要考虑两个算法的输入输出条件，以及满足我们需求的逻辑： 1、从牛耕的输入出发，需要的重要的参数是：

1、room_map ：地图
2、starting_position_ ：起始坐标

针对地图，如果我们想一个房间一个房间进行规划的话，那么最好的就是输入的地图为单独的房间？单独的房间？如何处理？回想分区的结果，我们可以得到些什么：

1、带有标签的地图，所谓的标签，就是在原地图的基础上，房间被赋值为顺序的标号，也就是indexed_map
2、房间的中心点坐标（room_centers_x_values，room_centers_y_values），以及房间的四个顶点的坐标（xmin,xmax,ymin,ymax）

如果有这些数据的话，是不是可以考虑每遍历一个房间，将其余的房间全部填充0（不可到达），然后规划路径；ummmmmm显然不太好，不去考虑填充时计算量的庞大，最后生成的各个房间的路径该如何连接在一起？有了上面的思考，思路变得清晰了，这里我们需要考虑两个问题：

1.如何单个房间进行牛耕覆盖，输入的地图应该是什么样的
2.两个房间的路径该如何连接起来

继续思考，先不去想输入的地图的格式，把问题转到starting_position_ 也就是起始坐标上，因为输入地图有笨办法（对标签地图进行处理，非当前房间置0）只需考虑优化，那么房间的起点该如何确定？（为什么不用考虑房间的终点呢？因为两个房间连接的话只需要两个点就好，新房间的起点和旧房间的终点，终点就是覆盖路径的最后一个点可以由算法直接给出。有这两个点之后来一个A*就可以连接两个房间）【补充：在做完之后才发现房间的终点也是十分重要的】由于比较容易得到每个房间的边界点的坐标，同时也可以根据这个坐标得到房间的中心点（代码中考虑了机器人能否到达）那么我们是否可以以设定的机器人的初始点到第一个房间的中心点生成一条路径（A*），然后随着路径寻找，属于房间范围的第一个点即为房间的初始点，理论可行！开始实践！ **Step 1 ：**确定机器人初始坐标点，由于不知道放哪比较好，毕竟实际的初始坐标是根据基站或者说开机的位置确定的，所以这里设定为整个地图的最中心：

1
starting_position_.x = msg.info.origin.position.x +  msg.info.width * 0.5;
2
starting_position_.y = msg.info.origin.position.y +  msg.info.height * 0.5;

**Step2 ：**了解牛耕覆盖调用的A*库（因为A*都差不多，不想去修改牛耕的代码，就直接拿它的A*了），并使用A*库去完成两点生成路径；

1
int index_room = 7;
2

3
cv::Point start_point = starting_position_;
4
cv::Point end_point(room_centers_x_values[index_room], room_centers_y_values[index_room]);
5
std::vector astar_path;
6

7
cv::Point start_point2;
8
if (start_point.x >= min_x_value_of_the_room[index_room] &&
9
    start_point.x <= max_x_value_of_the_room[index_room] && start_point.y >= min_y_value_of_the_room[index_room] &&
10
    start_point.y <= max_y_value_of_the_room[index_room])
11
{
12
    start_point2 = start_point;
13
}
14
else
15
{
16
    double path_length = 0.0;
17
    path_length = path_planner_.planPath(original_map, start_point, end_point, 1.0, 0.0, map_resolution_, robot_radius_, &astar_path);
18
    for (size_t i = 0; i < astar_path.size(); ++i) { // 检查点是否在房间的边界内 if (astar_path[i].x >= min_x_value_of_the_room[index_room] &&
19
            astar_path[i].x <= max_x_value_of_the_room[index_room] && astar_path[i].y >= min_y_value_of_the_room[index_room] &&
20
            astar_path[i].y <= max_y_value_of_the_room[index_room])
21
        {
22
            start_point2 = astar_path[i];
23
            astar_path.resize(i + 1);
24
            break;
25
        }
26
    }
27
}

**step3：**将生成的start_point2点作为牛耕的起点，输入地图修改为除了该房间其余都为不可到达区域的地图，下图为单个房间的路径输出可以看到效果并不好，因为我在从index_map转为room_map的时候直接将original_map的数据传入room_map中，实际效果并不好，应该先进行二值处理 **step4：**将A*的路径插入到该路径中尝试连续两个房间的覆盖下图是将房间进行二值处理后规划的路径；根据生成中的效果可以发现两个问题：

1、相邻两个房间连接的时候回穿墙，在单个房间规划的时候也存在穿墙现象；分析为牛耕带的A*库并不好用，下一步想办法替换为自己的A*库
2、房间0也就是右上角的房间会出问题，当我尝试访问房间0的时候就会出错，目前没找到导致问题出现的地点

2024/9/13 解决bug2，由于index_map的标签从1开始，但是room_centers_x_values从0开始，所以在room_map生成的时候应该判断 indexed_map.at<int>(x, y) == index_room + 1 如下：

1
cv::Mat room_map = original_map.clone();
2
for (int x = 0; x < indexed_map.rows; ++x)
3
{
4
    for (int y = 0; y < indexed_map.cols; ++y)
5
    {
6
        if (indexed_map.at(x, y) == index_room + 1)
7
            room_map.at(x, y) = 255;
8
        else
9
            room_map.at(x, y) = 0;
10
    }
11
}

bug1问题：A*改了半天的结果，用自己的A*视觉效果比较差（但实际路径还可以）自己的A*库的结果: 牛耕自带的A*库的结果： ！！！补充bug1：找到问题所在了，① 我用A*去找下一个区域的中点，然后到达该区域的第一个点作为区域的起点这里，如何判定到达该区域出现了问题，我直接设定路径上的点满足区域范围是不对的，应该还包括该区域的标签值是该区域的标签（说的比较绕）这个条件；② 由于index_map与astar输出坐标相反，所以在判断是否满足区域的时候应该选取index_map(j,i)修改代码：

1
if (astar_path[i].x >= min_x_value_of_the_room[index_room] &&
2
    astar_path[i].x <= max_x_value_of_the_room[index_room] && astar_path[i].y >= min_y_value_of_the_room[index_room] &&
3
    astar_path[i].y <= max_y_value_of_the_room[index_room] &&
4
    indexed_map.at(astar_path[i].y, astar_path[i].x) == (index_room + 1))
5
{
6
    room_begin_point = astar_path[i];
7
    astar_path.resize(i + 1);
8
    break;
9
}

实现效果：可以看出不同房间之间的穿墙变得少了。但在单个房间内覆盖还是存在穿墙现象

使用改进的牛耕分解：

1
//boustrophedon_explorer_.getExplorationPath(room_map, oneRoom_exploration_path, map_resolution_, room_begin_point, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);
2
boustrophedon_variant_explorer_.getExplorationPath(room_map, oneRoom_exploration_path, map_resolution_, room_begin_point, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);

效果：（有优化吗，好像看不太出来）设置 min_cell_area = 2，grid_obstacle_offset = 0.1，首先，这里的前提是我在进行牛耕前并没有对地图进行腐蚀和膨胀，所以并没有对安全距离进行考究；所以设置grid_obstacle_offset = 0.1 保持边界的安全距离，这样也为后续延边的路径留出空间，防止重复规划，效果：

2024/9/13 待解决问题：牛耕覆盖存在的路径穿墙问题，可能是：

① ros发布的是path，所以两个点之间直接用直线连接了起来，并不是实际规划的路径
② 牛耕使用的A*存在问题，可能地图传入的时候出现了问题

验证情况①：利用ros中的visualization_msgs::Marker将exploration_path发布为点集：

1
visualization_msgs::Marker points;
2
points.header.frame_id = "map"; // 使用你所在的坐标系
3
points.header.stamp = ros::Time::now();
4
points.ns = "exploration_points";
5
points.id = 0;
6
points.type = visualization_msgs::Marker::POINTS;
7
points.action = visualization_msgs::Marker::ADD;
8

9
// 设置点的尺寸
10
points.scale.x = 0.05; // 设置点的宽度
11
points.scale.y = 0.05; // 设置点的高度
12
points.color.r = 1.0f;
13
points.color.g = 0.0f;
14
points.color.b = 0.0f;
15
points.color.a = 1.0f;
16

17
// 将路径上的点添加到Marker中
18
for (const auto &pose : exploration_path)
19
{
20
    geometry_msgs::Point p;
21
    p.x = pose.x;
22
    p.y = pose.y;
23
    p.z = 0.0; // z 轴可以保持为 0，如果你的路径在平面上
24
    points.points.push_back(p);
25
}
26

27
// 发布Marker
28
marker_pub.publish(points);

没太多特殊的地方，记得在CMake中添加visualization_msgs就好，最终效果如下：和牛耕源码中描述的差不多，牛耕规划器会根据min_cell_area来划分单元格，然后每个单元格单独进行规划（单元格不会连接在一起），所以在先前的path中就会看起来有穿墙，其实是不同区域的点集连接。消除房间之间的路径，将房间内的规划看的更清晰：

至此，牛耕和分区的配合完成了，由于是直接使用分区算法和牛耕覆盖，所以对算法的实现并没有太多的关注，接下来将关注算法的实现，尽可能的熟悉算法从而在项目中快速使用。

牛耕覆盖算法 Boustrophedon Coverage Path Planning

Tue, 10 Sep 2024 07:24:00 GMT

牛耕覆盖路径算法 [ BCPP ]#

实现效果： 覆盖算法所需前置数据：

map_origin：地图原点，表示地图左上角的位置。
map_resolution：地图分辨率，即每个单元格代表的实际距离（米/单元格）。
robot_radius：机器人半径，表示机器人占据的空间大小。
starting_position：起始位置，转换为像素坐标。
planning_mode：规划模式，选择FOV还是FOOTPRINT，也就是按照视野面积还是机器人移动面积覆盖

对地图的前置处理：

获取地图和基本参数
计算房间大小（平方米）
使用腐蚀和膨胀修正地图
移除不连通的区域
确定网格大小

牛耕覆盖算法所需参数（除基础参数外）：

min_cell_area：这个参数定义了网格中每个单元格的最小面积
path_eps：这个参数定义了路径规划中的容错率。它可以用来调整路径规划的精度，例如，较小的值可以导致更精确但可能更复杂的路径。
grid_obstacle_offset：这个参数定义了网格中障碍物的偏移量。它可以用来调整障碍物在网格中的表示，以便更好地适应实际环境中障碍物的大小和形状。
max_deviation_from_track：这个参数定义了机器人在跟踪探索路径时允许的最大偏差。如果设置为负值，则表示没有偏差限制。
cell_visiting_order：这个参数定义了访问网格单元格的顺序。它可以用来控制探索算法的行为，例如，按照特定的顺序（如螺旋或随机）访问单元格。

房屋区域分割算法 Morphological Segmentation

Mon, 09 Sep 2024 09:58:41 GMT

Morphological Segmentation 形态分割算法的优点是：实现简单和计算速度快算法实现效果：算法实现大致描述：

步骤1：创建地图的副本，用于形态学操作。
步骤2：通过多次腐蚀操作来提取地图上的轮廓。
步骤3：找到腐蚀后的地图中的轮廓，并检查这些轮廓是否满足房间面积的标准。
步骤4：在原始地图的副本上绘制并填充符合条件的轮廓。
步骤5：从原始地图中提取障碍物信息，并在步骤4中创建的地图副本上绘制这些障碍物。
步骤6：将彩色区域扩展到白色像素，以完成分割。

主要对应函数

1
void MorphologicalSegmentation::segmentMap(const cv::Mat& map_to_be_labeled, cv::Mat& segmented_map, double map_resolution_from_subscription,double room_area_factor_lower_limit, double room_area_factor_upper_limit)

关键代码1：对当前地图进行腐蚀操作，temporary_map_to_find_rooms为输入的待处理地图，在每一次腐蚀操作后更新

1
cv::Mat eroded_map;
2
cv::Point anchor(-1, -1); //needed for opencv erode
3
cv::erode(temporary_map_to_find_rooms, eroded_map, cv::Mat(), anchor, 1);

关键代码2：在腐蚀后的地图上找到轮廓； temporary_contours：用于临时存储找到的轮廓。每个轮廓由一系列点（cv::Point）组成。 hierarchy：用于存储轮廓之间的层次关系。这包括每个轮廓的父轮廓、子轮廓、同级轮廓等信息。 CV_RETR_CCOMP参数指定检索模式，这里用于检索所有的轮廓并分类为外轮廓和内轮廓。 CV_CHAIN_APPROX_SIMPLE参数指定轮廓点的近似方法，这里用于压缩水平、垂直和对角线段。

1
cv::Mat contour_map = eroded_map.clone();
2
std::vector < std::vector > temporary_contours; //temporary saving-variable
3
std::vector < cv::Vec4i > hierarchy;
4
cv::findContours(contour_map, temporary_contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

关键代码3：找到轮廓后识别出符合特定条件的房间检查轮廓数量：首先检查temporary_contours中是否有轮廓。如果没有，就不需要进一步处理。遍历每个轮廓：对于temporary_contours中的每个轮廓，检查它是否满足作为房间的条件。层次关系检查：使用hierarchy数组来确定当前轮廓是否是第一层轮廓（即主轮廓）。如果当前轮廓是第一层轮廓（hierarchy[current_contour][3] == -1），则继续处理。计算轮廓面积：计算当前轮廓的面积，并考虑内部空洞（如果有）的面积。这是通过遍历所有轮廓并减去属于当前轮廓的内部空洞的面积来实现的。面积判断：根据预设的面积限制（room_area_factor_lower_limit和room_area_factor_upper_limit），判断当前轮廓是否可以被视为一个房间。保存和标记轮廓：如果当前轮廓满足房间的条件，将其保存到saved_contours中，并在原始地图上标记该轮廓（使用cv::drawContours）。

1
if (temporary_contours.size() != 0)
2
{
3
    for (int current_contour = 0; current_contour < temporary_contours.size(); current_contour++)
4
    {
5
  if (hierarchy[current_contour][3] == -1)
6
  {
7
      double room_area = map_resolution_from_subscription * map_resolution_from_subscription* cv::contourArea(temporary_contours[current_contour]);
8
      for (int hole = 0; hole < temporary_contours.size(); hole++)
9
      {
10
          if (hierarchy[hole][3] == current_contour)
11
    {
12
        room_area -= map_resolution_from_subscription * map_resolution_from_subscription * cv::contourArea(temporary_contours[hole]);
13
    }
14
      }
15
      if (room_area_factor_lower_limit < room_area && room_area < room_area_factor_upper_limit)
16
      {
17
                saved_contours.push_back(temporary_contours[current_contour]);
18
          cv::drawContours(temporary_map_to_find_rooms, temporary_contours, current_contour, cv::Scalar(0), CV_FILLED, 8, hierarchy, 2);
19
            }
20
  }
21
    }
22
}

main中需要理解的部分：补充：理解这部分了，就是说本身分割后的地图segmented_map由label构成，但是我现在创建一个indexed_map，存储和地图大小一样，但是每个地图位置上的值是房间编号（1-房间总数），需要配合label_vector_index_codebook生成函数进行使用；

1
cv::Mat indexed_map = segmented_map.clone();
2
for (int x = 0; x < segmented_map.rows; ++x)
3
{
4
   for (int y = 0; y < segmented_map.cols; ++y)
5
   {
6
      const int label = segmented_map.at(x, y);
7
      if (label > 0 && label < 65280)
8
         indexed_map.at(x, y) = label_vector_index_codebook[label] + 1;
9
    }
10
}

后续需要完成的工作 / 思考的内容：

√ 如何在代码中区分各个房间（需查看indexed_map的实现方式）
√ 如何对房间进行排序（需查看indexed_map的实现方式）
如何确定单个房间的起点和终点（房间口）

B样条曲线 C++实现

Thu, 05 Sep 2024 06:03:33 GMT

基于Hubery编写的Bspline源码进行实验：hubery05/A-start-with-B-spline (github.com) A* 部分程序来源：ROS package of A star algorithm (github.com) 实验效果： 绿色为A*规划的路径，蓝色为Bspline曲线 整理后代码仓库： Arik/Bspline（https://gitee.com/upcgyl/bspline.git）

Turtlebot3 项目运行

Mon, 02 Sep 2024 07:03:16 GMT

Turtlebot3项目：

启动虚拟环境（gazebo）：

source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_gazebo turtlebot3_world.launch

读取地图并运行导航程序

source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/home/gyl/桌面/VMWorks/catkin_turtlebot3/src/turtlebot3-master/turtlebot3_navigation/maps/map.yaml

Clean-robot-turtlebot3项目：

修改world和map文件为自己的虚拟环境
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch clean_robot clean_work.launch map_file:=/home/gyl/桌面/VMWorks/clean_robot_ws/src/clean_robot/maps/map.yaml

My-turtlebot3项目

（顾名思义，就是我自己的项目……）

catkin_make
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch path_plan clean_work.launch

A* 算法优化

Thu, 29 Aug 2024 06:09:08 GMT

一、程序上优化复杂度#

A*算法的核心计算在于：

从open和close表中进行查找
从open表中搜索最小栅格
从邻居中查找最小栅格

考虑从①②两点进行优化，传统的 A 使用 vector<Point >或者list<Point* >（Point为自己定义的点的结构体，包括坐标，FGH，父节点）

考虑到查找的复杂度较高，所以思考采用hash表进行查找，其查找的复杂度为O(1)，所以我们采用unordered_map对 openlist 和 closelist 进行备份，在查找时直接调用查找函数find或者count，由于unordered_map存在键值对应关系，可以设计为点的坐标的序号为key，即(point.x-1)*map.col+point.y，这样能确保唯一性；

二、优化启发函数 F(N) = G(N)+W(N)*H(N)#

参考文章： A* 算法及优化思路

三、优化邻域#

四、使用双向A*算法#

并不是一个比较稳定的做法，后续如果使用到后再做补充参考：双向A*搜索 | 双向启发式搜索（有几个关键问题做出了解答）

其他优化方法#

参考文章：【路径规划】A*算法方法改进思路简析

个人A*优化后程序：#

Arik/AStar (https://gitee.com/upcgyl/astar.git)

<编辑中>路径平滑处理

Thu, 29 Aug 2024 03:01:43 GMT

文章大致来源：#

路径平滑算法：机器人导航

问题背景：#

机器人通过栅格地图进行路径规划时，根据静态障碍物得到全局路径。通过各种路径规划算法，如 Dijkstra’s，A*，D-star，RRT等，规划出的路径都存在直线之间有急剧拐弯（曲率变化大）的问题，对于实际机器人是没办法连续完成这样的路径的，大多需要停止运动转向继续行驶，这样的话能量消耗较大且工作效率较低。针对这个问题就需要对寻找到的路径进行平滑处理。

解决思路：#

将路径修改为各段各点间 ①速度 ②加速度 ③曲率三部分连续

优化方法：#

多项式插值 插值法用于路径平滑十分原始，有两个缺点：①计算复杂度高；②Runge’s 现象；（Runge’s现象为龙格现象，即随着插值节点个数增加，反而不能更好地逼近被插值函数的现象。理论了解：函数逼近——拉格朗日插值下的(Runge)龙格现象 | 北太天元 or matlab ) 对于路径优化的应用：平滑轨迹插值方法之多项式插值（附代码）
贝塞尔曲线 贝塞尔曲线是应用于二维图形应用程序的数学曲线，由一组称为控制点的向量来确定，给定的控制点按顺序连接构成控制多边形，贝塞尔曲线逼近这个多边形，进而通过调整控制点坐标改变曲线的形状。控制点的作用是控制曲线的弯曲程度理论了解：详解样条曲线（上）（包含贝塞尔曲线）对于路径优化的应用：【路径规划】局部路径规划算法——贝塞尔曲线法（含python实现 | c++实现）
三次样条曲线 理论了解：基于三次样条插值的路径规划算法对于路径优化的应用：
B样条曲线理论了解：详解样条曲线（上）（包含贝塞尔曲线）对于路径优化的应用：基于B-spline的轨迹优化
NURBS 曲线理论了解：对于路径优化的应用：
Dubin’s曲线理论了解：对于路径优化的应用：
Clothoid曲线理论了解：对于路径优化的应用：
Hypocycloid理论了解：对于路径优化的应用：
Reeds-Shepp Curves理论了解：对于路径优化的应用：
Balkcom-Mason 曲线理论了解：对于路径优化的应用：

路径优化 minimum-snap

Wed, 28 Aug 2024 03:28:53 GMT

实现效果：#

使用说明：#

git clone https://gitee.com/upcgyl/minimum-snap.git
cd minimum-snap
catkin_make
source devel/setup.bash
roslaunch MyAstar astar.launch

osqp_Eigen 安装可参考：osqp_eigne ubuntu安装和使用 osqp_Eigen 安装后修改bug可参考：linux安装eigen/osqp/osqp_eigen库

目前存在问题：#

由于调用minimum_snap选点采用平均采样，导致最终得到的路径并不是特别平滑，尤其在直线路径影响较为明显，考虑之后再A*后的路径选取部分关键点进行minimum_snap优化，然后样条插值；

C++下的AStar实现

Fri, 23 Aug 2024 08:53:16 GMT

参考文献：

实现代码：https://gitee.com/upcgyl/astar.git 存在问题：

地图目前必须是可搜索到路径
周围点寻找太过复杂
OpenList和CloseList结构不统一导致查找函数需要写两个

后续优化：

参考 ROS package of A star algorithm 中对周围节点的查询方式进行优化
思考二叉堆的实现方式
优化地图输入
区分linux端：增加OpenCV的地图识别，将邻接矩阵输入变为pgm地图输入

马尔科夫奖励过程

Wed, 21 Aug 2024 06:21:46 GMT

原文链接：第二章马尔可夫决策过程 (MDP) (datawhalechina.github.io) 马尔可夫奖励过程（Markov reward process, MRP）是马尔可夫链加上奖励函数。在马尔可夫奖励过程中，状态转移矩阵和状态都与马尔可夫链一样，只是多了奖励函数（reward function）。奖励函数 R 是一个期望，表示当我们到达某一个状态的时候，可以获得多大的奖励。这里另外定义了折扣因子 γ 。如果状态数是有限的，那么 R 可以是一个向量。

回报与价值函数#

这里我们进一步定义一些概念。范围（horizon） 是指一个回合的长度（每个回合最大的时间步数），它是由有限个步数决定的。 **回报（return）**可以定义为奖励的逐步叠加，假设时刻 t 后的奖励序列为 rt+1,rt+2,rt+3,⋯ 则回报为

Gt=rt+1+γrt+2+γ2rt+3+γ3rt+4+…+γT−t−1rT

其中，T是最终时刻，γ 是折扣因子，越往后得到的奖励，折扣越多。这说明我们更希望得到现有的奖励，对未来的奖励要打折扣。当我们有了回报之后，就可以定义状态的价值了，就是状态价值函数（state-value function）。对于马尔可夫奖励过程，状态价值函数被定义成回报的期望，即

Vt(s)=E[Gt∣st=s]=E[rt+1+γrt+2+γ2rt+3+…+γT−t−1rT∣st=s]

其中，Gt 是之前定义的折扣回报（discounted return）。我们对Gt取了一个期望，期望就是从这个状态开始，我们可能获得多大的价值。所以期望也可以看成未来可能获得奖励的当前价值的表现，就是当我们进入某一个状态后，我们现在有多大的价值。我们使用折扣因子的原因如下。第一，有些马尔可夫过程是带环的，它并不会终结，我们想避免无穷的奖励。第二，我们并不能建立完美的模拟环境的模型，我们对未来的评估不一定是准确的，我们不一定完全信任模型，因为这种不确定性，所以我们对未来的评估增加一个折扣。我们想把这个不确定性表示出来，希望尽可能快地得到奖励，而不是在未来某一个点得到奖励。第三，如果奖励是有实际价值的，我们可能更希望立刻就得到奖励，而不是后面再得到奖励（现在的钱比以后的钱更有价值）。最后，我们也更想得到即时奖励。有些时候可以把折扣因子设为 0（γ=0），我们就只关注当前的奖励。我们也可以把折扣因子设为 1（γ=1），对未来的奖励并没有打折扣，未来获得的奖励与当前获得的奖励是一样的。折扣因子可以作为强化学习智能体的一个超参数（hyperparameter）来进行调整，通过调整折扣因子，我们可以得到不同动作的智能体。在马尔可夫奖励过程里面，我们如何计算价值呢？如图 2.4 所示，马尔可夫奖励过程依旧是状态转移，其奖励函数可以定义为：智能体进入第一个状态 s1s1 的时候会得到 5 的奖励，进入第七个状态 s7s7 的时候会得到 10 的奖励，进入其他状态都没有奖励。我们可以用向量来表示奖励函数，即

R=[5,0,0,0,0,0,10]

图马尔可夫奖励过程的例子

我们对 4 步的回合（γ=0.5）来采样回报 G。（1）s4,s5,s6,s7的回报<0>+0.5×0+0.25×0+0.125×10=1.25 （2）s4,s3,s2,s1的回报<0>+0.5×0+0.25×0+0.125×5=0.625 （3）s4,s5,s6,s6的回报<0>+0.5×0+0.25×0+0.125×0=0 我们现在可以计算每一个轨迹得到的奖励，比如我们对轨迹 s4,s5,s6,s7 的奖励进行计算，这里折扣因子是 0.5。在 s4 的时候，奖励为0。下一个状态 s5 的时候，因为我们已经到了下一步，所以要把 s5 进行折扣，s5 的奖励也是0。然后是 s6，奖励也是0，折扣因子应该是0.25。到达 s7 后，我们获得了一个奖励，但是因为状态 s7 的奖励是未来才获得的奖励，所以我们要对之进行3次折扣。最终这个轨迹的回报就是 1.25。类似地，我们可以得到其他轨迹的回报。这里就引出了一个问题，当我们有了一些轨迹的实际回报时，怎么计算它的价值函数呢？比如我们想知道 s4 的价值，即当我们进入 s4 后，它的价值到底如何？一个可行的做法就是我们可以生成很多轨迹，然后把轨迹都叠加起来。比如我们可以从 s4 开始，采样生成很多轨迹，把这些轨迹的回报都计算出来，然后将其取平均值作为我们进入 s4 的价值。这其实是一种计算价值函数的办法，也就是通过蒙特卡洛（Monte Carlo，MC）采样的方法计算 s4 的价值。

2的幂 | 4的幂

Tue, 06 Aug 2024 03:38:32 GMT

2的幂：

1
return n>0 && (n&(n-1))==0;

4的幂

1
return  n>0 && (n&(n-1))==0 && n%3==1;

快速幂 Pow(x,n)

Fri, 02 Aug 2024 06:08:03 GMT

题目描述#

实现 pow(x, n) ，即计算 x 的整数 n 次幂函数（即，xn ）。 示例 1：

1
输入：x = 2.00000, n = 10
2
输出：1024.00000

示例 2：

1
输入：x = 2.10000, n = 3
2
输出：9.26100

个人C++解答#

快速幂有两种方法，一种是分治思想，另一种是倍增法，我用的是分治的思路，将x的n次方化为x的n/2*n/2次方×x的n%2次方，下面给出代码；

1
class Solution {
2
public:
3
    double myPow(double x, int n) {
4
        if(n==0) return 1;
5
        if(n==1) return x;
6
        if(n==-1) return 1/x;
7

8
        double a = myPow(x,n/2);
9
        double b = myPow(x,n%2);
10
        return a*a*b;
11
    }
12
};

个人C++解答#

这一部分是倍增法的代码，我没有去尝试理解，来自评论区FATSHEEP好友提供，灵神的题解也是用到的倍增法

1
double myPow(double x, int n) {
2
    double ans = 1;
3
    while(n != 0)
4
    {
5
        if(n & 1)
6
        {
7
            if(n < 0) ans *= 1/x;
8
            else ans *= x;
9
        }
10
        n /= 2;
11
        x *= x;
12
    }
13
    return ans;
14
}

软件能力成熟度 CMM | CMMI阶段式 | CMMI连续式

Fri, 02 Aug 2024 03:55:57 GMT

CMM模型#

**初始级:**杂乱无章，甚至混乱，几乎没有明确定义的步骤，项目的成功完全依赖个人的努力和英雄式核心人物的作用。 **可重复级:**建立了基本的项目管理过程和实践来跟踪项目费用、进度和功能特性，有必要的过程准则来重复以前在同类项目中的成功。 **已定义级:**管理和工程两方面的软件过程已经文档化、标准化，并综合成整个软件开发组织的标准过程。 **已管理级:**制定了软件过程和产品质量的详细度量标准。 **优化级:**加强了定量分析，通过来自过程质量反馈和来自新观念、新技术的反馈使过程能不断持续地改进。

CMMI阶段式模型#

初始的<过程不可预测且缺乏控制>。 已管理的<过程为项目服务>。 已定义的<过程为组织服务>。 定量管理的<过程已度量和控制>。 优化的<集中于过程改进>

CMMI连续式模型#

CLO(未完成的)：过程域未执行或未得到 CL1 中定义的所有目标。 CL1(已执行的)<其共性目标是过程将可标识的输入工作产品转换成可标识的输出工作产品>，以实现支持过程域的特定目标。 CL2(已管理的)<其共性目标是集中于已管理的过程的制度化>。 CL3(已定义级的)<其共性目标集中于已定义的过程的制度化>。 CL4(定量管理的)<其共性目标集中于可定量管理的过程的制度化>。 CL5(优化的)<使用量化>(统计学)手段改变和优化过程域，以满足客户的改变和持续改进计划中的过程域的功效。

LeetCode 面试经典150题 [81/150 被围绕的区域]

Wed, 31 Jul 2024 08:23:24 GMT

题目描述#

给你一个 m x n 的矩阵 board ，由若干字符 ‘X’ 和 ‘O’ 组成，捕获所有被围绕的区域：连接：一个单元格与水平或垂直方向上相邻的单元格连接。区域：连接所有 ‘O’ 的单元格来形成一个区域。围绕：如果您可以用 ‘X’ 单元格连接这个区域，并且区域中没有任何单元格位于 board 边缘，则该区域被 ‘X’ 单元格围绕。通过将输入矩阵 board 中的所有 ‘O’ 替换为 ‘X’ 来捕获被围绕的区域。 示例 1：

1
输入：board = [["X","X","X","X"],["X","O","O","X"],["X","X","O","X"],["X","O","X","X"]]
2

3
输出：[["X","X","X","X"],["X","X","X","X"],["X","X","X","X"],["X","O","X","X"]]
4

5
解释：
6

7
在上图中，底部的区域没有被捕获，因为它在 board 的边缘并且不能被围绕。

示例 2：

1
输入：board = [["X"]]
2

3
输出：[["X"]]

个人C++解答#

先找到外围一圈的O，然后找他的连通，找到后标记，然后整体遍历，将所有的O变为X，将被标记的变回O

1
class Solution {
2
public:
3
void dfs(vector<vector<char>>& board, int i, int j) {
4
        int i_num = board.size();
5
        int j_num = board[0].size();
6

7
        board[i][j] = '-';
8

9
        if (i - 1 >= 0 && board[i - 1][j] == 'O')
10
            dfs(board, i - 1, j);
11
        if (i + 1 < i_num && board[i + 1][j] == 'O')
12
            dfs(board, i + 1, j);
13
        if (j - 1 >= 0 && board[i][j - 1] == 'O')
14
            dfs(board, i, j - 1);
15
        if (j + 1 < j_num && board[i][j + 1] == 'O')
16
            dfs(board, i, j + 1);
17
    }
18
    void solve(vector<vector<char>>& board) {
19
        int i_num = board.size();
20
        int j_num = board[0].size();
21
        for(int j=0;j<j_num;j++){
22
            if(board[0][j]=='O'){
23
                dfs(board,0,j);
24
            }
25
        }
26
        for(int j=0;j<j_num;j++){
27
            if(board[i_num-1][j]=='O'){
28
                dfs(board,i_num-1,j);
29
            }
30
        }
31
        for(int i=0;i<i_num;i++){
32
            if(board[i][0]=='O'){
33
                dfs(board,i,0);
34
            }
35
        }
36
        for(int i=0;i<i_num;i++){
37
            if(board[i][j_num-1]=='O'){
38
                dfs(board,i,j_num-1);
39
            }
40
        }
41
        for(int i=0;i<i_num;i++){
42
            for(int j=0;j<j_num;j++){
43
                if(board[i][j] == 'O') board[i][j] = 'X';
44
                if(board[i][j] == '-') board[i][j] = 'O';
45
            }
46
        }
47

48
    }
49
};

LeetCode 面试经典150题 [81/150 岛屿数量]

Wed, 31 Jul 2024 07:18:54 GMT

题目描述#

给你一个由 ‘1’（陆地）和 ‘0’（水）组成的的二维网格，请你计算网格中岛屿的数量。岛屿总是被水包围，并且每座岛屿只能由水平方向和/或竖直方向上相邻的陆地连接形成。此外，你可以假设该网格的四条边均被水包围。 示例 1：

1
输入：grid = [
2
  ["1","1","1","1","0"],
3
  ["1","1","0","1","0"],
4
  ["1","1","0","0","0"],
5
  ["0","0","0","0","0"]
6
]
7
输出：1

示例 2：

1
输入：grid = [
2
  ["1","1","0","0","0"],
3
  ["1","1","0","0","0"],
4
  ["0","0","1","0","0"],
5
  ["0","0","0","1","1"]
6
]
7
输出：3

个人C++解答#

递归搜索，果然一使用递归就有点不会了，说是深度优先搜索，但是没感觉到像

1
class Solution {
2
public:
3
    void dfs(vector<vector<char>>& grid, int i, int j) {
4
        int i_num = grid.size();
5
        int j_num = grid[0].size();
6

7
        grid[i][j] = '0';
8

9
        if (i - 1 >= 0 && grid[i - 1][j] == '1')
10
            dfs(grid, i - 1, j);
11
        if (i + 1 < i_num && grid[i + 1][j] == '1')
12
            dfs(grid, i + 1, j);
13
        if (j - 1 >= 0 && grid[i][j - 1] == '1')
14
            dfs(grid, i, j - 1);
15
        if (j + 1 < j_num && grid[i][j + 1] == '1')
16
            dfs(grid, i, j + 1);
17
    }
18
    int numIslands(vector<vector<char>>& grid) {
19
        int i_num = grid.size();
20
        if (!i_num)
21
            return 0;
22
        int j_num = grid[0].size();
23

24
        int ans = 0;
25
        for (int i = 0; i < i_num; i++) {
26
            for (int j = 0; j < j_num; j++) {
27
                if (grid[i][j] == '1') {
28
                    ++ans;
29
                    dfs(grid, i, j);
30
                }
31
            }
32
        }
33
        return ans;
34
    }
35
};

LeetCode 面试经典150题 [80/150 验证二叉搜索树]

Wed, 31 Jul 2024 06:19:20 GMT

题目描述#

给你一个二叉树的根节点 root ，判断其是否是一个有效的二叉搜索树。有效二叉搜索树定义如下：节点的左子树只包含小于当前节点的数。节点的右子树只包含大于当前节点的数。所有左子树和右子树自身必须也是二叉搜索树。 示例 1：

1
输入：root = [2,1,3]
2
输出：true

示例 2：

1
输入：root = [5,1,4,null,null,3,6]
2
输出：false
3
解释：根节点的值是 5 ，但是右子节点的值是 4 。

个人C++解答#

利用二叉搜索树的定义，中序遍历为升序这样的条件去判断，起初用递归按照条件来写，但是由于没考虑左右字数必须都是二叉搜索树也就是说不能只小于或大于根节点这一个条件，所以没做出来

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left),
10
 * right(right) {}
11
 * };
12
 */
13
class Solution {
14
    vector<long> vec;
15
public:
16
    bool isValidBST(TreeNode* root) {
17
        inOrder(root);
18
        for (int i = 1; i < vec.size(); i++) {
19
            if (vec[i] - vec[i - 1] <= 0) {
20
                return false;
21
            }
22
        }
23
        return true;
24
    }
25
    void inOrder(TreeNode* root) {
26
        if(root == nullptr) return;
27
        inOrder(root->left);
28
        vec.push_back(root->val);
29
        inOrder(root->right);
30
    }
31
};

LeetCode 面试经典150题 [79/150 二叉搜索树中第k小的元素]

Wed, 31 Jul 2024 05:53:16 GMT

题目描述#

给定一个二叉搜索树的根节点 root ，和一个整数 k ，请你设计一个算法查找其中第 k 小的元素（从 1 开始计数）。 示例 1：

1
输入：root = [3,1,4,null,2], k = 1
2
输出：1

示例 2：

1
输入：root = [5,3,6,2,4,null,null,1], k = 3
2
输出：3

个人C++解答#

搜索树中序遍历得到递增序列，然后取第k个元素

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
    vector<int> vec;
14
public:
15
    int kthSmallest(TreeNode* root, int k) {
16
        inOrder(root);
17
        return vec[k-1];
18
    }
19
    void inOrder(TreeNode* root){
20
        if(root == nullptr) return;
21
        inOrder(root->left);
22
        vec.push_back(root->val);
23
        inOrder(root->right);
24
    }
25
};

LeetCode 面试经典150题 [78/150 二叉搜索树的最小绝对差]

Wed, 31 Jul 2024 05:48:02 GMT

题目描述#

给你一个二叉搜索树的根节点 root ，返回树中任意两不同节点值之间的最小差值。差值是一个正数，其数值等于两值之差的绝对值。 示例 1：

1
输入：root = [4,2,6,1,3]
2
输出：1

示例 2：

1
输入：root = [1,0,48,null,null,12,49]
2
输出：1

个人C++解答#

看了评论区的思路写的，二叉树中序遍历，得到递增序列，然后找相邻差最小就好

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
    vector<int> vec;
14
public:
15
    int getMinimumDifference(TreeNode* root) {
16
        inOrder(root);
17
        int ans = INT_MAX;
18
        for(int i=1;i<vec.size();i++){
19
            ans = min(ans,vec[i]-vec[i-1]);
20
        }
21
        return ans;
22
    }
23

24
    void inOrder(TreeNode* root) {
25
    if (root == nullptr) return;
26
    inOrder(root->left);
27
    vec.push_back(root->val);
28
    inOrder(root->right);
29

30
}
31
};

LeetCode 面试经典150题 [77/150 二叉树的锯齿形层序遍历]

Wed, 31 Jul 2024 03:46:09 GMT

题目描述#

给你二叉树的根节点 root ，返回其节点值的锯齿形层序遍历。（即先从左往右，再从右往左进行下一层遍历，以此类推，层与层之间交替进行）。 示例 1：

1
输入：root = [3,9,20,null,null,15,7]
2
输出：[[3],[20,9],[15,7]]

示例 2：

1
输入：root = [1]
2
输出：[[1]]

个人C++解答#

层序遍历模板，数组保存每一层，层的奇偶变化改变判断是偶逆置（reverse）;

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
public:
14
    vector<vector<int>> zigzagLevelOrder(TreeNode* root) {
15
        if (root == nullptr) return {};
16
        vector<TreeNode*> q;
17
        vector<vector<int>> ans;
18
        int flag = 0;
19
        q.push_back(root);
20
        while (!q.empty()) {
21
            vector<TreeNode*> vec;
22
            vector<int> temp;
23
            flag ++;
24
            for (int i = 0; i <q.size(); i++) {
25
                temp.push_back(q[i]->val);
26
                TreeNode* node = q[i];
27
                if (node->left)
28
                    vec.push_back(node->left);
29
                if (node->right)
30
                    vec.push_back(node->right);
31
            }
32
            if(flag%2==0) reverse(temp.begin(),temp.end());
33
            ans.push_back(temp);
34
            q = move(vec);
35
        }
36
        return ans;
37
    }
38
};

LeetCode 面试经典150题 [76/150 二叉树的层序遍历]

Wed, 31 Jul 2024 02:55:51 GMT

题目描述#

给你二叉树的根节点 root ，返回其节点值的层序遍历。（即逐层地，从左到右访问所有节点）。 示例 1：

1
输入：root = [3,9,20,null,null,15,7]
2
输出：[[3],[9,20],[15,7]]

示例 2：

1
输入：root = [1]
2
输出：[[1]]

个人C++解答#

依旧是套层序遍历模板，注意处理root为空的情况，上一道题由于root不为空，所以可以省略

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
public:
14
    vector<vector<int>> levelOrder(TreeNode* root) {
15
        if(root == nullptr) return {};
16
        vector<TreeNode*> q;
17
        vector<vector<int>> ans;
18
        q.push_back(root);
19
        while (!q.empty()) {
20
            vector<TreeNode*> vec;
21
            vector<int> temp;
22
            for (int i = 0; i < q.size(); i++) {
23
                temp.push_back(q[i]->val);
24
                TreeNode* node = q[i];
25
                if (node->left)
26
                    vec.push_back(node->left);
27
                if (node->right)
28
                    vec.push_back(node->right);
29
            }
30
            ans.push_back(temp);
31
            q = move(vec);
32
        }
33
        return ans;
34
    }
35
};

LeetCode 面试经典150题 [75/150 二叉树的层平均值]

Wed, 31 Jul 2024 02:46:21 GMT

题目描述#

给定一个非空二叉树的根节点 root , 以数组的形式返回每一层节点的平均值。与实际答案相差 10-5 以内的答案可以被接受。 示例 1：

1
输入：root = [3,9,20,null,null,15,7]
2
输出：[3.00000,14.50000,11.00000]
3
解释：第 0 层的平均值为 3,第 1 层的平均值为 14.5,第 2 层的平均值为 11 。
4
因此返回 [3, 14.5, 11] 。

示例 2：

1
输入：root = [3,9,20,15,7]
2
输出：[3.00000,14.50000,11.00000]

个人C++解答#

套用上一道题的层次遍历的模板，稍作修改，主要是double类型可能会出错；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left),
10
 * right(right) {}
11
 * };
12
 */
13
class Solution {
14
public:
15
    vector<double> averageOfLevels(TreeNode* root) {
16
        vector<TreeNode*> q;
17
        vector<double> ans;
18
        q.push_back(root);
19
        while (!q.empty()) {
20
            vector<TreeNode*> vec;
21
            double count = 0, sum = 0;
22
            for (int i = 0; i < q.size(); i++) {
23
                count++;
24
                sum += (q[i]->val);
25
                TreeNode* node = q[i];
26
                if (node->left)
27
                    vec.push_back(node->left);
28
                if (node->right)
29
                    vec.push_back(node->right);
30
            }
31
            ans.push_back(sum / count);
32
            q = move(vec);
33
        }
34
        return ans;
35
    }
36
};

LeetCode 面试经典150题 [74/150 二叉树的右视图]

Wed, 31 Jul 2024 02:34:43 GMT

题目描述#

给定一个二叉树的根节点 root，想象自己站在它的右侧，按照从顶部到底部的顺序，返回从右侧所能看到的节点值。 示例 1：

1
输入: [1,2,3,null,5,null,4]
2
输出: [1,3,4]

示例 2：

1
输入: [1,null,3]
2
输出: [1,3]

个人C++解答#

哈哈，按照自己思路做出来的，果然只要不是递归就好写多了，主要思路就是层次遍历，用数组存每一层，然后保存最后节点的值到ans数组中；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left),
10
 * right(right) {}
11
 * };
12
 */
13
class Solution {
14
public:
15
    vector<int> rightSideView(TreeNode* root) {
16
        if (root == nullptr) return {};
17
        vector<TreeNode*> q;
18
        vector<int> ans;
19
        q.push_back(root);
20
        while (!q.empty()) {
21
            vector<TreeNode*> vec;
22
            for (int i = 0; i <q.size(); i++) {
23
                TreeNode* node = q[i];
24
                if (node->left)
25
                    vec.push_back(node->left);
26
                if (node->right)
27
                    vec.push_back(node->right);
28
            }
29
            ans.push_back(q[q.size()-1]->val);
30
            q = move(vec);
31
        }
32
        return ans;
33
    }
34
};

LeetCode 面试经典150题 [73/150 二叉树的最近公共祖先]

Wed, 31 Jul 2024 01:14:00 GMT

题目描述#

给定一个二叉树, 找到该树中两个指定节点的最近公共祖先。百度百科中最近公共祖先的定义为：“对于有根树 T 的两个节点 p、q，最近公共祖先表示为一个节点 x，满足 x 是 p、q 的祖先且 x 的深度尽可能大（一个节点也可以是它自己的祖先）。” 示例 1：

1
输入：root = [3,5,1,6,2,0,8,null,null,7,4], p = 5, q = 1
2
输出：3
3
解释：节点 5 和节点 1 的最近公共祖先是节点 3 。

示例 2：

1
输入：root = [3,5,1,6,2,0,8,null,null,7,4], p = 5, q = 4
2
输出：5
3
解释：节点 5 和节点 4 的最近公共祖先是节点 5 。因为根据定义最近公共祖先节点可以为节点本身。

个人C++解答#

官方题解二：存储父节点；利用哈希表存储所有节点的父节点，然后利用节点的父节点信息从p节点不断往上跳，并记录已经访问过的节点，再从q节点不断往上跳，如果碰到已经访问过的节点，那么这个节点就是我们要找的最近公共祖先 dfs用作遍历整棵二叉树，将父节点信息存储在fa表中，创建vis表，在fa表中向上查询p的父节点，查询到即在vis表中置true，在fa表中查询q的父节点，付过查到q的父节点在vis表中，则返回这个父节点；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode(int x) : val(x), left(NULL), right(NULL) {}
8
 * };
9
 */
10
class Solution {
11
public:
12
    unordered_map<int, TreeNode*> fa;
13
    unordered_map<int, bool> vis;
14
    void dfs(TreeNode* root) {
15
        if (root->left != nullptr) {
16
            fa[root->left->val] = root;
17
            dfs(root->left);
18
        }
19
        if (root->right != nullptr) {
20
            fa[root->right->val] = root;
21
            dfs(root->right);
22
        }
23
    }
24
    TreeNode* lowestCommonAncestor(TreeNode* root, TreeNode* p, TreeNode* q) {
25
        fa[root->val] = nullptr;
26
        dfs(root);
27
        while(p!=nullptr){
28
            vis[p->val] = true;
29
            p = fa[p->val];
30
        }
31
        while(q!=nullptr){
32
            if(vis[q->val]) return q;
33
            q = fa[q->val];
34
        }
35
        return nullptr;
36

37
    }
38
};

LeetCode 面试经典150题 [72/150 完全二叉树的节点个数]

Tue, 30 Jul 2024 08:08:03 GMT

题目描述#

给你一棵完全二叉树的根节点 root ，求出该树的节点个数。完全二叉树的定义如下：在完全二叉树中，除了最底层节点可能没填满外，其余每层节点数都达到最大值，并且最下面一层的节点都集中在该层最左边的若干位置。若最底层为第 h 层，则该层包含 1~ 2h 个节点。 示例 1：

1
输入：root = [1,2,3,4,5,6]
2
输出：6

示例 2：

1
输入：root = []
2
输出：0

个人C++解答#

比较简单，但是由于对递归不熟悉，看了答案才知道在哪+1；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
public:
14
    int countNodes(TreeNode* root) {
15
        if(root == nullptr) return 0;
16
        return countNodes(root->left) + countNodes(root->right) + 1;
17
    }
18
};

LeetCode 面试经典150题 [71/150 求个节点到叶节点数字之和]

Tue, 30 Jul 2024 07:15:20 GMT

题目描述#

给你一个二叉树的根节点 root ，树中每个节点都存放有一个 0 到 9 之间的数字。每条从根节点到叶节点的路径都代表一个数字：例如，从根节点到叶节点的路径 1 -> 2 -> 3 表示数字 123 。计算从根节点到叶节点生成的所有数字之和。叶节点是指没有子节点的节点。 示例 1：

1
输入：root = [1,2,3]
2
输出：25
3
解释：
4
从根到叶子节点路径 1->2 代表数字 12
5
从根到叶子节点路径 1->3 代表数字 13
6
因此，数字总和 = 12 + 13 = 25

示例 2：

1
输入：root = [4,9,0,5,1]
2
输出：1026
3
解释：
4
从根到叶子节点路径 4->9->5 代表数字 495
5
从根到叶子节点路径 4->9->1 代表数字 491
6
从根到叶子节点路径 4->0 代表数字 40
7
因此，数字总和 = 495 + 491 + 40 = 1026

个人C++解答#

还是老样子，看题解一眼就会，自己一写就不会

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left),
10
 * right(right) {}
11
 * };
12
 */
13
class Solution {
14
public:
15
    int dfs(TreeNode* root, int prevSum) {
16
        if(root == nullptr) return 0;
17
        int sum = prevSum * 10 + root->val;
18
        if(root->left == nullptr && root->right == nullptr){
19
            return sum;
20
        }else{
21
            return dfs(root->left,sum) + dfs(root->right,sum);
22
        }
23
    }
24
    int sumNumbers(TreeNode* root) {
25
        return  dfs(root,0);
26
    }
27
};

LeetCode 面试经典150题 [70/150 路径总和]

Tue, 30 Jul 2024 06:53:11 GMT

题目描述#

给你二叉树的根节点 root 和一个表示目标和的整数 targetSum 。判断该树中是否存在根节点到叶子节点的路径，这条路径上所有节点值相加等于目标和 targetSum 。如果存在，返回 true ；否则，返回 false 。叶子节点是指没有子节点的节点。 示例 1：

1
输入：root = [5,4,8,11,null,13,4,7,2,null,null,null,1], targetSum = 22
2
输出：true
3
解释：等于目标和的根节点到叶节点路径如上图所示。

示例 2：

1
输入：root = [1,2,3], targetSum = 5
2
输出：false
3
解释：树中存在两条根节点到叶子节点的路径：
4
(1 --> 2): 和为 3
5
(1 --> 3): 和为 4
6
不存在 sum = 5 的根节点到叶子节点的路径。

个人C++解答#

官方题解的做法，用递归，逆向思维，判断叶节点是否为最终的值，层层减少target，而不是层层增加sum，应征了灵神之前说的，处理递归，永远不要想子问题的过程，要想子问题的结果；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left),
10
 * right(right) {}
11
 * };
12
 */
13
class Solution {
14

15
public:
16
    bool hasPathSum(TreeNode* root, int targetSum) {
17
        if (root == nullptr)
18
            return false;
19
        targetSum -= root->val;
20
        if (root->left == root->right)
21
            return targetSum == 0;
22
        return hasPathSum(root->left, targetSum) ||
23
               hasPathSum(root->right, targetSum);
24
    }
25
};

LeetCode 面试经典150题 [69/150 二叉树展开为链表]

Tue, 30 Jul 2024 06:37:36 GMT

题目描述#

给你二叉树的根结点 root ，请你将它展开为一个单链表：展开后的单链表应该同样使用 TreeNode ，其中 right 子指针指向链表中下一个结点，而左子指针始终为 null 。展开后的单链表应该与二叉树先序遍历顺序相同。 示例 1：

1
输入：root = [1,2,5,3,4,null,6]
2
输出：[1,null,2,null,3,null,4,null,5,null,6]

示例 2：

1
输入：root = []
2
输出：[]

个人C++解答#

将先序遍历的结果保存在数组中，重新构造这个树，左节点指向空，右节点指向数组下一位；

1
/**
2
 * Definition for a binary tree node.
3
 * struct TreeNode {
4
 *     int val;
5
 *     TreeNode *left;
6
 *     TreeNode *right;
7
 *     TreeNode() : val(0), left(nullptr), right(nullptr) {}
8
 *     TreeNode(int x) : val(x), left(nullptr), right(nullptr) {}
9
 *     TreeNode(int x, TreeNode *left, TreeNode *right) : val(x), left(left), right(right) {}
10
 * };
11
 */
12
class Solution {
13
    vector<TreeNode*> vec;
14
public:
15
    void flatten(TreeNode* root) {
16
        preOrder(root);
17
        TreeNode* p = root;
18
        for(int i=1;i<vec.size();i++){
19
            p->left = nullptr;
20
            p->right = vec[i];
21
            p = p->right;
22
        }
23
    }
24
    void preOrder(TreeNode* root){
25
        if(root == nullptr) return ;
26
        vec.push_back(root);
27
        preOrder(root->left);
28
        preOrder(root->right);
29
    }
30

31
};

LeetCode 面试经典150题 [68/150 填充每个节点的下一个右侧节点指针II]

Tue, 30 Jul 2024 06:29:12 GMT

题目描述#

给定一个二叉树： struct Node { int val; Node *left; Node *right; Node *next; } 填充它的每个 next 指针，让这个指针指向其下一个右侧节点。如果找不到下一个右侧节点，则将 next 指针设置为 NULL 。初始状态下，所有 next 指针都被设置为 NULL 。 示例 1：

1
输入：root = [1,2,3,4,5,null,7]
2
输出：[1,#,2,3,#,4,5,7,#]
3
解释：给定二叉树如图 A 所示，你的函数应该填充它的每个 next 指针，以指向其下一个右侧节点，如图 B 所示。序列化输出按层序遍历顺序（由 next 指针连接），'#' 表示每层的末尾。

示例 2：

1
输入：root = []
2
输出：[]

个人C++解答#

看着灵神的题解写的，自己的话还是写不出递归来，菜就多练 ①用DFS，用辅助数组记录深度和左节点，可以理解为每个深度只记录一个左节点，然后查询到右节点的时候让左节点指向右节点，由于开始的时候，所有的next节点均为null那么只需改变存在右边节点的即可。 ②用BFS，遍历，将每一层都用next连接起来，但实际写起来感觉也很麻烦，直接复制灵神代码了； ③BFS+链表，依旧是灵神，

DFS

1
class Solution {
2
    vector<Node*> pre;
3
public:
4
    Node* connect(Node* root) {
5
        dfs(root,0);
6
        return root;
7
    }
8

9
    void dfs(Node* node, int depth) {
10
        if(node == nullptr){
11
            return ;
12
        }
13
        if(depth == pre.size()){
14
            pre.push_back(node);
15
        }else{
16
            pre[depth]->next = node;
17
            pre[depth] = node;
18
        }
19
        dfs(node->left,depth+1);
20
        dfs(node->right,depth+1);
21
    }
22
};

BFS

1
class Solution {
2
public:
3
    Node* connect(Node* root) {
4
        if (root == nullptr)
5
            return nullptr;
6
        vector<Node*> q = {root};
7
        while(!q.empty()){
8
            vector<Node*> nxt;
9
            for (int i = 0; i < q.size(); i++) {
10
                Node* node = q[i];
11
                if (i) {
12
                    q[i - 1]->next = node;
13
                }
14
                if (node->left) {
15
                    nxt.push_back(node->left);
16
                }
17
                if (node->right) {
18
                    nxt.push_back(node->right);
19
                }
20
            }
21
            q = move(nxt);
22
        }
23
        return root;
24
    }
25
};

LeetCode 407周赛将1移动至末尾的最大次数

Mon, 22 Jul 2024 00:31:56 GMT

还是自己菜，在第三道题上直接g，思路一直不对…………最后code依据灵神思路

1
class Solution {
2
public:
3
    int maxOperations(string s) {
4
        int result = 0;
5
        int count = 0;
6
        for (int i = 0; i < s.length();i++) {
7
            if (s[i] == '1') {
8
                count++;
9
            } else if (i && s[i-1] == '1') {
10
                result+=count;
11
            }
12
        }
13
        return result;
14
    }
15
};

附灵神题解：堵车（Python/Java/C++/Go）

ipa_coverage_planning 算法解读（四） RoomRotator::computeRoomMainDirection

Fri, 19 Jul 2024 03:03:31 GMT

RoomRotator::computeRoomMainDirection 作为比较单独且核心的一个算法，这里单独拿出来方便后续查看

1
double RoomRotator::computeRoomMainDirection(const cv::Mat& room_map, const double map_resolution)
2
{
3
  const double map_resolution_inverse = 1./map_resolution;
4

5
  // compute Hough transform on edge image of the map
6
  cv::Mat edge_map;
7
  cv::Canny(room_map, edge_map, 50, 150, 3);
8
  std::vector<cv::Vec4i> lines;
9
  double min_line_length = 1.0;  // in [m]
10
  for (; min_line_length > 0.1; min_line_length -= 0.2)
11
  {
12
    cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse);
13
    cv::Mat room_hough = edge_map.clone();
14
    for (size_t i=0; i<lines.size(); ++i)
15
    {
16
      cv::Point p1(lines[i][0], lines[i][1]), p2(lines[i][2], lines[i][3]);
17
      cv::line(room_hough, p1, p2, cv::Scalar(128), 2);
18
    }
19
    //cv::imshow("room_hough", room_hough);
20
    if (lines.size() >= 4)
21
      break;
22
  }
23
  // setup a histogram on the line directions weighted by their length to determine the major direction
24
  Histogram<double> direction_histogram(0, CV_PI, 36);
25
  for (size_t i=0; i<lines.size(); ++i)
26
  {
27
    double dx = lines[i][2] - lines[i][0];
28
    double dy = lines[i][3] - lines[i][1];
29
    if(dy*dy+dx*dx > 0.0)
30
    {
31
      double current_direction = std::atan2(dy, dx);
32
      while (current_direction < 0.)
33
        current_direction += CV_PI;
34
      while (current_direction > CV_PI)
35
        current_direction -= CV_PI;
36
      direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx));
37
      //std::cout << " dx=" << dx << "   dy=" << dy << "   dir=" << current_direction << "   len=" << sqrt(dy*dy+dx*dx) << std::endl;
38
    }
39
  }
40
  return direction_histogram.getMaxBinPreciseVal();
41
}

详细解释： 计算分辨率的倒数，用于长度单位转换 const double map_resolution_inverse = 1./map_resolution; 使用 cv::Canny 函数对房间地图进行边缘检测，得到边缘图 edge_map cv::Mat edge_map; cv::Canny(room_map, edge_map, 50, 150, 3); 使用霍夫变化检测直线

1
std::vector<cv::Vec4i> lines;
2
double min_line_length = 1.0;  // in [m]
3
for (; min_line_length > 0.1; min_line_length -= 0.2)
4
{
5
  cv::HoughLinesP(edge_map, lines, 1, CV_PI/180, min_line_length*map_resolution_inverse, min_line_length*map_resolution_inverse, 1.5*min_line_length*map_resolution_inverse);
6
  cv::Mat room_hough = edge_map.clone();
7
  for (size_t i=0; i<lines.size(); ++i)
8
  {
9
    cv::Point p1(lines[i][0], lines[i][1]), p2(lines[i][2], lines[i][3]);
10
    cv::line(room_hough, p1, p2, cv::Scalar(128), 2);
11
  }
12
  //cv::imshow("room_hough", room_hough);
13
  if (lines.size() >= 4)
14
    break;
15
}

使用 Histogram 类构建一个直方图，记录所有检测到的直线的方向和长度。 Histogram direction_histogram(0, CV_PI, 36); 遍历所有直线，计算它们的方向，并更新方向直方图。最后，返回方向直方图中最大值对应的方向，即为房间地图的主要方向。

1
for (size_t i=0; i<lines.size(); ++i)
2
{
3
  double dx = lines[i][2] - lines[i][0];
4
  double dy = lines[i][3] - lines[i][1];
5
  if(dy*dy+dx*dx > 0.0)
6
  {
7
    double current_direction = std::atan2(dy, dx);
8
    while (current_direction < 0.)
9
      current_direction += CV_PI;
10
    while (current_direction > CV_PI)
11
      current_direction -= CV_PI;
12
    direction_histogram.addData(current_direction, sqrt(dy*dy+dx*dx));
13
    //std::cout << " dx=" << dx << "   dy=" << dy << "   dir=" << current_direction << "   len=" << sqrt(dy*dy+dx*dx) << std::endl;
14
  }
15
}
16
return direction_histogram.getMaxBinPreciseVal();

ipa_coverage_planning 算法解读（三）

Thu, 18 Jul 2024 03:04:14 GMT

书接上文，本文主要介绍牛耕（弓字型）的代码 getExplorationPath函数接口定义如下：

1
void BoustrophedonExplorer::getExplorationPath(const cv::Mat& room_map, std::vector<geometry_msgs::Pose2D>& path,
2
    const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin,
3
    const double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order,
4
    const bool plan_for_footprint, const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track)

getExplorationPath函数的参数说明： const cv::Mat& room_map: 输入的房间地图，通常是一个二维矩阵，表示房间的布局。 std::vector& path: 输出的探索路径，存储了机器人在房间中探索时的位置和方向。 const float map_resolution: 地图的分辨率，即每个像素或单元格代表的实际面积大小。 const cv::Point starting_position: 探索的起始位置。 const cv::Point2d map_origin: 地图的原点坐标。 const double grid_spacing_in_pixel: 网格间距（以像素为单位）。 const double grid_obstacle_offset: 网格障碍物偏移量。 const double path_eps: 路径精度参数。 const int cell_visiting_order: 单元格访问顺序，可以是最优的旅行商问题（TSP）解决方案或简单的从左到右的顺序。 const bool plan_for_footprint: 是否为机器人的实际占用空间（footprint）规划路径。 const Eigen::Matrix robot_to_fov_vector: 机器人视野中心到机器人实际位置的向量。 const double min_cell_area: 最小单元格面积。 const int max_deviation_from_track: 最大偏离轨迹的程度。 getExplorationPath函数的操作流程包括： 1.计算房间地图的主要方向，并根据这个方向和额外的旋转偏移量来确定旋转角度。 2.确定旋转中心，如果没有提供，则根据房间地图的最小和最大坐标点来确定。 3.计算旋转矩阵，使用OpenCV的 cv::getRotationMatrix2D 函数。 4.确定旋转后的边界矩形，以便知道新图像的大小。 5.调整变换矩阵，以确保旋转后的图像正确对齐。 6.返回旋转角度。 1> grid_spacing_as_int: 将 grid_spacing_in_pixel（网格间距，以像素为单位）转换为整数。这是通过取其下限（std::floor）来实现的，确保网格间距是一个整数值。 2> half_grid_spacing_as_int: 计算网格间距的一半，并将其转换为整数。这个值在后续的计算中可能用于确定单元格的边界或者是用于路径规划时考虑的安全边距。 3> min_cell_width: 计算最小单元格宽度。这是通过将 half_grid_spacing_as_int（即网格间距的一半）加上两倍的 grid_obstacle_offset（网格障碍物偏移量）除以 map_resolution（地图分辨率）来实现的。这个值用于确定在探索路径规划中每个单元格的最小尺寸，以确保机器人在探索时不会太靠近障碍物。

1
const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); // convert fov-radius to int
2
const int half_grid_spacing_as_int = (int)std::floor(0.5*grid_spacing_in_pixel); // convert fov-radius to int
3
const int min_cell_width = half_grid_spacing_as_int + 2.*grid_obstacle_offset/map_resolution;

1> cv::Mat R;: 声明一个 cv::Mat 类型的变量 R，用于存储旋转矩阵。 2> cv::Rect bbox;: 声明一个 cv::Rect 类型的变量 bbox，用于存储旋转后的房间地图的边界矩形。 3> cv::Mat rotated_room_map;: 声明一个 cv::Mat 类型的变量 rotated_room_map，用于存储旋转后的房间地图。 4> std::vector cell_polygons;: 声明一个 GeneralizedPolygon 类型的向量 cell_polygons，用于存储房间地图中每个单元格的多边形表示。 5> std::vector polygon_centers;: 声明一个 cv::Point 类型的向量 polygon_centers，用于存储每个单元格多边形的中心点。 6> computeCellDecompositionWithRotation(…): 调用 computeCellDecompositionWithRotation 函数，传入房间地图、地图分辨率、最小单元格面积、最小单元格宽度等参数，以及之前声明的变量。这个函数的目的是计算出房间地图的单元格分解，包括每个单元格的多边形表示和中心点，同时返回旋转矩阵 R 和边界矩形 bbox。

1
cv::Mat R;
2
cv::Rect bbox;
3
cv::Mat rotated_room_map;
4
std::vector<GeneralizedPolygon> cell_polygons;
5
std::vector<cv::Point> polygon_centers;
6
computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R, bbox, rotated_room_map, cell_polygons, polygon_centers);

computeCellDecompositionWithRotation 1> RoomRotator room_rotation;: 创建一个 RoomRotator 类的实例，用于处理房间地图的旋转。 2> room_rotation.computeRoomRotationMatrix(…): 调用 RoomRotator 类的 computeRoomRotationMatrix 方法，计算出旋转矩阵 R 和边界矩形 bbox。这个方法根据房间地图的主要方向和旋转偏移量来确定旋转角度。 3> room_rotation.rotateRoom(…): 使用计算出的旋转矩阵 R 和边界矩形 bbox 来旋转房间地图，得到旋转后的房间地图 rotated_room_map。 4> computeCellDecomposition(…): 调用 computeCellDecomposition 函数，传入旋转后的房间地图、地图分辨率、最小单元格面积和最小单元格宽度等参数，以及之前声明的变量。这个函数的目的是计算出旋转后房间地图的单元格分解，包括每个单元格的多边形表示和中心点。

1
void BoustrophedonExplorer::computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
2
    const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
3
    std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
4
{
5
  RoomRotator room_rotation;
6
  room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset);
7
  room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);
8
  computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers);
9
}

1> 计算旋转角度：首先，通过 computeRoomMainDirection 函数计算出房间地图的主要方向，并加上一个旋转偏移量 rotation_offset，得到旋转角度 rotation_angle。 2> 确定旋转中心：如果没有提供旋转中心（即 center 指针为空），则通过 getMinMaxCoordinates 函数找到房间地图的最小和最大坐标点，计算出旋转中心 center_of_rotation。如果提供了旋转中心，则直接使用该中心。 3> 计算旋转矩阵：使用 OpenCV 的 cv::getRotationMatrix2D 函数，根据旋转中心、旋转角度（转换为度数）和缩放因子（这里为 1.0）来计算旋转矩阵 R。 4> 确定旋转后的边界矩形：计算旋转后的房间地图的边界矩形 bounding_rect，这是为了确定新图像的大小。 5> 调整变换矩阵：调整旋转矩阵 R，以确保旋转后的图像正确对齐。这涉及到调整 R 矩阵的第二行和第三行，以考虑边界矩形的大小和位置。 6> 返回旋转角度：最后，函数返回计算出的旋转角度 rotation_angle。

1
double RoomRotator::computeRoomRotationMatrix(const cv::Mat& room_map, cv::Mat& R, cv::Rect& bounding_rect,
2
    const double map_resolution, const cv::Point* center, const double rotation_offset)
3
{
4
  // rotation angle of the map s.t. the most occurring gradient is in 90 degree to the x-axis
5
  double rotation_angle = computeRoomMainDirection(room_map, map_resolution) + rotation_offset;
6
  std::cout << "RoomRotator::computeRoomRotationMatrix: main rotation angle: " << rotation_angle << std::endl;
7

8
  // get rotation matrix R for rotating the image around the center of the room contour
9
  //  Remark: rotation angle in degrees for opencv
10
  cv::Point center_of_rotation;
11
  if (center == 0)
12
  {
13
    cv::Point min_room, max_room;
14
    getMinMaxCoordinates(room_map, min_room, max_room);
15
    center_of_rotation.x = 0.5*(min_room.x+max_room.x);
16
    center_of_rotation.y = 0.5*(min_room.y+max_room.y);
17
  }
18
  else
19
    center_of_rotation = *center;
20

21
  // compute rotation
22
  R = cv::getRotationMatrix2D(center_of_rotation, (rotation_angle*180)/CV_PI, 1.0);
23

24
  // determine bounding rectangle to find the size of the new image
25
  bounding_rect = cv::RotatedRect(center_of_rotation, room_map.size(), (rotation_angle*180)/CV_PI).boundingRect();
26
  // adjust transformation matrix
27
  R.at<double>(0,2) += 0.5*bounding_rect.width - center_of_rotation.x;
28
  R.at<double>(1,2) += 0.5*bounding_rect.height - center_of_rotation.y;
29

30
  return rotation_angle;
31
}