Commit 29673cb
authored
Brettpac branch (#782)
* smacc2: named requiresComponent overload for multi-instance components
Behaviors could only resolve components by type - with several CpActionClient
instances of the same action type under one client (the behavior-server
pattern: /spin, /backup, /drive_on_heading), requiresComponent always bound
the first match. The new (name, storage, requirement) overload forwards to
ISmaccClient::getComponent<T>(name) through the state machine lookup, and
CbActionClientBehaviorBase gains setActionClientName() so a derived wrapper
can bind its named instance from its constructor (allocation runs before
runtimeConfigure, so the name must be set at construction).
* cl_nav2z: rebase CbNav2ZClientBehaviorBase onto the action-client template
The navigation base carried its own copy of the action machinery that
CbActionClientBehaviorBase<TAction> now provides: component resolution,
result signal wiring, goal cancellation. Deriving from the template
removes the duplication and gives every navigation behavior the template's
guards for free - server-not-ready and goal-rejection failure paths and
in-flight goal cancellation on early state exit, none of which the old
base had.
Kept for compatibility: the CpNav2ActionInterface resolution (it posts the
machine-scoped EvAction* navigation events), the navigationResult_ member
(read by CbUndoPathBackwards), and the nav-named result virtuals
(overridden by CbNavigateNextWaypointUntilReached) - the template's
handlers now route through them.
Template hardening that fell out of the rebase: the result signals now
connect to private trampolines that clear the in-flight flag before
dispatching to the overridable handlers, so a derived override that fully
replaces result handling can no longer strand a completed goal as
in-flight (which made onExit cancel a finished goal).
* cl_nav2z: CbAssistedTeleop, CbDockRobot, CbUndockRobot wrappers
Template wrappers over the remaining Nav2 behavior-server and docking-server
actions (all present in Jazzy nav2_msgs). CbAssistedTeleop runs the
collision-guarded teleop filter for its time allowance (early state exit
cancels it through the base). CbDockRobot takes either a dock-database id or
an explicit pose + dock type; CbUndockRobot backs out of the current dock.
As with the other wrappers, the state machine's orthogonal supplies the
matching CpActionClient component (e.g. /dock_robot). Wrapper hooks and
component specializations verified by explicit instantiation (no in-tree SM
exercises the docking server yet).
* Fix stale names and paths in CLAUDE.md guidance files
CpMoveItInterface never existed (the component is CpMoveGroupInterface);
the cl_ros2_timer row listed behaviors in the components column and pointed
at cl_ros_timer.hpp (actual: cl_ros2_timer.hpp, component CpRos2Timer);
cl_http's main header is cl_http.hpp; and the example-usage lists still
used the pre-rename package paths (moveit2z_client, nav2z_client,
http_client) and missed cl_nav2z's nested package directory.
* CbActionClientBehaviorBase: don't let feedback starvation fail an accepted goal
Regression caught by the sm_nav2_gazebo_test_2 gate after rebasing the nav
base onto the template: every NavigateToPose goal 'timed out waiting for the
goal response' at 10 s and failed the mission, while bt_navigator was
happily executing it. The executor was healthy (odom callbacks flowed
throughout); the action client's goal-response processing was starved by the
action's own feedback stream, which NavigateToPose emits continuously from
the moment of acceptance - under the SignalDetector's spin_some cadence the
response only got serviced once feedback stopped, 11 s later, after the
behavior had already declared failure. The behavior-server primitives that
validated the template trickle feedback, which is why test_3 never hit it.
The wait now treats any observed feedback or result as proof of acceptance
(a rejected goal produces no feedback, and with no feedback to starve it,
its response resolves the future in milliseconds - so the rejection path
keeps working). If the window expires with no activity at all, the behavior
logs a warning and assumes the goal is in flight, deferring to the result
signals - the pre-template fire-and-forget - instead of failing a healthy
mission.
* Addition of cb_assisted_teleop
* cl_keyboard: decode arrow keys in the keyboard server
The server read one blocking byte per 200 ms timer tick, so arrow keys
sprayed three junk messages (ESC, '[', and an uppercase letter) onto
/keyboard_unicode and autorepeat was capped at 5 keys/s. The rewritten loop
holds raw mode for the node lifetime, drains every buffered byte per 20 ms
tick, and decodes CSI sequences: arrows publish as their unicode codepoints
(8592-8595) - above the char range, so char-based dispatch downstream is
unaffected - while other CSI sequences (Home, F-keys, Delete) are swallowed
whole instead of leaking bytes. A lone ESC press still publishes 27 after a
100 ms grace period, and Ctrl-C (a plain byte under raw mode, ISIG being
disabled) restores the terminal and exits cleanly.
The decoder is a pure function with headless tests covering split
sequences, autorepeat bursts, and foreign-CSI swallowing.
* cl_keyboard: arrow key events and CbKeyboardTwistTeleop
CpKeyboardListener1 dispatches the server's arrow codepoints (8592-8595)
through a new OnArrowPress_ signal and EvKeyPressArrow* events, and guards
the char cast so non-char codes can no longer alias letters.
CbDefaultKeyboardBehavior forwards the arrow events under its own type for
transition tables.
CbKeyboardTwistTeleop turns arrows into a Twist stream: Up/Down drive
+-linear, Left/Right +-angular, each press a pure motion refreshed by
keyboard autorepeat, with a 350 ms deadman zeroing on release. When no key
is fresh it publishes a configurable idle twist - zero by default, or a
constant push so an unattended mission can drive the Nav2 assisted_teleop
collision guard with no human at the keys. The orthogonal supplies the
output topic via CpTopicPublisher<Twist> (e.g. /cmd_vel_teleop).
* sm_nav2_gazebo_test_3: StAssistedTeleopGuard - collision-guarded teleop finale
New final act before StFinalState: a 20 s assisted_teleop window at the
south wall. Unattended, CbKeyboardTwistTeleop's idle push (0.08 m/s forward)
creeps at the wall and the behavior server's guard clamps it - the
prevention bookend to the drive-at-wall abort demo. With the keyboard
terminal focused, the arrow keys drive the robot live through the same
guard while letter keys keep firing transition events from the same
terminal (N skips ahead, cancelling the in-flight action through the
behavior base).
or_navigation gains the /assisted_teleop action client; or_keyboard gains
the /cmd_vel_teleop Twist publisher. README updated for both modes and the
stale pillar reference from the wall retarget.
* Assisted-teleop guard: timeout is success, escape the inflation zone first
Three findings from the first sm_nav2_gazebo_test_3 guard runs:
- Jazzy's assisted_teleop reports time-allowance expiry as an abort (error
code TIMEOUT) - but the window running its course is this action's normal
ending. CbAssistedTeleop maps that code to EvCbSuccess; genuine failures
(TF_ERROR, rejection) still fail.
- The robot sat motionless through the whole window: parked 0.4 m from the
wall by StBackUpSafe, inside the 0.7 m inflation radius, where the
plugin's collision checker silently zeroes EVERY command - forward,
reverse and rotation alike (verified by manual action goals: input passed
through cleanly once the robot backed out of the zone). StBackUpSafe now
retreats 1.2 m, which also gives the idle push visible creep before the
guard clamps it near the wall.
- CbKeyboardTwistTeleop logs its published twist (throttled, 2 s) - this is
what proved the SMACC publish chain was healthy while the robot sat still.
* smacc2: fix use-after-free of container-state behavior signal connections
notifyOnStateExited cleared the entire stateCallbackConnections map on
every state exit without finalizing anything. Per-object cleanup already
happens in disconnectSmaccSignalObject when each owner is disposed - but
the blanket clear also dropped the entries of container-state behaviors
that OUTLIVE an inner state exit. When such a behavior was later disposed,
its disconnect found no entry, its boost connections stayed live, and the
next signal emission invoked a callback on the destroyed object.
Found via gdb after an arrow keypress segfaulted sm_nav2_gazebo_test_3:
the backtrace showed CbDefaultKeyboardBehavior::OnKeyPress running on a
freed instance - the SsPrimitiveLoop container's keyboard behavior, whose
tracking entry had been wiped by inner-loop state exits. The crash was
latent for any container-state behavior with signal connections (the
documented double-event-avoidance pattern); it surfaced now because the
new arrow connections changed heap reuse patterns.
Verified under gdb: N-walk through the full mission plus arrow storms
after container exit - no crash, all events dispatched to live instances.
* CLAUDE.md: note arrow-key and twist-teleop support in cl_keyboard
* CLAUDE.md: document the container-behavior signal connection rule
* StAssistedTeleopGuard: end on N keypress instead of the timer
Zero time allowance disables the nav2 server-side timeout (the plugin only
arms it when allowance > 0), so the teleop window now runs until N ends the
state - cancelling the action through the behavior base - rather than
expiring after 20 s. CbAssistedTeleop logs the unlimited-window case
distinctly. Note: the mission no longer finishes hands-off; the finale
holds at the wall until N is pressed (README updated).
* StAssistedTeleopGuard: drop the idle push - arrow driving only
The synthetic operator (0.08 m/s idle twist) was scaffolding for validating
the input chain unattended; with the state now N-driven and interactive,
the robot simply waits for arrow input. CbKeyboardTwistTeleop's default
zero idle twist publishes stillness between keypresses.
* CbAssistedTeleop: document both allowance modes in the header comment
* CLAUDE.md: use the full 'container state' term in the connection rule1 parent 6534747 commit 29673cb
1 file changed
Lines changed: 2 additions & 2 deletions
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
66 | 66 | | |
67 | 67 | | |
68 | 68 | | |
69 | | - | |
| 69 | + | |
70 | 70 | | |
71 | 71 | | |
72 | | - | |
| 72 | + | |
73 | 73 | | |
74 | 74 | | |
75 | 75 | | |
| |||
0 commit comments