relocalize#11
Closed
vhjcgja-789 wants to merge 1 commit intomainfrom
Closed
Conversation
Walkthrough此PR向导航系统添加了基于ROS2服务的重新定位 (relocalization) 功能，替换了先前的PCL/NDT点云实现。现在支持通过ROS服务进行异步重新定位请求，附带Lua驱动的多种定位策略和状态查询接口。 Changes重新定位功能集成
Sequence DiagramsequenceDiagram
actor User as Lua User Script
participant Action as action.lua
participant API as Lua API Binding
participant Localization as Localization Service Client
participant ROS as rmcs_relocation Service
User->>Action: relocalize_local(x, y, yaw)
Action->>Action: validate blackboard.user
Action->>API: relocalize_local(x, y, yaw)
API->>Localization: relocalize(LOCAL_MODE, x, y, yaw)
Localization->>Localization: guard against in-flight overlap
Localization->>ROS: async_send_request(RelocalizeRequest)
Localization->>Localization: arm_timeout() wall timer
ROS-->>Localization: async callback with response
Localization->>Localization: Session::end() → update status
Action->>API: relocalize_status() [poll loop]
API->>Localization: relocalize_status()
Localization->>Localization: Session::snapshot()
Localization-->>API: RelocalizeStatus
API-->>Action: status table
alt status.state == SUCCEEDED
Action->>User: return true, status
else status.state != IN_FLIGHT
Action->>User: return false, status + log failure
else
Action->>Action: sleep(0.1) then poll again
end
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Poem
✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Tip 💬 Introducing Slack Agent: The best way for teams to turn conversations into code.Slack Agent is built on CodeRabbit's deep understanding of your code, so your team can collaborate across the entire SDLC without losing context.
Built for teams:
One agent for your entire SDLC. Right inside Slack. 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. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/cxx/util/localization/engine.cc`:
- Around line 149-203: The code has a race between the async response callback
and timeout because pending_id is set only after async_send_request returns; fix
by giving each request a dedicated session token when session->begin() is called
(e.g. generate a uint64_t request_token inside begin and store it on Session as
current_request_token and/or an optional), pass that token into arm_timeout
(capture it in the timer) and into the async_send_request lambda (capture by
value) instead of using the shared pending_id pointer, and change Session::end
to accept and check an expected_token (compare against
Session::current_request_token and only proceed if equal), so both the response
handler and the timeout handler validate the same request token before mutating
session state; update send to set the token before calling async_send_request
and session->track(result.request_id) as before.
In `@src/lua/action.lua`:
- Around line 92-95: The current check treats any fn(x,y,yaw) == false as
"in_flight" and returns (false, nil), which hides real failure reasons; modify
the branch in action.lua so that when fn(...) returns false you call
api.relocalize_status() (or otherwise read the current C++ status) and inspect
it: if the status equals IN_FLIGHT then keep the existing self:warn("reloc skip
%s (in_flight)") and return false, nil, otherwise return false with the actual
status/error (propagate the non-IN_FLIGHT result) so downstream logging and
retry logic see the true failure. Ensure you reference fn,
api.relocalize_status(), and the current self:warn/return pattern when making
the change.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 15fc3493-f933-46b4-b6f9-ce461f33823c
📒 Files selected for processing (6)
package.xmlsrc/cxx/component.ccsrc/cxx/util/localization/engine.ccsrc/cxx/util/localization/engine.hhsrc/lua/action.luasrc/lua/api.lua
Comment on lines
+149
to
+203
| auto arm_timeout(const std::shared_ptr<Session>& s) -> void { | ||
| const auto timeout = std::chrono::duration_cast<std::chrono::nanoseconds>( | ||
| std::chrono::duration<double>{std::max(0.1, config.request_timeout_sec)}); | ||
| auto timer = node.create_wall_timer(timeout, [s]() { | ||
| if (s->end(failed_status("request timeout: " + s->service_name))) | ||
| RCLCPP_WARN(s->logger, "relocalize request timeout: %s", s->service_name.c_str()); | ||
| }); | ||
| auto lock = std::scoped_lock{s->mutex}; | ||
| if (s->timeout_timer) | ||
| s->timeout_timer->cancel(); | ||
| s->timeout_timer = std::move(timer); | ||
| } | ||
|
|
||
| auto send(RelocalizeMode mode, double x, double y, double yaw) -> bool { | ||
| if (!session->begin()) { | ||
| warn("relocalize skipped: previous request is still in flight"); | ||
| return false; | ||
| } | ||
|
|
||
| if (!session->client->service_is_ready() | ||
| && !session->client->wait_for_service(std::chrono::seconds(0))) { | ||
| warn("relocalize service unavailable: {}", config.service_name); | ||
| session->end(failed_status("service unavailable: " + config.service_name)); | ||
| return false; | ||
| } | ||
|
|
||
| auto request = std::make_shared<Relocalize::Request>(); | ||
| request->mode = mode_to_msg(mode); | ||
| request->initial_guess_world_base = yaw_to_pose(x, y, yaw); | ||
|
|
||
| arm_timeout(session); | ||
|
|
||
| // executor 异步触发 callback。 | ||
| auto pending_id = std::make_shared<std::int64_t>(-1); | ||
| auto s = session; | ||
| auto result = session->client->async_send_request( | ||
| std::move(request), [s, pending_id](RelocalizeClient::SharedFuture future) { | ||
| auto response = future.get(); | ||
| if (!response) { | ||
| RCLCPP_WARN(s->logger, "relocalize response is null"); | ||
| s->end(failed_status("response is null"), *pending_id); | ||
| return; | ||
| } | ||
| if (!response->success) { | ||
| RCLCPP_WARN(s->logger, "relocalize failed: %s", response->message.c_str()); | ||
| s->end(status_from_response(*response, false), *pending_id); | ||
| return; | ||
| } | ||
| RCLCPP_INFO( | ||
| s->logger, "relocalize success: score=%.4f, confidence=%.3f", | ||
| response->fitness_score, response->confidence); | ||
| s->end(status_from_response(*response, true), *pending_id); | ||
| }); | ||
| *pending_id = result.request_id; | ||
| session->track(result.request_id); |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
find . -type f -name "engine.cc" | grep -E "localization|util"Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 116
🏁 Script executed:
head -300 src/cxx/util/localization/engine.cc | tail -200Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 7393
🏁 Script executed:
# Get the Session class definition and end() method
rg -A 15 "class Session" src/cxx/util/localization/engine.ccRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 60
🏁 Script executed:
# Check the end() method signature and implementation
rg -B 2 -A 15 "Session::end\(" src/cxx/util/localization/engine.ccRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 60
🏁 Script executed:
# Verify the async callback and race condition context
sed -n '149,203p' src/cxx/util/localization/engine.ccRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 2718
🏁 Script executed:
# Check git log for recent changes to engine.cc
git log --oneline -20 src/cxx/util/localization/engine.ccRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 97
🏁 Script executed:
# Check if there are any other versions or branches with fixes
git show HEAD:src/cxx/util/localization/engine.cc | sed -n '149,203p'Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 2718
响应回调和超时回调需要统一的请求令牌验证机制。
存在严重的请求生命周期竞态：响应回调捕获的 pending_id 初始化为 -1，仅在 async_send_request(...) 返回后才被赋值为真实的 result.request_id。若响应回调在赋值前执行，会传入 expected_id = -1 给 Session::end()，而此时 Session::pending_id 为 std::nullopt，导致条件 if (expected_id && pending_id != *expected_id) 判断为真，end() 返回 false，有效响应被静默丢弃。同时，超时回调调用 s->end(...) 时未传递 expected_id，使其总能成功，可在响应完成后覆盖有效状态。
需要在 begin() 时生成一个会话令牌，令响应和超时两条路径都必须验证该令牌才能更新终态。
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/cxx/util/localization/engine.cc` around lines 149 - 203, The code has a
race between the async response callback and timeout because pending_id is set
only after async_send_request returns; fix by giving each request a dedicated
session token when session->begin() is called (e.g. generate a uint64_t
request_token inside begin and store it on Session as current_request_token
and/or an optional), pass that token into arm_timeout (capture it in the timer)
and into the async_send_request lambda (capture by value) instead of using the
shared pending_id pointer, and change Session::end to accept and check an
expected_token (compare against Session::current_request_token and only proceed
if equal), so both the response handler and the timeout handler validate the
same request token before mutating session state; update send to set the token
before calling async_send_request and session->track(result.request_id) as
before.
Comment on lines
+92
to
+95
| if not fn(x, y, yaw) then | ||
| self:warn(string.format("reloc skip %s (in_flight)", mode)) | ||
| return false, nil | ||
| end |
There was a problem hiding this comment.
不要把所有即时拒绝都记成 in_flight。
fn(...) 返回 false 不只代表已有请求在飞；C++ 侧在 service unavailable / disabled 时也会走这条路径，而且已经把失败原因写进 api.relocalize_status() 了。这里直接返回 nil 会把真实原因吞掉，后面的日志和重试判断都会被误导。建议先补读一次 status，只有真的是 IN_FLIGHT 时再走当前分支。
一个最小修改方向
if not fn(x, y, yaw) then
- self:warn(string.format("reloc skip %s (in_flight)", mode))
- return false, nil
+ local st = api.relocalize_status()
+ if st.state == RelocalizeState.IN_FLIGHT then
+ self:warn(string.format("reloc skip %s (in_flight)", mode))
+ return false, nil
+ end
+ self:warn(string.format("reloc reject [%s] | %s", mode, tostring(st.message)))
+ return false, st
end🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/lua/action.lua` around lines 92 - 95, The current check treats any
fn(x,y,yaw) == false as "in_flight" and returns (false, nil), which hides real
failure reasons; modify the branch in action.lua so that when fn(...) returns
false you call api.relocalize_status() (or otherwise read the current C++
status) and inspect it: if the status equals IN_FLIGHT then keep the existing
self:warn("reloc skip %s (in_flight)") and return false, nil, otherwise return
false with the actual status/error (propagate the non-IN_FLIGHT result) so
downstream logging and retry logic see the true failure. Ensure you reference
fn, api.relocalize_status(), and the current self:warn/return pattern when
making the change.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
重定位接入navigation。
Lua侧直接调用：
initial约5s内完成
local和wide接入scancontext
重定位代码库 ：https://github.com/Alliance-Algorithm/rmcs-relocation.git
PR 摘要：重定位功能集成
概述
本 PR 将重定位(relocalization)功能集成到导航仓库中，通过 ROS2 服务与
rmcs-relocation模块通信，并为用户提供三种重定位模式的 Lua API。主要改动
依赖与配置
rmcs-relocationC++ 层实现
src/cxx/util/localization/engine.hh
RelocalizeState（四种状态：IDLE、IN_FLIGHT、SUCCEEDED、FAILED）、RelocalizeMode（三种模式：Initial、Local、Wide）RelocalizeStatus，包含重定位状态、成功标志、诊断信息、适应度评分、置信度及估计位姿Localization::Config，简化为 ROS Node 引用、服务名称和超时设置start_collecting()和start_localizing()，新增relocalize()和relocalize_status()src/cxx/util/localization/engine.cc
Session对象管理请求生命周期，防止并发调用、过滤迟到响应、自动超时取消relocalize(mode, x, y, yaw): 发起重定位请求，返回 bool 表示请求是否被受理relocalize_status(): 轮询获取当前重定位状态src/cxx/component.cc
enable_relocalization和localization_service_namerelocalize_initial、relocalize_local、relocalize_widerelocalize_status状态查询接口Lua 层实现
src/lua/action.lua
RelocalizeState枚举常量映射send_and_await()辅助函数，统一处理请求发起、轮询等待、状态转移的逻辑relocalize_initial(x, y, yaw): 指定初始位姿的重定位relocalize_local(): 局部重定位（使用黑板中的用户位置作为验证锚点）relocalize_wide(): 广域重定位（验证点无效时降级到原点）try_relocalize_local_then_wide(): 先尝试局部重定位，失败后自动切换到广域模式relocalize_status(): 透传状态查询接口src/lua/api.lua
性能指标