Gripper task 5 - #506
Draft
daviddgonzalez wants to merge 140 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
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