Yuyuyu/relocation#13
Conversation
|
Caution Review failedThe pull request is closed. ℹ️ Recent review info⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Run ID: 📒 Files selected for processing (2)
Walkthrough本PR引入基于 ROS2 服务的重定位支持：新增 Changes重定位功能集成（单一变更 DAG）
Sequence Diagram(s)sequenceDiagram
participant Lua as Lua Action
participant Nav as Navigation Component
participant Loc as Localization (client)
participant Service as Relocalize ROS2 Service
Lua->>Nav: call relocalize_initial(x,y,yaw)
Nav->>Loc: relocalize(Initial, x,y,yaw)
Loc->>Service: async request (mode, pose)
activate Service
Note over Service: 处理重定位请求并计算指标
Service-->>Loc: response (success, pose, fitness, confidence)
deactivate Service
Loc->>Loc: 更新 Session 状态（SUCCEEDED / FAILED）
Lua->>Loc: poll relocalize_status()
Loc-->>Lua: RelocalizeStatus {state, success, message, fitness_score, confidence, pose}
Estimated code review effort🎯 4 (Complex) | ⏱️ ~45 minutes Poem
🚥 Pre-merge checks | ✅ 3 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (3 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
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: 3
🧹 Nitpick comments (3)
src/lua/api.lua (1)
23-23: 💤 Low value
relocalize_status类型注解漏写 estimated_ 字段。*
src/cxx/component.cc的绑定（114-140 行）实际把estimated_x/y/z、estimated_qx/qy/qz/qw一并塞进了返回 table；这里的@field只声明了 5 个字段，会让下游在try_relocalize_local_then_wide等流程里用 LSP 取这些坐标时丢失补全/类型校验。📝 建议补全注解
---- `@field` relocalize_status fun(): { state: integer, success: boolean, message: string, fitness_score: number, confidence: number } +--- `@field` relocalize_status fun(): { state: integer, success: boolean, message: string, fitness_score: number, confidence: number, estimated_x: number, estimated_y: number, estimated_z: number, estimated_qx: number, estimated_qy: number, estimated_qz: number, estimated_qw: number }🤖 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/api.lua` at line 23, The relocalize_status Lua type annotation is missing the estimated pose fields; update the `@field` declaration for relocalize_status in src/lua/api.lua to include estimated_x, estimated_y, estimated_z and estimated_qx, estimated_qy, estimated_qz, estimated_qw (all as numbers) so LSP/completion and type checks used by callers like try_relocalize_local_then_wide can see the estimated_* fields returned by the C++ binding (component.cc bind code around the relocalize return).src/lua/action.lua (1)
8-13: 💤 Low valueMinor: Lua 侧
RelocalizeState枚举值与 C++enum class RelocalizeState是手工镜像的，存在漂移风险。如果
src/cxx/util/localization/engine.hh中RelocalizeState顺序/值改动（目前是IDLE=0/IN_FLIGHT=1/SUCCEEDED=2/FAILED=3，经static_cast<int>暴露），这里需要同步修改，否则send_and_await的轮询判断会静默失效。建议在 C++ 绑定层额外暴露一个常量表（例如在relocalize_status()返回中包含state_name或单独api.relocalize_state表），或在该文件顶部加一行强约束注释引用 C++ 枚举位置。🤖 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 8 - 13, 当前 Lua 文件中硬编码的 RelocalizeState 枚举可能与 C++ enum class RelocalizeState 不同步，导致 send_and_await 的轮询判断静默失败；请修改绑定层以从 C++ 暴露一个常量表（例如在 relocalize_status() 返回中附带 state_name 或单独导出 api.relocalize_state），然后在本 Lua 文件中用该导出的表替代硬编码的 RelocalizeState 常量（并更新所有引用该表的地方，如 send_and_await 的状态判断），或者至少在文件顶部添加一条强约束注释指向 C++ 枚举定义位置以提醒同步；确保使用的标识符包括 RelocalizeState 和 send_and_await 以便定位修改点。src/cxx/util/localization/engine.cc (1)
168-173: 💤 Low value可简化冗余的服务可用性检查
service_is_ready()已是非阻塞探测；后置wait_for_service(std::chrono::seconds(0))亦为非阻塞（零超时时立刻返回，等价于再次检查当前状态），故该 AND 条件冗余。建议直接保留单一检查：♻️ 建议简化
- if (!session->client->service_is_ready() - && !session->client->wait_for_service(std::chrono::seconds(0))) { + if (!session->client->service_is_ready()) { warn("relocalize service unavailable: {}", config.service_name); session->end(failed_status("service unavailable: " + config.service_name)); return false; }🤖 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 168 - 173, The service availability check is redundant because session->client->service_is_ready() and session->client->wait_for_service(std::chrono::seconds(0)) are both non-blocking; remove the duplicate check and keep a single call (preferably session->client->service_is_ready()) in the relocalization precondition, leaving the warn(...) and session->end(failed_status(...)) behavior unchanged; update the conditional that currently references service_is_ready() and wait_for_service(...) so it only calls session->client->service_is_ready().
🤖 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 148-160: arm_timeout installs a timer that can fire before the
session's request_id is written, so the timer lambda may see *pending_id == -1
and call s->end(...) for the wrong id; to fix, delay creating/assigning the
timer until after the request id is known (i.e., call arm_timeout or create the
create_wall_timer and assign s->timeout_timer only after
track(result.request_id) / after writing the real request id), ensure the timer
closure captures the shared_ptr pending_id that points to the finalized id, and
keep the existing cancellation logic (s->timeout_timer->cancel() /
s->timeout_timer = ...) around the new placement so the timeout always observes
the correct request id when invoking s->end(failed_status(...), *pending_id).
- Around line 180-203: The async callback can run before *pending_id is set,
causing the lambda to call s->end(..., -1) and have Session::pending_id still
nullopt so the response is dropped; fix by making request_id assignment and
session tracking atomic before the call returns — i.e., obtain result.request_id
and set *pending_id and session->pending_id (or call
session->track(result.request_id)) before yielding control where the callback
may execute (move the assignment of *pending_id and the call to session->track
to occur synchronously prior to or as part of invoking
session->client->async_send_request, or protect the sequence with the same lock
used by end()), referencing pending_id, async_send_request, result.request_id,
session->track, and s->end.
In `@src/lua/action.lua`:
- Around line 92-95: When fn(x,y,yaw) returns false, do not always log "reloc
skip %s (in_flight)" and return false,nil; instead call api.relocalize_status()
to fetch the actual terminal status, include that status in the warning (use the
existing self:warn call) and return false along with the status so
try_relocalize_local_then_wide can distinguish "in_flight" from "service
unavailable" (i.e., replace the unconditional warn/return in the fn failure
branch with a call to api.relocalize_status(), log the returned status, and
return it).
---
Nitpick comments:
In `@src/cxx/util/localization/engine.cc`:
- Around line 168-173: The service availability check is redundant because
session->client->service_is_ready() and
session->client->wait_for_service(std::chrono::seconds(0)) are both
non-blocking; remove the duplicate check and keep a single call (preferably
session->client->service_is_ready()) in the relocalization precondition, leaving
the warn(...) and session->end(failed_status(...)) behavior unchanged; update
the conditional that currently references service_is_ready() and
wait_for_service(...) so it only calls session->client->service_is_ready().
In `@src/lua/action.lua`:
- Around line 8-13: 当前 Lua 文件中硬编码的 RelocalizeState 枚举可能与 C++ enum class
RelocalizeState 不同步，导致 send_and_await 的轮询判断静默失败；请修改绑定层以从 C++ 暴露一个常量表（例如在
relocalize_status() 返回中附带 state_name 或单独导出 api.relocalize_state），然后在本 Lua
文件中用该导出的表替代硬编码的 RelocalizeState 常量（并更新所有引用该表的地方，如 send_and_await
的状态判断），或者至少在文件顶部添加一条强约束注释指向 C++ 枚举定义位置以提醒同步；确保使用的标识符包括 RelocalizeState 和
send_and_await 以便定位修改点。
In `@src/lua/api.lua`:
- Line 23: The relocalize_status Lua type annotation is missing the estimated
pose fields; update the `@field` declaration for relocalize_status in
src/lua/api.lua to include estimated_x, estimated_y, estimated_z and
estimated_qx, estimated_qy, estimated_qz, estimated_qw (all as numbers) so
LSP/completion and type checks used by callers like
try_relocalize_local_then_wide can see the estimated_* fields returned by the
C++ binding (component.cc bind code around the relocalize return).
🪄 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: a9364807-486b-41b2-aef4-ab117ab96179
📒 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
| auto arm_timeout(const std::shared_ptr<Session>& s, const std::shared_ptr<std::int64_t>& pending_id) | ||
| -> 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, pending_id]() { | ||
| if (s->end(failed_status("request timeout: " + s->service_name), *pending_id)) | ||
| 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); | ||
| } |
There was a problem hiding this comment.
Minor: arm_timeout 在拿到 request_id 之前装填，超时回调里读到 *pending_id == -1。
定时器在 arm_timeout 内即开始计时；如果在 *pending_id = result.request_id（第 201 行）之前先触发（极端情况下 0.1s 也可能赶上 executor 抖动 / 进程冻结恢复），s->end(failed_status(...), -1) 会与 Session::pending_id（此时仍为 nullopt 或已被 track 写入真实 id）做对比，导致超时无法生效或对错误的请求生效。
建议把 arm_timeout 推迟到 track(result.request_id) 之后再装上，这样定时器闭包看到的 *pending_id 始终是真实 id；上一条 critical 修复同时解决此处。
🤖 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 148 - 160, arm_timeout
installs a timer that can fire before the session's request_id is written, so
the timer lambda may see *pending_id == -1 and call s->end(...) for the wrong
id; to fix, delay creating/assigning the timer until after the request id is
known (i.e., call arm_timeout or create the create_wall_timer and assign
s->timeout_timer only after track(result.request_id) / after writing the real
request id), ensure the timer closure captures the shared_ptr pending_id that
points to the finalized id, and keep the existing cancellation logic
(s->timeout_timer->cancel() / s->timeout_timer = ...) around the new placement
so the timeout always observes the correct request id when invoking
s->end(failed_status(...), *pending_id).
| auto pending_id = std::make_shared<std::int64_t>(-1); | ||
| arm_timeout(session, pending_id); | ||
| 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); | ||
| return true; |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
rclcpp Client async_send_request callback thread invocation timing multi-threaded executor
💡 Result:
In rclcpp, when using Client::async_send_request(request, callback) with a user-provided callback, the callback is scheduled as regular work in the executor and invoked by one of the executor's threads when the response arrives. This applies to MultiThreadedExecutor, where the exact thread depends on scheduling and callback group rules (MutuallyExclusive or Reentrant).[1][2][3][4] The response is detected via the RMW layer during executor's wait_for_work (rcl_wait/take), triggering handle_response in the executor thread, which sets the future and queues the user callback if provided.[1][5][6] Timing: The callback executes promptly after response receipt, subject to executor load, wait timeouts, and callback group contention. In MultiThreadedExecutor, multiple threads poll waitsets concurrently; whichever detects the response executes the associated work.[3] Without user callback (just future), use spin_until_future_complete to block until handled.[1] Official docs confirm callbacks from async_send_request are "scheduled in the executor".[1][2]
Citations:
- 1: https://docs.ros.org/en/rolling/p/rclcpp/generated/classrclcpp_1_1GenericClient.html
- 2: https://docs.ros2.org/galactic/api/rclcpp/classrclcpp_1_1Client.html
- 3: https://github.com/ros2/rclcpp/blob/master/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
- 4: http://docs.ros.org/en/rolling/How-To-Guides/Using-callback-groups.html
- 5: https://docs.ros2.org/galactic/api/rclcpp/classrclcpp_1_1ClientBase.html
- 6: https://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Client.html
🏁 Script executed:
# First, find the engine.cc file and examine the Session::end() implementation
find . -name "engine.cc" -o -name "engine.h" | head -20Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 116
🏁 Script executed:
# Look at the directory structure to understand the layout
ls -la src/cxx/util/localization/ 2>/dev/null || find . -type f -name "*engine*" | head -10Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 308
🏁 Script executed:
# Search for Session struct and end() method implementation
rg -A 20 "bool\s+end\(" --type cpp --type hRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 60
🏁 Script executed:
# Search for the track() method implementation
rg -A 5 "void\s+track\(" --type cpp --type hRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 60
🏁 Script executed:
# Search for arm_timeout implementation
rg -B 2 -A 15 "arm_timeout" --type cpp --type hRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 3180
🏁 Script executed:
cat -n src/cxx/util/localization/engine.cc | head -220Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 9455
🏁 Script executed:
cat -n src/cxx/util/localization/engine.hhRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 1638
🏁 Script executed:
# Check for brace initialization compliance in the snippet lines 180-203
sed -n '175,205p' src/cxx/util/localization/engine.cc | grep -E "(make_shared|auto.*=)"Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 370
🏁 Script executed:
# Search for any other async_send_request usages in the codebase to see if this pattern is repeated
rg "async_send_request" --type cpp --type h -B 3 -A 10Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 1368
关键：异步回调与 pending_id 赋值存在竞态，导致响应被丢弃。
session->client->async_send_request(...) 在第 183 行调用后即可在 executor 线程触发回调（尤其在多线程 executor 中），但 *pending_id = result.request_id 的赋值延迟到第 201 行。若回调在赋值前先执行：
- lambda 内
*pending_id仍为-1，调用s->end(status, -1)； - 此时
Session::pending_id仍为std::nullopt（track()未执行），触发 end() 中的过滤条件（行 104）：pending_id != *expected_id→nullopt != -1→ true，返回 false； - 响应被静默丢弃，
last_status滞留IN_FLIGHT，最终仅由arm_timeout触发超时失败。
建议在 async_send_request 前同步设置 *pending_id 和 session->pending_id，确保回调读取时已是正确的 request_id：
修复思路
auto pending_id = std::make_shared<std::int64_t>(-1);
- arm_timeout(session, pending_id);
auto s = session;
+ auto result = session->client->async_send_request(...)
+ [返回后同步赋值]
+ *pending_id = result.request_id;
+ session->track(result.request_id);
+ arm_timeout(session, pending_id); // 在赋值后创建超时或在持锁状态下完成「dispatch + 记录 id」的原子化，防止回调抢先读取 -1。
🤖 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 180 - 203, The async
callback can run before *pending_id is set, causing the lambda to call
s->end(..., -1) and have Session::pending_id still nullopt so the response is
dropped; fix by making request_id assignment and session tracking atomic before
the call returns — i.e., obtain result.request_id and set *pending_id and
session->pending_id (or call session->track(result.request_id)) before yielding
control where the callback may execute (move the assignment of *pending_id and
the call to session->track to occur synchronously prior to or as part of
invoking session->client->async_send_request, or protect the sequence with the
same lock used by end()), referencing pending_id, async_send_request,
result.request_id, session->track, and s->end.
| 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.
Minor: fn(...)==false 时日志/返回值与 C++ 真实失败原因不一致。
src/cxx/util/localization/engine.cc 的 Impl::send 在以下两种情况都返回 false：
session->begin()失败（确实 in_flight）；- service 不可用（且会同步把
last_status标为 FAILED："service unavailable: ..."）。
这里直接打 "reloc skip %s (in_flight)" 并 return false, nil，会让上层 try_relocalize_local_then_wide 也丢掉「service unavailable」的诊断状态，无法区分这两类。建议在 fn 返回 false 时再 api.relocalize_status() 取一次末态返回，便于上层告警/降级判断：
📝 建议
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()
+ self:warn(string.format("reloc skip %s | %s", mode, tostring(st and st.message or "in_flight")))
+ 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, When fn(x,y,yaw) returns false, do
not always log "reloc skip %s (in_flight)" and return false,nil; instead call
api.relocalize_status() to fetch the actual terminal status, include that status
in the warning (use the existing self:warn call) and return false along with the
status so try_relocalize_local_then_wide can distinguish "in_flight" from
"service unavailable" (i.e., replace the unconditional warn/return in the fn
failure branch with a call to api.relocalize_status(), log the returned status,
and return it).
1 participant
接入重定位
PR摘要：集成重定位功能
概述
本PR将外部重定位仓库 rmcs_relocation 集成到 rmcs-navigation 中，新增重定位服务调用链与 Lua 接口，支持初始（Initial）、局部（Local）和广域（Wide）三种重定位模式，并在CI中检出该依赖仓库以供构建。
主要改动
依赖与构建
<depend>rmcs_relocation</depend>。C++实现
src/cxx/util/localization/engine.hh / engine.cc
src/cxx/component.cc
Lua层
src/lua/action.lua
src/lua/api.lua
移除/替换
代码变更量（大致）
注意点 / 后续检查建议