Gripper task 5 - #506
Draft
daviddgonzalez wants to merge 145 commits into
Draft
Conversation
…r for the target and filter
…onar_follower mission
Add a closed-loop visual-servo BT node (HoneOverTarget) that centers the sub in body-frame XY over a YOLO detection from the down-facing camera, holding depth and yaw. Plumb a separate down-cam detection/image stream into Context (subscribe-once, shared) behind declared ROS params, and add a 'camera' selector port so vision nodes (HoneOverTarget, DetectTarget) can pick the front or down stream. The settle gate is progress-based (distance moved from a privately recorded step-start pose, not proximity to the shared last_goal) and corrections only act on YOLO frames newer than the last one used, so the loop neither spams goals on small steps nor re-corrects off a pre-move detection. Add hone_over_table.xml (HoneOverTable): a centering-only subtree wrapping HoneOverTarget in a Timeout with an AlwaysSuccess fallback so the parent mission degrades to dead-reckoning when the table is not detected.
Note the required sim override (down_image_topic:=/down_cam/image_raw) both at the param declaration in mission_planner_node and in subjugator_bringup's README, and explain that down_detect_topic matches when the down YOLO node uses namespace:=yolo_down.
Introduces select_target_logic.hpp with parse_labels, labels_for_role, and pick_best functions, decoupled from ROS/yolo_msgs types. Adds a gtest target (6 cases) covering CSV parsing, role mapping, confidence filtering, exclusion, and no-match cases.
Integrate S3 SelectTarget work with the base branch's HoneOverTarget->CenterCamera node rename and new octagon/center-camera missions. Conflicts resolved in mission_planner_node.cpp (keep CenterCamera registration + add SelectTarget) and sub9_missions.xml (take renamed hone_over_target.xml + new mission includes + add select_target.xml).
Cherry-picks the generic servo-actuation BT leaf (target=dropper|gripper| torpedo, angle) from the mechanisms work so Task 5 S4 can command the gripper. Calls the Servo service directly via clients held in Context. Bumps Servo.srv angle to uint16 to match upstream and keep the leaf byte-identical.
AcquireTable's hone used CenterCamera, a P-only loop that copies the sub's current orientation into every goal. Attitude is therefore unregulated, so a pitch disturbance latches, tilts the down-cam, grows the image-x error, and the relative current+step goal runs away. Measured ex,ey growing 0.10 -> 0.47 while AcquireTable still "succeeded" through its AlwaysSuccess degrade, leaving the sub off-target so the following SelectTarget exhausted its budget. LockOverTarget wraps LockTargetXY in the same Fallback[RosTimeout, AlwaysSuccess] degrade shell as HoneOverTarget, but commands an absolute world point with a fixed level orientation and hover depth, which removes both the moving-setpoint windup and the pitch latch. table_z is a port rather than a constant because it is the plane the pixel ray is cast onto and it differs per target; only S2 is wired up, where the table lies exactly on that plane. Three-trial driver runs on this box: before, 3/3 died at S3 with the table acquired but no lock; after, 3/3 lock electric_box and reach S4. S4 and S5 still use CenterCamera and still show the same divergence.
The first live run never reached the mission: the readiness gates waited for /odometry/filtered before the settle, but the sim launches PAUSED, so /clock does not advance, cameras do not render, and robot_localization's ekf_node sits in "Waiting for clock to start..." without advertising the topic at all. sim_bringup.py is what unpauses gz, so no topic-level gate can pass before it. Reordered: a gz world-control-service gate before the settle (the only thing observable while paused), then /clock actually advancing and each ready topic carrying data after it. The clock check now requires two distinct sim times -- counting messages passes against a frozen sim, which republishes one stamp. Teardown escalates to SIGKILL: a run left four processes behind, two of them gz servers that ignore SIGTERM, and merely reporting them leaves exactly the orphaned stack preflight exists to catch. find_stale also matches yolo_ros by install path, so debug_node/tracking_node stop surviving alongside yolo_node.
…t warning Two defects the first completed calib run exposed. The mission node's down-cam subscriber defaults to the real robot's /down_camera/rgb/image_raw, which nothing publishes in sim, so CenterCamera logged "waiting for down image size" every tick and would have burned its whole timeout without seeing a frame -- while the harness reported a healthy anchor and healthy perception. task5_sim.launch.py passes down_image_topic when it starts the mission itself; the harness owns mission start, so it must too. TaskSpec.mission_params carries it, applied before stage params so a stage can still override. Vehicle stats now cover the mission window only. Collection starts before the settle and robot_localization diverges to ~1e24 m until sim_bringup re-anchors it, which reported a path length of 6.1e25 m. trace.csv still holds everything. Stage.places gates the scorer's contradiction flag. "SUCCESS but nothing physically placed" is correct behaviour for a centering test, and a warning that fires on every run is one people learn to ignore -- which is the one thing the ground-truth check must never become.
… 0.046, SUCCESS, exit 0)
The octagon down-cam model emits a tiny, very confident phantom 'table' box in a frame corner in essentially every frame, and scores the real table far below it: measured on this model, the phantom is ~0.03x0.06 of frame at conf 0.85 while the real table is ~0.60x0.40 at conf 0.24-0.42. best_detection ranked purely on score, so every table consumer locked onto the phantom. That is not a cosmetic mis-pick. In an S2 trial the gripper sat at x=-7.24 with the true table at (-7.25, 14.0) — already over it — yet LockTargetXY cast its ray through the phantom's pixel, estimated the table at (-6.97, 14.36), and chased that point for its whole budget. The lock then degrades, and the sub enters S3 off-target, which is what times SelectTarget out. Add Select::kLargestArea and rank by bbox area under it. Area separates the two by an order of magnitude where confidence inverts them, and the table is by far the largest thing in a down-cam frame. It is opt-in per call, not global, because the same helper picks the small grasp props, where "biggest box wins" would be actively wrong: select_from() falls back to confidence for any unrecognised value so a typo cannot enable it by accident. Plumbed as a `select` port on CenterCamera and LockTargetXY, passed explicitly at every call site the way miss_frames already is — "largest" for the three table sites, "confidence" for the prop and basket-marker sites. Verified end to end against a synthetic stream carrying both boxes, with the image centre at (480,300): select="largest" logs ex=-0.625 ey=0.000 (the real table), select="confidence" logs ex=-0.958 ey=-0.667 (the phantom). 71 tests pass; the three affected trees instantiate.
621e095 set select="largest" at the three mission table sites but missed three tuning/test trees that call the leaf directly with label="table" and no select, so they fell through to CenterCamera/LockTargetXY's own default of "confidence" and kept ranking the model's tiny corner phantom above the real table: hone_over_table_select_mission.xml CenterCamera center_camera_test_mission.xml CenterCamera (passes every OTHER knob) lock_target_xy_test_mission.xml LockTargetXY, tree LockTargetXYTest None of these is on a competition path, but they are what you reach for to debug or tune S2 centering, so they would have reproduced the phantom chase and read as "the fix did not work". LockTargetXYTest matters most: it is the geometry-check harness that introduces a known 0.35 m lateral offset to validate the camera basis signs, and it was casting its ray at the phantom, so any basis conclusion drawn from it was measured against a corner blob. Swept every label="table" site afterward: no best_detection consumer is left on the confidence default. The three remaining hits (acquire_table.xml:18,23, 24) are DescendUntilDetected / DetectTarget / SearchForTarget, which have no select port because they go through contains_label -- that is the presence half of the bug, still open.
Contributor
Author
|
Update, I've been working on sim testing and working on grasping this week further. In the pool the new motion improvements will help a lot, but I want it to be working in sim within a couple of days hopefully. Some of the biggest bugs were with the buoyancy and gravity being at the same point, locking on to tiny phantom objects because of a high confidence, and relative moves not acknowledging the pitch of the sub so I had it raycast to go to a fixed point. I coordinated with Jack and further specified what x,y,z and angles the model could improve from. He's done a great job w the training of the model and I appreciate his help! |
Review follow-up, comments only. Two accuracy fixes: The "see Select below for the phantom measurements" cross-reference pointed at a DIFFERENT sample than the one the geometry rests on. Select records the 621e095 two-box sample (phantom ~0.03x0.06 at conf 0.85); the 0.15 floor was sized against the later 3-pose sample (artifacts 0.018-0.081 at conf 0.21-0.79). "~5x the largest measured artifact" only reproduces against the latter (0.39/0.081 = 4.8); read against Select's numbers it would be ~216x. Name the band at the cross-reference so the factor is checkable, and point the "props are INSIDE the artifact band" claim at a band the header actually states. "(and non-finite, e.g. NaN)" over-generalized: +inf does NOT disable the filter -- enabled() is true and it rejects every box, i.e. it lands in the > 1.0 bullet, not the <= 0 one. Only NaN and -inf disable. 29 tests still pass.
Review follow-up on 87e8126. SizeGate::passes' comment claimed callers "hold RUNNING before reaching this, so it is a backstop". That was true when CenterCamera was the only caller. 87e8126 falsified it: DetectTarget lets the fail-closed path fall through to a real FAILURE, and SearchForTarget relies on it DIRECTLY -- unverifiable means "not seen", which is what keeps the spiral running. A maintainer reading "backstop" could reasonably relax it to fail-open and silently turn SearchForTarget's cold start into "phantom sighted, stop spiralling". State what each caller actually does with it instead. SearchForTarget also discarded the readiness bit entirely, so a silent image topic made it spiral the whole pattern and report "exhausted N waypoints" -- indistinguishable from a genuine miss, after burning the full 60 s on a gate that could never pass. Warn, without adding the hold the other nodes have. Spell out why DescendUntilDetected's hold sits below the timeout guard: it reads as misplaced, and "fixing" it for locality would remove the only bound on that hold and pin the node RUNNING forever on a dead image topic. Comments and one log line; no behaviour change at the 0.0 default. 29 tests pass.
Review follow-up on 202632f. Every other node treats presence and selection as separate questions; this one does not -- its presence check IS `best_detection(...) != nullptr`, so an unfiltered phantom is not merely ranked first, it is reported as "the table is in frame" and then ray-cast. That is the specific observed failure (estimate frozen 0.44 m off while the gripper was already over the real table, chased to the 20 s timeout), and it was the only motivation in this change not written down anywhere. Comment only.
Review follow-up on 9111ec3. The select paragraph in both reusable subtrees carries an "every call site passes it explicitly" note; the new min_area_frac paragraph did not. It needs it more than select does. The leaf reads the port into a pre-initialized 0.0 local, so a future call site that simply omits the attribute gets the gate silently disabled -- the phantom bug back, with nothing in the log to say so. A wrong select produces visibly wrong ranking; a missing min_area_frac produces nothing at all. Also fix the noun at the transport_and_place site: it was copy-pasted from approach_and_grasp and says "props" / "the real prop", but that call targets a basket MARKER (red_cross / warning). The 0.04-0.11 figure holds for both; the noun did not, and the surrounding comments in that file all say "marker". Comments only.
ce3611c wrote "SILENTLY DISABLED -- the phantom bug back" inside a comment. A double hyphen is illegal in an XML comment body, so both files stopped parsing. Use a colon. Self-inflicted and caught by the parse sweep straight after; the same defect class as the pre-existing one in lock_target_xy_test_mission.xml:72, which is why that file is skipped by the sweep. Every other mission XML parses.
min_conf 0.40 rejected the real table before min_area_frac ever saw it. The model scores the real table 0.24-0.42 (recorded in test_detection_gate.cpp's own comment) and its corner phantom 0.85, so the floor sat above most of the real range. Worse than per-frame recall suggests: DescendUntilDetected wants 3 CONSECUTIVE hits and MissGate zeroes the streak on any miss, so a floor inside the jitter band never accumulates a confirmation. Measured against a synthetic stream at the real 0.31: descent ran to max_steps and S2 failed outright. 0.20 is a temporary value to get Task 5 working in sim while the YOLO model is being retrained. Once the model is good, make confidence 0.80 or higher, most likely. min_area_frac="0.15" does the real discrimination meanwhile -- every measured artifact is <=0.081 of frame, so none survive the area floor at any confidence. Also forward min_conf through LockOverTarget, which did not have the port at all: presence would have passed at 0.20 while the lock kept its own 0.30 default, so the lock could go kLost on a table the presence checks had just confirmed, and degrade to dead-reckon with nothing in the log. Re-verified after the change, clean graph, no orphaned publishers: real table at the measured 0.31 -> confirmed (3 frames) after 0 steps, LockTargetXY converges phantom only (conf 0.85) -> 0 confirmations, reached max_steps, FAILURE
max_steps bounds how many descend steps we take, not how deep they reach: 12 x 0.20 m from z=-0.35 reaches -2.75, while the tabletop sits at -0.85 with props on it at -0.80. A measured run sank to -1.41, a half metre BELOW the tabletop, where a down camera cannot see the table at any lateral offset -- so the descent AND the spiral meant to rescue it were both doomed. DescendUntilDetected gains a min_z port checked against the current pose, defaulting to -inf so existing trees are unaffected. AcquireTable sets min_z="-0.55", keeping 0.30 m of clearance over the tabletop. The descend is also wrapped in ForceSuccess. AcquireTable's subtree is a Sequence, so a descend FAILURE aborted it before the Fallback could tick, which made SearchForTarget dead code -- the existing comment there reasoned only about DetectTarget failing after a successful descent, but at a laterally offset start it is the descent that fails, and that is exactly when the spiral is needed. Descending 2.4 m cannot correct a 1.5 m lateral offset. Measured 2026-07-28 (run_task 5, stage `approach`, `near` start): the descent correctly rejected all 159 phantom boxes over 452 frames, then failed the subtree at 45 s and never spiralled.
robot_localization intermittently blows up at bring-up on this box (observed: a first odom sample at y=4.5e41 settling into a ~1644 m bias that never recovers). The controller then holds station in that corrupted frame and flies the sub tens of metres from the commanded start. Every odom-vs-world number in the report is meaningless afterwards, including the drift stat meant to catch exactly this. sim_bringup.py already prints a WARN and exits 0, so the run continued and reported a MISSION failure (exit 1) for an infrastructure fault. Check the anchor offset as a bring-up postcondition instead, before the data gates -- a diverged anchor still produces a healthy-looking topic stream, so those gates pass and the mission runs on a bad frame. max_anchor_offset defaults to 1.0 m. Measured here: 0.008 and 0.013 m when the EKF behaved, 1658 m when it diverged -- three orders of magnitude apart, so the threshold only has to land somewhere between. Not tightened to sim_bringup's own 0.02 m "clean" line, because a merely imperfect settle is worth reporting, not worth throwing the run away over. Also adds a task-5 `approach` stage: S2 alone from the `near` start, the only short stage that does not begin over the table. It makes the down-cam approach path -- descend, spiral, lock -- testable for 125 sim-s of declared timeouts instead of OctagonMission's 1089.
Brings the branch up to date with 21 commits of main, and fixes the CI failure on this branch: main deleted mil_common/mil_preflight (#451) while the branch kept it, so the clang-tidy step's `git diff --name-only origin/main` started listing every preflight header and died on a pre-existing latent error in uis/ftxui/dialog.h. Merging drops the package from the branch and picks up #527, which disables that step entirely. Conflict notes: - #519 replaced the central registerNodeType list in mission_planner_node with per-file REGISTER() macros. Added REGISTER for the 11 Task-5 nodes that were only in the deleted list (LockTargetXY, SelectTarget, SelectBasket, SelectFaceSymbol, CenterCamera, RecordTargetScale, ConfirmGraspByScale, DescendUntilDetected, SearchForTarget, RosTimeout, RosDelay) -- without these the trees fail at creation, not at compile. - mission_planner links WHOLE_ARCHIVE (keeps the REGISTER constructors from being dropped) *and* yaml-cpp (still used by mission_planner_node). - SonarFollower: kept our .cpp/.hpp split and the target_freq gate that HomeOnOctagonPinger relies on to ignore the Task-4 pinger; took main's ping-counting algorithm, required_passing_pings port, child restart and failure propagation. Output sonar_z/sonar_w -> sonar_yaw_deg, since the old acos() heading was unsigned and could not express a left turn. - OctagonPingerMission keeps both sides: descend to -0.6 m first, then the yaw sweep for the first ping. - AlignYaw: main's last-goal base (odom fallback) with our quat_math helpers. - PublishGoalPose keeps keep_current_pos_abs alongside main's keep_current_xy_abs / keep_current_ori_abs. - start_gate_mission uses main's {gate_label} but the <Action> element form, since DetectTarget is a StatefulActionNode on this branch. - header_only.cpp: roll_style.hpp/pitch_style.hpp -> spin_style.hpp. Verified: full workspace builds (43 packages, 0 failures); pre-commit clean; OctagonMission, LockTargetXYTest, HoneOverTableSelect, SelectOnly and MechGraspTest all create and tick.
…l pgid
Two colcon test failures surfaced by the main merge.
test_btbudget pinned roll_style.xml as known-unparsable. It was unparsable
because of a missing space between two attributes
(momentum_ms="1500"ctx="{ctx}"); main rewrote the file and it now parses, so
the pin is stale. Dropped it -- the pin exists to catch a NEW malformed file,
not to require an old one stay broken.
test_lifecycle::test_kill_stale_accepts_a_different_signal asserted a killpg
against fake group 7 while FakeKills left os.getpgrp() real. kill_stale
refuses to signal its own group, so on any runner whose process group happens
to be 7 -- a CI container, as it turns out -- the assertion silently took that
branch and saw no call. FakeKills now pins the harness's own group to a
sentinel far outside the range the tests use, which closes the whole
collision class rather than just pid 7; the one test that deliberately
exercises the own-group guard uses the same sentinel.
Verified: the old test reproduces the CI failure locally under a forced
os.getpgrp()==7 and the new one passes; 450 subjugator_bringup tests pass.
perception's ament_lint_auto walks its own source dir, which contains the nested pcd package, so it lints pcd's files as well. #573 added the uncrustify skip to pcd/CMakeLists.txt but not to the parent, so the same five files are still checked here and main has been red on it since that merge. Reformatting is not the fix: running ament_uncrustify --reformat over those files then leaves clang-format wanting 17 lines back, because the repo style (4-space/Allman) is one uncrustify's ROS profile cannot express. clang-format in pre-commit is the source of truth, which is what pcd/CMakeLists.txt already says. This mirrors that skip one level up. Verified: colcon test --packages-select perception now exits 0, uncrustify no longer appears in test_results, and the other five linters still run clean.
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
3 participants
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.
This has all my work as I'm making it for task 5