Yuyuyu/relocation

[vhjcgja-789]

接入重定位服务

• 重定位仓库：https://github.com/Alliance-Algorithm/rmcs_relocation.git

重定位服务集成

概览

本PR集成了重定位服务(rmcs_relocation)，将导航系统从基于PCL/ND-T的点云定位重构为ROS2服务客户端驱动的重定位机制。主要改动包括新增重定位功能的编译依赖、服务接入逻辑以及完整的Lua API绑定。

关键变更

构建系统

• CI流程 (github/workflows/ci.yml)：新增步骤在RMCS工作空间克隆rmcs_relocation仓库至/tmp/RMCS/rmcs_ws/src/skills/rmcs_relocation
• CMakeLists.txt：调整CMake最小版本（3.30→3.28），添加rmcs_relocation依赖并链接其包含目录和库文件
• package.xml：新增rmcs_relocation运行时依赖

核心C++组件

导航组件 (src/cxx/component.cc)

• 集成可选的重定位功能，通过ROS参数控制启用状态
• 新增Localization实例管理和relocalization_enabled标志
• 扩展Lua API：添加relocalize_initial、relocalize_local、relocalize_wide三个重定位函数，以及relocalize_status状态查询函数
• 当重定位禁用时返回警告日志

定位引擎重构 (src/cxx/util/localization/engine.cc/hh)

从PCL点云定位完全重写为ROS2服务客户端模式：

API变更：

• 移除：start_collecting()、start_localizing()方法
• 新增：relocalize(RelocalizeMode mode, double x, double y, double yaw) -> bool
• 新增：relocalize_status() const -> RelocalizeStatus

新增公开类型：

• RelocalizeState枚举：IDLE、IN_FLIGHT、SUCCEEDED、FAILED
• RelocalizeMode枚举：Initial、Local、Wide（对应初始化、局部、广域三种定位模式）
• RelocalizeStatus结构体：包含状态、成功标志、消息、适应度分数、置信度、估计位置（xyz）和四元数方向

Config结构简化：

• 移除：地图文件、NDT参数配置
• 新增：ROS节点引用、服务名称（默认"/rmcs_relocation/relocalize"）、请求超时时长（默认10秒）

实现机制：

• 通过互斥锁管理会话状态，跟踪请求ID防止响应乱序
• 异步服务调用配合超时计时器
• 将服务响应转换为统一的RelocalizeStatus格式

Lua脚本层

Lua API定义 (src/lua/api.lua)

文档化新增函数签名：

• relocalize_initial/local/wide(x: number, y: number, yaw: number): boolean
• relocalize_status(): table（包含state、success、message、fitness_score、confidence字段）

动作控制流程 (src/lua/action.lua)

新增结构化重定位工作流：

• relocalize_initial：单次重定位请求
• relocalize_local：循环重试模式，依赖验证锚点位姿（不可用则立即失败）
• relocalize_wide：使用用户位姿作为先验，回退至原点（位姿不可用时仍继续发送）
• send_and_await共享辅助函数：发送请求后轮询状态直至SUCCEEDED或终止态，返回最终状态和成功标志
• 新增pose_unavailable校验器检测缺失或NaN位姿值

架构变更

从点云驱动的本地定位方案演进为分布式服务模型，支持三层定位策略（初始、局部、广域），降低了导航系统与具体定位实现的耦合度。

[coderabbitai]

Caution

Review failed

Pull request was closed or merged during review

概览

此 PR 将 ROS2 服务驱动的 rmcs_relocation 重定位功能集成到导航系统中，替换原有的 PCL/NDT 本地化实现。通过在 CI 中克隆依赖、更新构建配置、重构定位引擎为基于服务的客户端，以及添加相应的 Lua API 绑定和导航组件集成。

变更

重定位功能集成

图层 / 文件	概述
依赖与构建配置 .github/workflows/ci.yml, CMakeLists.txt, package.xml	在 CI 中添加 rmcs_relocation 克隆步骤；更新 CMake 最低版本至 3.28；新增 find_package(rmcs_relocation) 和相应的头文件/库链接；添加运行时依赖。
定位引擎 API 重定义 src/cxx/util/localization/engine.hh	引入 RelocalizeState 和 RelocalizeMode 枚举；新增 RelocalizeStatus 结构体（包含状态、成功标志、消息、适应度、估计位姿和朝向）；简化 Localization::Config 为仅包含节点引用、服务名和超时配置；用 relocalize(mode, x, y, yaw) → bool 和 relocalize_status() → RelocalizeStatus 替代旧的异步收集/定位 API。
定位引擎实现 src/cxx/util/localization/engine.cc	重写为 ROS2 服务客户端驱动的重定位机制；新增 Session 结构体管理请求生命周期（挂起 ID、最后状态、超时定时器）；实现异步发送与超时处理；提供 relocalize() 和 relocalize_status() 公开方法；移除旧的 PCL/NDT 点云处理逻辑。
导航组件集成 src/cxx/component.cc	新增 localization 智能指针成员和 relocalization_enabled 标志；在构造函数中从 ROS 参数读取启用状态并初始化定位实例；通过共享 lambda 注册三个重定位 Lua 函数（relocalize_initial、relocalize_local、relocalize_wide）和 relocalize_status 查询函数。
Lua API 声明 src/lua/api.lua	在 Api 表中新增 relocalize_initial、relocalize_local、relocalize_wide 和 relocalize_status 的函数声明与文档（参数形状、返回值结构）。
Lua 行为实现 src/lua/action.lua	添加模块级 blackboard 单例、RelocalizeState 枚举和位姿有效性验证器；实现 send_and_await() 助手函数用于发送请求并轮询状态至完成；实现 relocalize_initial() 单次触发、relocalize_local() 带重试循环（锚点位姿不可用时立即失败）、relocalize_wide() 使用黑板位姿或回退至原点；公开 relocalize_status() 访问器。

序列图

sequenceDiagram
    participant Lua as Lua 脚本
    participant Nav as Navigation 组件
    participant API as Lua API 层
    participant Loc as Localization 引擎
    participant Service as rmcs_relocation<br/>ROS2 服务
    
    Lua->>Nav: 调用 api.relocalize_initial(x, y, yaw)
    Nav->>API: 触发注册的重定位绑定
    API->>Loc: 调用 relocalize(mode, x, y, yaw)
    
    Loc->>Service: 异步发送 Relocalize 服务请求
    activate Service
    
    Loc->>Loc: arm_timeout() 启动超时定时器
    
    Lua->>Nav: 轮询 api.relocalize_status()
    Nav->>API: 查询状态
    API->>Loc: 调用 relocalize_status()
    Loc->>Loc: 从 Session 快照返回状态
    API-->>Nav: 返回状态表<br/>(state, success, ...)
    
    Service-->>Loc: 返回服务响应
    deactivate Service
    
    Loc->>Loc: status_from_response()<br/>更新 Session::last_status
    Loc->>Loc: 取消超时定时器
    
    Lua->>Nav: 轮询 api.relocalize_status()
    Nav->>API: 查询状态
    API->>Loc: 调用 relocalize_status()
    Loc-->>API: 返回 SUCCEEDED 或失败态
    API-->>Lua: 返回最终状态表
    Lua->>Lua: 记录结果，继续执行

Loading

代码审查工作量估计

🎯 3 (中等复杂) | ⏱️ ~20 分钟

诗歌

🐰 服务来敲门，重定位启程，
会话守护时，超时保护好，
轮询轻轻问，状态款款答，
Lua 脚本欢欣，导航又新生！✨

✨ Finishing Touches

📝 Generate docstrings

• Create stacked PR
• Commit on current branch

🧪 Generate unit tests (beta)

• Create PR with unit tests
• Commit unit tests in branch yuyuyu/relocation

---

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

• X
• Mastodon
• Reddit
• LinkedIn

_{Comment @coderabbitai help to get the list of available commands and usage tips.}