Conversation
- Extract gimbal yaw controller into separate class with P-controller, first-order low-pass filter, acceleration limiting, and Lua override - Add cancel_target() to cancel active navigation goals - Implement update_chassis_mode() with string-to-enum mapping - Add chassis_vel_override/clear_chassis_vel_override for straight-line control - Add enable_local_obstacle output for terrain crossing - Harden lua_sync() with blackboard schema validation and context readiness checks - Harden lua_init() with blackboard validity assertion - Remove dead rotate_chassis output
- Add navigate_straight() for straight-line navigation with perpendicular offset correction and optional chassis velocity override - Add crossing-rough-terrain.lua task: step_down mode, perpendicular gimbal, disable local obstacle, straight-line crossing, then restore - Add set_local_obstacle/cancel_target action wrappers - Add rough_terrain_begin/final to blackboard defaults with lswitch getter - Wire test endpoint to trigger rough terrain on rswitch UP
概览本PR为RMCS导航库添加了一个完整的万向轴（gimbal）偏航角（yaw）控制系统，包括新的NavigationGimbalController类、扩展的Navigation API以支持yaw参数、Lua绑定以控制gimbal和本地障碍规避，以及一个新的越野地形穿越任务。共计+450行/-30行代码。 变更内容导航系统与万向轴控制集成
时序图sequenceDiagram
participant Lua as Lua脚本
participant Comp as Navigation<br/>Component
participant Nav as Navigation
participant Gimbal as GimbalController
participant ROS2 as ROS2输出
Lua->>Comp: update_gimbal_direction(yaw)
Comp->>Gimbal: set_lua_target_yaw(yaw)
Gimbal->>Gimbal: 存储Lua override目标
Lua->>Comp: navigate_to(x, y, yaw)
Comp->>Nav: send_target(x, y, yaw)
Nav->>Nav: 创建Target{x, y, yaw}<br/>设置active_goal
Comp->>Gimbal: update()
Gimbal->>Nav: check_active_goal()
Nav-->>Gimbal: 返回(goal_x, goal_y, goal_yaw)
Gimbal->>Gimbal: 计算target_yaw<br/>应用yaw速度平滑<br/>和加速度限制
Gimbal->>ROS2: 输出gimbal_velocity
Comp->>Nav: check_bottom_yaw()
Nav-->>Comp: 返回当前yaw
Gimbal->>Nav: check_position()
Nav-->>Gimbal: 返回(x, y, yaw)
sequenceDiagram
participant Nav as Navigation
participant Goal as PoseStamped<br/>话题
Goal->>Nav: 接收pose消息
Nav->>Nav: parse_pose_yaw()<br/>提取orientation
Nav->>Nav: 创建Target{x, y, yaw}
Nav->>Nav: 设置active_goal<br/>发送目标
Note over Nav: yaw感知的目标转发
代码审查工作量估计🎯 4 (复杂) | ⏱️ ~60 分钟 理由: 涉及多个相互关联的文件（C++实现、头文件、Lua脚本、构建配置）；包含新C++类的完整实现，含线程安全机制（互斥锁、线程安全的Lua目标设置）；多个新的Lua API绑定和脚本任务；yaw数学计算和速度平滑算法需要验证；Component集成涉及多个新成员和生命周期方法；虽然变更集中在单一功能上，但跨越了C++和Lua两个技术栈，且包含状态管理、参数加载、控制流等多个维度。 可能相关的PR
诗句
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
⚔️ Resolve merge conflicts
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: 8
🧹 Nitpick comments (1)
src/cxx/navigation.cc (1)
227-234: 💤 Low value可选重构:析构函数与
cancel_target()逻辑完全重复。两段实现一字不差(只是局部变量名不同),可以让析构函数直接复用
cancel_target(),降低后续维护时两处不同步的风险。♻️ 建议
~Impl() { - auto current_handle = std::exchange(current_goal_handle, std::shared_ptr<GoalHandle>{}); - reset_active_goal(); - ++latest_request_id; - - if (current_handle) - client->async_cancel_goal(current_handle); + cancel_target(); }Also applies to: 263-270
🤖 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/navigation.cc` around lines 227 - 234, The ~Impl() destructor duplicates the cancel_target() logic; refactor by making the destructor call cancel_target() instead of repeating the code. Replace the body of ~Impl() with a call to cancel_target(), ensuring cancel_target() performs the same operations on current_goal_handle, reset_active_goal(), ++latest_request_id, and client->async_cancel_goal(...); if cancel_target()'s signature or exception safety differs, adjust it (e.g., make it noexcept or suitable for destructor use) so it can be safely invoked from ~Impl().
🤖 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/navigation_gimbal_controller.cc`:
- Around line 57-69: The smoothing currently hardcodes kControlDt = 1e-3 which
incorrectly treats acc_limit_ as if the loop runs at 1 kHz; in
NavigationGimbalController::smooth_yaw_speed replace the fixed kControlDt with
the real elapsed time (dt) since the last update (use the controller's last
timestamp or monotonic clock), clamp dt to a sensible positive range, then
compute max_step = acc_limit_ * dt and apply that to the delta when limiting
acceleration; ensure you still use smooth_alpha_, speed_command_ (prev) and
target_speed clamping as before and update any stored timestamp after computing
the filtered speed.
In `@src/cxx/navigation_gimbal_controller.hh`:
- Around line 2-6: The header navigation_gimbal_controller.hh is not
self-contained because it uses std::numeric_limits<double>::quiet_NaN() and
std::numbers::pi_v<double> in member initializers (used around the members
initialized with NaN and pi) but does not include <limits> or <numbers>; fix by
adding `#include` <limits> and `#include` <numbers> to the header so the
class/constructors that reference numeric_limits and numbers (e.g., the members
initialized with quiet_NaN() and pi_v<double>) can be used by any translation
unit that includes this header.
In `@src/cxx/navigation.cc`:
- Around line 217-220: The log uses the general-format precision specifier
"{:.1}" which treats precision as significant digits; update the format string
in the info(...) call that logs forward position (the call using "forward {} ->
({:.1}, {:.1}, yaw={:.2f}rad)", topic, position.x, position.y, yaw) to use
fixed-point with two decimal places (e.g. replace both "{:.1}" occurrences with
"{:.2f}") so position.x and position.y print as expected fixed two-decimal
coordinates.
In `@src/lua/action.lua`:
- Around line 85-87: The cancel_target implementation only calls
api.cancel_target() but doesn't clear the cached goal so action:bind() will
resend it; update action:cancel_target to also clear self.target (set it to nil)
after or before calling api.cancel_target(), ensuring the internal state is
reset and the target won't be reissued by action:bind() on the next cycle.
- Around line 117-147: In action:navigate_straight validate that target and
blackboard.user have numeric, non-NaN x/y before computing dx/dy/length; if any
coordinate is missing or NaN, clear the goal (set self.target to {x=NaN,y=NaN}),
call self:cancel_target() and return immediately to avoid propagating NaNs into
length/ux/uy and calls like api.set_chassis_vel_override() and self:navigate();
keep the existing short-distance check (length < 0.01) after this validation.
In `@src/lua/endpoint/test.lua`:
- Around line 26-30: run_rough_terrain currently appends a new coroutine every
time it's called (via scheduler:append_task calling crossing_rough_terrain),
which can create concurrent tasks that race updating chassis mode,
local_obstacle and navigation targets; to fix, add a running guard or
task-cancellation check inside run_rough_terrain to prevent queuing another task
while one is active (e.g. maintain a boolean like rough_terrain_running set
before append_task and cleared when crossing_rough_terrain finishes, or query
and cancel existing scheduler tasks for crossing_rough_terrain before
appending), ensuring only one rough-terrain task modifies state at a time.
In `@src/lua/task/crossing-rough-terrain.lua`:
- Around line 30-54: The current selection uses distance to
begin_ours/begin_them even when forward_center is false, causing wrong faction
choice on reverse traversal; change the comparison to use the actual route start
based on forward_center (i.e., if forward_center then compare user to
begin_ours/begin_them else compare user to final_ours/final_them), then set
begin/final accordingly and continue to compute from/to as before (referencing
variables forward_center, begin_ours, begin_them, final_ours, final_them, begin,
final, from, to).
- Around line 60-63: The gimbal direction is computed as the segment tangent
using math.atan(dy, dx) but needs to be perpendicular; modify the computation of
perpendicular_yaw (the variable set from math.atan) to add or subtract math.pi/2
(e.g., perpendicular_yaw = math.atan(dy, dx) + math.pi/2) so the value passed to
action:update_gimbal_direction is orthogonal to the path; adjust the sign if the
opposite perpendicular orientation is desired and normalize the angle if your
codebase expects a wrapped range.
---
Nitpick comments:
In `@src/cxx/navigation.cc`:
- Around line 227-234: The ~Impl() destructor duplicates the cancel_target()
logic; refactor by making the destructor call cancel_target() instead of
repeating the code. Replace the body of ~Impl() with a call to cancel_target(),
ensuring cancel_target() performs the same operations on current_goal_handle,
reset_active_goal(), ++latest_request_id, and client->async_cancel_goal(...); if
cancel_target()'s signature or exception safety differs, adjust it (e.g., make
it noexcept or suitable for destructor use) so it can be safely invoked from
~Impl().
🪄 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: a52c7a64-0a94-405f-a3ca-adff6a658031
📒 Files selected for processing (11)
CMakeLists.txtsrc/cxx/component.ccsrc/cxx/navigation.ccsrc/cxx/navigation.hhsrc/cxx/navigation_gimbal_controller.ccsrc/cxx/navigation_gimbal_controller.hhsrc/lua/action.luasrc/lua/api.luasrc/lua/blackboard.luasrc/lua/endpoint/test.luasrc/lua/task/crossing-rough-terrain.lua
| auto NavigationGimbalController::smooth_yaw_speed(double target_speed) -> double { | ||
| constexpr double kControlDt = 1e-3; | ||
|
|
||
| target_speed = std::clamp(target_speed, -speed_max_, speed_max_); | ||
|
|
||
| const auto alpha = std::clamp(smooth_alpha_, 0.0, 1.0); | ||
| const auto prev = speed_command_; | ||
| auto filtered = prev + alpha * (target_speed - prev); | ||
|
|
||
| if (acc_limit_ > 0.0) { | ||
| const auto max_step = acc_limit_ * kControlDt; | ||
| const auto delta = std::clamp(filtered - prev, -max_step, max_step); | ||
| filtered = prev + delta; |
There was a problem hiding this comment.
不要把加速度限幅绑死在固定 1 ms 周期上。
acc_limit_ 是按真实时间定义的，但这里用常量 1e-3 计算每次允许步进。只要 update() 不是稳定 1 kHz，限幅就会明显失真；常见控制频率下响应会慢很多。建议改成基于上一帧时间戳的真实 dt。
🤖 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/navigation_gimbal_controller.cc` around lines 57 - 69, The smoothing
currently hardcodes kControlDt = 1e-3 which incorrectly treats acc_limit_ as if
the loop runs at 1 kHz; in NavigationGimbalController::smooth_yaw_speed replace
the fixed kControlDt with the real elapsed time (dt) since the last update (use
the controller's last timestamp or monotonic clock), clamp dt to a sensible
positive range, then compute max_step = acc_limit_ * dt and apply that to the
delta when limiting acceleration; ensure you still use smooth_alpha_,
speed_command_ (prev) and target_speed clamping as before and update any stored
timestamp after computing the filtered speed.
| #include <Eigen/Geometry> | ||
| #include <rclcpp/node.hpp> | ||
| #include <rmcs_executor/component.hpp> | ||
| #include <mutex> | ||
| #include <optional> |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
# First, find and read the navigation_gimbal_controller.hh file
find . -name "navigation_gimbal_controller.hh" -type fRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 120
🏁 Script executed:
# Also check for any .cc or .cpp files related to this
find . -name "*navigation_gimbal_controller*" -type fRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 162
🏁 Script executed:
# Read the header file to check its content and verify the claims
wc -l ./src/cxx/navigation_gimbal_controller.hhRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 123
🏁 Script executed:
# Read the entire header file to check includes and usage
cat -n ./src/cxx/navigation_gimbal_controller.hhRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 1946
🏁 Script executed:
# Check the .cc file to see if it includes <limits> and <numbers>
cat -n ./src/cxx/navigation_gimbal_controller.cc | head -30Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 1673
头文件缺少 <limits> 和 <numbers> 头，自包含性不成立。
此头文件在成员初始化器中直接使用了 std::numeric_limits<double>::quiet_NaN()（第 35、38 行）和 std::numbers::pi_v<double>（第 42 行），但这两个标准库头仅在 .cc 文件中引入。任何单独包含此头文件的编译单元都会编译失败。请将依赖补到头文件本身，确保其能独立被包含并编译。
修复建议
`#include` <Eigen/Geometry>
`#include` <rclcpp/node.hpp>
`#include` <rmcs_executor/component.hpp>
+#include <limits>
`#include` <mutex>
+#include <numbers>
`#include` <optional>📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| #include <Eigen/Geometry> | |
| #include <rclcpp/node.hpp> | |
| #include <rmcs_executor/component.hpp> | |
| #include <mutex> | |
| #include <optional> | |
| `#include` <Eigen/Geometry> | |
| `#include` <rclcpp/node.hpp> | |
| `#include` <rmcs_executor/component.hpp> | |
| `#include` <limits> | |
| `#include` <mutex> | |
| `#include` <numbers> | |
| `#include` <optional> |
🧰 Tools
🪛 Clang (14.0.6)
[error] 2-2: 'Eigen/Geometry' file not found
(clang-diagnostic-error)
🤖 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/navigation_gimbal_controller.hh` around lines 2 - 6, The header
navigation_gimbal_controller.hh is not self-contained because it uses
std::numeric_limits<double>::quiet_NaN() and std::numbers::pi_v<double> in
member initializers (used around the members initialized with NaN and pi) but
does not include <limits> or <numbers>; fix by adding `#include` <limits> and
`#include` <numbers> to the header so the class/constructors that reference
numeric_limits and numbers (e.g., the members initialized with quiet_NaN() and
pi_v<double>) can be used by any translation unit that includes this header.
| info( | ||
| "forward {} -> ({:.1}, {:.1}, yaw={:.2f}rad)", topic, position.x, position.y, | ||
| yaw); | ||
| }); |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
In C++ std::format, what does the format spec "{:.1}" produce for a double like 1.567 — is it "1.6" or "2"? Is the precision interpreted as significant digits (general format) when no presentation type is given?
💡 Result:
For a double like 1.567 with the format specifier "{:.1}", std::format produces "1.6"[1][2][3]. This is because when no presentation type (like 'f', 'g', etc.) is specified but precision is given, it uses std::to_chars with std::chars_format::general and the specified precision (1), which formats to 1 significant digit after rounding, resulting in "1.6"[1][4][2]. The precision is interpreted as the number of significant digits in the general format[5][2][6].
Citations:
- 1: https://en.cppreference.com/cpp/utility/format/spec
- 2: https://ch.cppreference.com/w/cpp/utility/format/spec.html
- 3: https://en.cppreference.com/w/cpp/utility/format/spec.html
- 4: https://en.cppreference.com/cpp/utility/to_chars
- 5: https://timsong-cpp.github.io/cppwp/std20/format.string.std
- 6: https://fmt.dev/dev/syntax/
🏁 Script executed:
# First, find and examine the navigation.cc file
fd -name "navigation.cc" -type fRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 309
🏁 Script executed:
# Check if the file exists and examine the specific lines
if [ -f "src/cxx/navigation.cc" ]; then
wc -l src/cxx/navigation.cc
sed -n '210,225p' src/cxx/navigation.cc
fiRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 681
🏁 Script executed:
# Search for fmt or std::format usage in the codebase
rg -t cpp "fmt::|std::format" --max-count 20Repository: Alliance-Algorithm/rmcs-navigation
Length of output: 883
🏁 Script executed:
cat > /tmp/test_format.cpp << 'EOF'
`#include` <iostream>
`#include` <format>
int main() {
double val1 = 1.567;
double val2 = 0.0123;
double val3 = 1.234;
// Test {:.1} format specifier
std::cout << "Testing {:.1} format:\n";
std::cout << "1.567 with {:.1}: " << std::format("{:.1}", val1) << "\n";
std::cout << "0.0123 with {:.1}: " << std::format("{:.1}", val2) << "\n";
std::cout << "1.234 with {:.1}: " << std::format("{:.1}", val3) << "\n";
// Test {:.2f} format specifier for comparison
std::cout << "\nTesting {:.2f} format:\n";
std::cout << "1.567 with {:.2f}: " << std::format("{:.2f}", val1) << "\n";
std::cout << "0.0123 with {:.2f}: " << std::format("{:.2f}", val2) << "\n";
std::cout << "1.234 with {:.2f}: " << std::format("{:.2f}", val3) << "\n";
return 0;
}
EOF
g++ -std=c++20 /tmp/test_format.cpp -o /tmp/test_format 2>&1
/tmp/test_formatRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 205
🌐 Web query:
std::format general format precision specification C++20 how many significant digits
💡 Result:
In C++20 std::format, for the general format (type 'g', 'G', or no type with precision specified), the precision specifies the maximum number of significant digits, equivalent to printf's %g specifier. This is because it calls std::to_chars with chars_format::general and the specified precision, which behaves "in the style of printf in the "C" locale with the given precision" using the 'g' specifier [1][2]. For printf %g, precision is the maximum number of significant digits: the number is rounded to that many significant digits, then formatted in f or e style (whichever is shorter), with trailing zeros removed [3][4][5]. Default precision is 6 if unspecified [6][7]. Unlike fixed ('f') where precision is digits after decimal, or scientific ('e') where it's digits after decimal, general uses significant digits [8].
Citations:
- 1: https://en.cppreference.com/cpp/utility/to_chars
- 2: https://timsong-cpp.github.io/cppwp/std20/charconv.to.chars
- 3: https://en.cppreference.com/cpp/io/c/printf
- 4: https://linux.die.net/man/3/printf
- 5: https://xiedantibu.github.io/cplusplus/www.cplusplus.com/reference/cstdio/printf/default.htm
- 6: https://en.cppreference.com/cpp/utility/format/spec
- 7: https://en.cppreference.com/w/cpp/utility/format/spec.html
- 8: https://stackoverflow.com/questions/77818357/how-does-stdsetprecision-affect-general-floating-point-formatting
🌐 Web query:
cppreference std::format precision meaning without type specifier
💡 Result:
In C++ std::format, the precision specifier (introduced by '.') can only be used with floating-point and string types; for other types like integers, it is invalid.[1][2] When a type specifier is absent (no 'f', 'd', etc.): For floating-point types without a type specifier: - If precision is specified, it formats using std::to_chars with std::chars_format::general and the given precision.[1][2] - If no precision is specified, it uses the shortest round-trip format.[1] For example: std::format("{:.5}", 3.14159) uses general format with precision 5.[1] For non-floating-point types (e.g., integers), precision is not allowed and may cause undefined behavior or errors, as the precision field "can only be used with floating-point and string types."[1][2][3]
Citations:
- 1: https://en.cppreference.com/w/cpp/utility/format/spec.html
- 2: https://en.cppreference.com/cpp/utility/format/spec
- 3: https://github.com/paulkazusek/std_format_cheatsheet
日志格式说明符 {:.1} 使用通用格式（significant digits），而非小数位数。
在 C++20 std::format 中，当未指定类型字符但给定精度时，浮点数使用通用格式（等同于 printf 的 %g）。此时精度表示最大有效位数而非小数位数：{:.1} 将 1.567 舍入为 "2"，将 0.0123 格式化为 "0.01"。对于机器人位置坐标，这种行为会产生误导性的输出。
改用 {:.2f}（固定格式，2 位小数）可得到预期结果："1.57" 和 "0.01"。
建议修复
- info(
- "forward {} -> ({:.1}, {:.1}, yaw={:.2f}rad)", topic, position.x, position.y,
- yaw);
+ info(
+ "forward {} -> ({:.2f}, {:.2f}, yaw={:.2f}rad)", topic, position.x,
+ position.y, yaw);📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| info( | |
| "forward {} -> ({:.1}, {:.1}, yaw={:.2f}rad)", topic, position.x, position.y, | |
| yaw); | |
| }); | |
| info( | |
| "forward {} -> ({:.2f}, {:.2f}, yaw={:.2f}rad)", topic, position.x, | |
| position.y, yaw); | |
| }); |
🤖 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/navigation.cc` around lines 217 - 220, The log uses the
general-format precision specifier "{:.1}" which treats precision as significant
digits; update the format string in the info(...) call that logs forward
position (the call using "forward {} -> ({:.1}, {:.1}, yaw={:.2f}rad)", topic,
position.x, position.y, yaw) to use fixed-point with two decimal places (e.g.
replace both "{:.1}" occurrences with "{:.2f}") so position.x and position.y
print as expected fixed two-decimal coordinates.
| function action:cancel_target() | ||
| api.cancel_target() | ||
| end |
There was a problem hiding this comment.
cancel_target() 需要同时清掉 self.target。
这里只调用底层取消接口，但 action:bind() 在 Line 19-46 还会持续重发 self.target。外部如果单独调用 action:cancel_target()，旧目标会在下一轮被重新发回去，取消实际上不会生效。
修复建议
function action:cancel_target()
+ self.target = { x = NaN, y = NaN }
api.cancel_target()
end📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| function action:cancel_target() | |
| api.cancel_target() | |
| end | |
| function action:cancel_target() | |
| self.target = { x = NaN, y = NaN } | |
| api.cancel_target() | |
| 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 85 - 87, The cancel_target implementation
only calls api.cancel_target() but doesn't clear the cached goal so
action:bind() will resend it; update action:cancel_target to also clear
self.target (set it to nil) after or before calling api.cancel_target(),
ensuring the internal state is reset and the target won't be reissued by
action:bind() on the next cycle.
| function action:navigate_straight(target, tolerance, timeout, speed) | ||
| if tolerance == nil then tolerance = 0.15 end | ||
| if timeout == nil then timeout = 30 end | ||
|
|
||
| local start = { x = blackboard.user.x, y = blackboard.user.y } | ||
|
|
||
| local dx = target.x - start.x | ||
| local dy = target.y - start.y | ||
| local length = math.sqrt(dx * dx + dy * dy) | ||
|
|
||
| if length < 0.01 then | ||
| self:warn("navigate_straight: start and target are too close") | ||
| self.target = { x = NaN, y = NaN } | ||
| self:cancel_target() | ||
| return | ||
| end | ||
|
|
||
| local ux = dx / length | ||
| local uy = dy / length | ||
| local nx = -uy | ||
| local ny = ux | ||
|
|
||
| local elapsed = 0 | ||
| local interval = 0.5 | ||
|
|
||
| while elapsed < timeout do | ||
| local user = blackboard.user | ||
|
|
||
| if speed then | ||
| api.set_chassis_vel_override(speed * ux, speed * uy) | ||
| end |
There was a problem hiding this comment.
先校验当前位置和目标点，再计算直线轨迹。
如果 target 或 blackboard.user 里有 NaN/未就绪值，length、ux、uy 会一路变成 NaN，后面还会继续传给 set_chassis_vel_override() 和 self:navigate()。这里最好在 Line 121 前做一次快速失败，并清掉当前目标。
修复建议
function action:navigate_straight(target, tolerance, timeout, speed)
if tolerance == nil then tolerance = 0.15 end
if timeout == nil then timeout = 30 end
local start = { x = blackboard.user.x, y = blackboard.user.y }
+ if util.check_nan(target.x, target.y) or util.check_nan(start.x, start.y) then
+ self:warn("navigate_straight: position or target is not ready")
+ self.target = { x = NaN, y = NaN }
+ self:cancel_target()
+ return
+ end
local dx = target.x - start.x
local dy = target.y - start.y🤖 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 117 - 147, In action:navigate_straight
validate that target and blackboard.user have numeric, non-NaN x/y before
computing dx/dy/length; if any coordinate is missing or NaN, clear the goal (set
self.target to {x=NaN,y=NaN}), call self:cancel_target() and return immediately
to avoid propagating NaNs into length/ux/uy and calls like
api.set_chassis_vel_override() and self:navigate(); keep the existing
short-distance check (length < 0.01) after this validation.
| local run_rough_terrain = function() | ||
| scheduler:append_task(function() | ||
| crossing_rough_terrain(true) | ||
| end) | ||
| end |
There was a problem hiding this comment.
避免重复排队多个 rough terrain 任务。
这里每次触发都会 append_task() 一个新协程；如果开关重复进 UP，多个任务会并发修改底盘模式、local_obstacle 和导航目标，互相打架。最好在调度前加运行中标记，或忽略/取消已有任务。
🤖 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/endpoint/test.lua` around lines 26 - 30, run_rough_terrain currently
appends a new coroutine every time it's called (via scheduler:append_task
calling crossing_rough_terrain), which can create concurrent tasks that race
updating chassis mode, local_obstacle and navigation targets; to fix, add a
running guard or task-cancellation check inside run_rough_terrain to prevent
queuing another task while one is active (e.g. maintain a boolean like
rough_terrain_running set before append_task and cleared when
crossing_rough_terrain finishes, or query and cancel existing scheduler tasks
for crossing_rough_terrain before appending), ensuring only one rough-terrain
task modifies state at a time.
| -- 离哪一个入口近就用哪个阵营的坐标 | ||
| local dist_ours = math.sqrt( | ||
| (user.x - begin_ours.x) ^ 2 + (user.y - begin_ours.y) ^ 2 | ||
| ) | ||
| local dist_them = math.sqrt( | ||
| (user.x - begin_them.x) ^ 2 + (user.y - begin_them.y) ^ 2 | ||
| ) | ||
|
|
||
| local begin, final | ||
| if dist_ours <= dist_them then | ||
| begin = begin_ours | ||
| final = final_ours | ||
| else | ||
| begin = begin_them | ||
| final = final_them | ||
| end | ||
|
|
||
| local from, to | ||
| if forward_center then | ||
| from = begin | ||
| to = final | ||
| else | ||
| from = final | ||
| to = begin | ||
| end |
There was a problem hiding this comment.
反向穿越时阵营选择基准错了。
这里总是拿 begin_* 和当前位置比距离，再决定使用哪一组坐标。forward_center == false 时，实际入口在 final_* 一侧；如果车已经靠近 final_them，这段逻辑仍可能选到另一侧路线。建议按“当前行进方向的起点”来选阵营。
修复建议
- local dist_ours = math.sqrt(
- (user.x - begin_ours.x) ^ 2 + (user.y - begin_ours.y) ^ 2
- )
- local dist_them = math.sqrt(
- (user.x - begin_them.x) ^ 2 + (user.y - begin_them.y) ^ 2
- )
+ local entry_ours = forward_center and begin_ours or final_ours
+ local entry_them = forward_center and begin_them or final_them
+ local dist_ours = math.sqrt(
+ (user.x - entry_ours.x) ^ 2 + (user.y - entry_ours.y) ^ 2
+ )
+ local dist_them = math.sqrt(
+ (user.x - entry_them.x) ^ 2 + (user.y - entry_them.y) ^ 2
+ )🤖 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/task/crossing-rough-terrain.lua` around lines 30 - 54, The current
selection uses distance to begin_ours/begin_them even when forward_center is
false, causing wrong faction choice on reverse traversal; change the comparison
to use the actual route start based on forward_center (i.e., if forward_center
then compare user to begin_ours/begin_them else compare user to
final_ours/final_them), then set begin/final accordingly and continue to compute
from/to as before (referencing variables forward_center, begin_ours, begin_them,
final_ours, final_them, begin, final, from, to).
| local dx = to.x - from.x | ||
| local dy = to.y - from.y | ||
| local perpendicular_yaw = math.atan(dy, dx) | ||
| action:update_gimbal_direction(perpendicular_yaw) |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
# First, verify the file exists and check its content around lines 60-63
if [ -f "src/lua/task/crossing-rough-terrain.lua" ]; then
echo "=== File found, reading lines 50-75 for context ==="
wc -l src/lua/task/crossing-rough-terrain.lua
echo "---"
sed -n '50,75p' src/lua/task/crossing-rough-terrain.lua
else
echo "File not found at src/lua/task/crossing-rough-terrain.lua"
# Try to find it
find . -name "*crossing-rough-terrain*" -o -name "*rough-terrain*" 2>/dev/null | head -20
fiRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 686
🏁 Script executed:
# Read the beginning of the file to understand task context
head -60 src/lua/task/crossing-rough-terrain.luaRepository: Alliance-Algorithm/rmcs-navigation
Length of output: 1460
修正云台朝向计算：缺少垂直方向偏转
当前代码计算的是路段的切线方向（平行于路段），而非垂直方向。math.atan(dy, dx) 得到的是 from → to 的方向角。根据任务描述（第9行）和代码注释（第62行）的要求，云台方向应该垂直于路段方向，因此需要在计算结果上加上 ± math.pi / 2。
现在云台会沿着路段方向对齐，与 rough terrain 过起伏路段场景的设计意图不一致。
🤖 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/task/crossing-rough-terrain.lua` around lines 60 - 63, The gimbal
direction is computed as the segment tangent using math.atan(dy, dx) but needs
to be perpendicular; modify the computation of perpendicular_yaw (the variable
set from math.atan) to add or subtract math.pi/2 (e.g., perpendicular_yaw =
math.atan(dy, dx) + math.pi/2) so the value passed to
action:update_gimbal_direction is orthogonal to the path; adjust the sign if the
opposite perpendicular orientation is desired and normalize the angle if your
codebase expects a wrapped range.
1 participant
feat(nav): gimbal controller, navigate_straight, and new navigation tasks
拉取请求摘要
概述
此PR为RMCS导航系统引入了一个完整的万向节（gimbal）控制器、直线导航动作以及粗糙地形穿越任务支持。
核心功能新增
万向节控制器实现
NavigationGimbalController类，实现了P控制器、一阶低通滤波和加速度限制功能直线导航动作
navigate_straight()动作：用于直线导航，支持垂直偏移修正和可选的底盘速度覆盖粗糙地形穿越任务
crossing-rough-terrain.lua任务模块，自动化地形穿越流程导航API扩展
Navigation 类更新
send_target(double x, double y, double yaw)新重载check_bottom_yaw() const -> double：查询底部万向节yaw角check_active_goal() const -> std::tuple<double, double, double>：查询活跃目标cancel_target() -> void：取消当前目标Lua API 扩展
action:set_local_obstacle(enable)：控制局部障碍物避免action:cancel_target()：取消当前导航目标action:navigate_straight(target, tolerance, timeout, speed)：直线导航原语输出和模式管理
chassis_mode、enable_local_obstacle、base_link_yawrotate_chassis输出chassis_vel_override_active_）支持直线控制内部改进
Lua同步增强
lua_sync()以验证lua_blackboard模式lua_init()增加endpoint黑板有效性验证黑板更新
测试端点
文件变更统计