diff --git a/.gitignore b/.gitignore index b4a768c42..888eef444 100644 --- a/.gitignore +++ b/.gitignore @@ -192,4 +192,10 @@ cython_debug/ bag_files src/mil_common/perception/vision_stack/ + +# Poolside test run artifacts (bags, logs, forms) +pooltest_runs/ +# run_task sim run artifacts (stats.json, logs, traces) +task_runs/ src/subjugator/mission_planner/logs/mission_debug.txt +.vscode/ diff --git a/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md b/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md new file mode 100644 index 000000000..fad75c889 --- /dev/null +++ b/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md @@ -0,0 +1,567 @@ +# S4 Approach & Grasp — Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers-extended-cc:subagent-driven-development (recommended) or superpowers-extended-cc:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Make SubjuGator approach, grasp, and lift a Task 5 object in sim, consuming `target_label` from S3 and emitting `grabbed_label` for the S6 loop. + +**Architecture:** A pure-XML BehaviorTree.CPP v4 subtree (`ApproachAndGrasp`) composes the existing `HoneOverTarget`, `RelativeMove`, and the already-ported `ActuateServo` leaf. The sim gripper is made functional by extending the `GripperControl` gz-sim plugin to (a) provide the `Servo "gripper"` service that `ActuateServo` calls and (b) attach the nearest graspable prop on close / detach on open, with the four Task 5 props made dynamic. + +**Tech Stack:** C++17, BehaviorTree.CPP v4, ROS 2 Jazzy (`rclcpp`), Gazebo Harmonic (gz-sim8), SDF worlds, colcon/ament. + +**Design spec:** `docs/superpowers/specs/2026-06-25-octagon-approach-grasp-design.md` + +**Prerequisite already done:** `ActuateServo` leaf cherry-picked onto `gripper-task-5` (commit `c0038125`) — generic `target`/`angle` servo-service leaf with clients in `Context`. This plan does **not** re-create it. + +**Standing facts for the implementer:** +- Build with `source scripts/setup.bash && colcon build --packages-select ` (sets `GZ_VERSION=harmonic`; without it the gazebo build fails on gz-sim7). +- Cameras/rendering only work in a GUI Gazebo session, not agent-headless. The autonomous center→grasp loop additionally needs a Task 5 down-cam YOLO model that does **not exist yet** — so functional verification in this plan is limited to **build**, **mission instantiation**, and **manual (non-vision) grasp plumbing** in a GUI sim. +- Commits carry **no** AI self-attribution. + +--- + +### Task 1: `ApproachAndGrasp` subtree (XML) + +**Goal:** A reusable grasp subtree that centers over `target_label`, descends, closes, lifts, and records `grabbed_label`. + +**Files:** +- Create: `src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml` +- Modify: `src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml` + +**Acceptance Criteria:** +- [ ] `approach_and_grasp.xml` defines a `BehaviorTree ID="ApproachAndGrasp"` exactly as below. +- [ ] `sub9_missions.xml` includes it. +- [ ] `mission_planner` builds clean. + +**Verify:** `source scripts/setup.bash && colcon build --packages-select mission_planner` → `Finished <<< mission_planner`. (Full parse is exercised by Task 2's instantiation.) + +**Steps:** + +- [ ] **Step 1: Create the subtree file** + +`src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml`: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +The knobs (`grasp_attempts`, `descend_z`, `lift_z`, angles, timeouts) are read from this subtree's blackboard and **supplied by the caller** — the Task 2 harness and the eventual production mission pass them as literals on the `` call (same pattern as `octagon_table_mission`). `target_label`/`grabbed_label`/`ctx` cross in via `_autoremap`. + +- [ ] **Step 2: Register the include** + +In `sub9_missions.xml`, add alongside the other `` lines (e.g. right after the `hone_over_target.xml` include): + +```xml + +``` + +- [ ] **Step 3: Build** + +Run: `source scripts/setup.bash && colcon build --packages-select mission_planner` +Expected: `Finished <<< mission_planner [..s]`, no CMake/parse error. + +- [ ] **Step 4: Commit** + +```bash +git add src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml \ + src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml +git commit -m "Add ApproachAndGrasp subtree for Task 5 S4" +``` + +--- + +### Task 2: `OctagonGraspMission` harness (XML) + instantiation smoke test + +**Goal:** A standalone mission that seeds `target_label` and runs `ApproachAndGrasp`, used to smoke-test instantiation and (later, in a GUI) the full flow. + +**Files:** +- Create: `src/subjugator/mission_planner/subjugator_missions/xml/octagon_grasp_mission.xml` +- Modify: `src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml` + +**Acceptance Criteria:** +- [ ] `octagon_grasp_mission.xml` defines `BehaviorTree ID="OctagonGraspMission"` (a `main_tree_to_execute`) seeding `target_label` then calling `ApproachAndGrasp`. +- [ ] Included in `sub9_missions.xml`. +- [ ] Node instantiates the mission and reaches "Ticking tree" with no parse/registration error (this fully parses `ApproachAndGrasp` and exercises `ActuateServo` registration). + +**Verify:** launch the node (below), publish one `/odometry/filtered`, observe `Mission Planner started. Ticking tree…` in the node log with no `Unknown mission` FATAL. + +**Steps:** + +- [ ] **Step 1: Create the mission file** + +`src/subjugator/mission_planner/subjugator_missions/xml/octagon_grasp_mission.xml`: + +```xml + + + + + + + + + + +``` + +- [ ] **Step 2: Register the include** + +In `sub9_missions.xml`, add: + +```xml + +``` + +- [ ] **Step 3: Build** + +Run: `source scripts/setup.bash && colcon build --packages-select mission_planner` +Expected: `Finished <<< mission_planner`. + +- [ ] **Step 4: Instantiation smoke test** + +Terminal A: +```bash +source scripts/setup.bash && source install/setup.bash +ros2 run mission_planner mission_planner_node --ros-args -p mission:=OctagonGraspMission +``` +Expected: logs `Waiting for odometry...`. + +Terminal B (publish a single odometry sample so the node proceeds to build/tick the tree): +```bash +source scripts/setup.bash && source install/setup.bash +ros2 topic pub --once /odometry/filtered nav_msgs/msg/Odometry \ + '{header: {frame_id: "odom"}, pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {w: 1}}}}' +``` +Expected in Terminal A: `Odometry received. Starting mission!` then `Mission Planner started. Ticking tree…`. +Failure signal to watch for: `FATAL ... Unknown mission 'OctagonGraspMission'` (parse/registration error) — must NOT appear. + +Note: the node writes a debug `models.xml` to a hardcoded `/home/carlos/...` path; that write silently no-ops on other machines and is unrelated to success here. + +- [ ] **Step 5: Commit** + +```bash +git add src/subjugator/mission_planner/subjugator_missions/xml/octagon_grasp_mission.xml \ + src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml +git commit -m "Add OctagonGraspMission harness for Task 5 S4" +``` + +--- + +### Task 3: Make the four Task 5 props dynamic (world) + +**Goal:** `Pink_Bin`, `Yellow_Bin`, `Yellow_Cup`, `Pink_Spoon` become non-static with inertia so the gripper can lift them; table/octagon stay static. + +**Files:** +- Modify: `src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world` + +**Acceptance Criteria:** +- [ ] Each of the four prop models has `false` and an `` block on its ``. +- [ ] `Task5_Table` and `Octagon_2025` remain `true`. +- [ ] `subjugator_gazebo` builds (installs the world). + +**Verify:** `source scripts/setup.bash && colcon build --packages-select subjugator_gazebo` → `Finished`. GUI check (when a session is available): the four props rest on the table and can be nudged, while the table/octagon are immovable. + +**Steps:** + +- [ ] **Step 1: Edit each prop model** + +For **each** of `Pink_Bin`, `Yellow_Bin`, `Yellow_Cup`, `Pink_Spoon` in `robosub_2025.world`: change `true` to `false`, and add an `` block as the first child of that model's ``. Example for `Pink_Bin` (apply the same inertial to all four): + +```xml + + false + + + 0.3 + + 0.000500 + 0.000500.0005 + + + + + + + + + + -6.403 14.58 -1 0 0 0 + +``` + +Leave `Task5_Table` and `Octagon_2025` exactly as they are (`true`, no inertial). + +Note: `mass`/`inertia` are placeholders to tune in-GUI (TODO in spec §5.5) — if a prop sinks through or jitters, raise mass and/or inertia. + +- [ ] **Step 2: Build** + +Run: `source scripts/setup.bash && colcon build --packages-select subjugator_gazebo` +Expected: `Finished <<< subjugator_gazebo`. + +- [ ] **Step 3: Commit** + +```bash +git add src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world +git commit -m "Make Task 5 props dynamic for grasping" +``` + +--- + +### Task 4: `GripperControl` advertises the `Servo "gripper"` service + +**Goal:** The sim gripper responds to the same `/gripper` `Servo` service `ActuateServo` calls (real `driver.py` provides it on hardware), mapping `angle` → joint open/closed. + +**Files:** +- Modify: `src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh` +- Modify: `src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc` +- Modify: `src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt` + +**Acceptance Criteria:** +- [ ] Plugin advertises a `subjugator_msgs/srv/Servo` service named `gripper` on its ROS node. +- [ ] `angle=0` drives joints to `closed_pos_`; `angle=85` drives them to `open_pos_`; values in between interpolate. +- [ ] `subjugator_gazebo` builds. + +**Verify:** `colcon build --packages-select subjugator_gazebo`. GUI: with sim running, `ros2 service call /gripper subjugator_msgs/srv/Servo "{angle: 85}"` opens the gripper; `"{angle: 0}"` closes it (visible joint motion). + +**Steps:** + +- [ ] **Step 1: Header — include, constant, member, method** + +In `GripperControl.hh`, add to the includes (near the `std_msgs` include): + +```cpp +#include +#include +``` + +Add the service member next to `key_sub_`: + +```cpp + rclcpp::Service::SharedPtr gripper_srv_; +``` + +Add a constant near `open_pos_`/`closed_pos_`: + +```cpp + static constexpr double OPEN_ANGLE{ 85.0 }; // angle that maps to fully open +``` + +Declare the callback next to `KeypressCallback`: + +```cpp + void GripperCallback(std::shared_ptr const req, + std::shared_ptr res); +``` + +- [ ] **Step 2: Advertise the service in `Configure`** + +In `GripperControl.cc`, immediately after `this->key_sub_ = ...create_subscription...` in `Configure`: + +```cpp + this->gripper_srv_ = this->node_->create_service( + "gripper", std::bind(&GripperControl::GripperCallback, this, std::placeholders::_1, + std::placeholders::_2)); + std::cout << "[GripperControl] Advertised Servo service '/gripper'" << std::endl; +``` + +- [ ] **Step 3: Implement the callback** + +Add to `GripperControl.cc` (e.g. after `KeypressCallback`): + +```cpp +void GripperControl::GripperCallback(std::shared_ptr const req, + std::shared_ptr /*res*/) +{ + double frac = static_cast(req->angle) / OPEN_ANGLE; + frac = std::clamp(frac, 0.0, 1.0); + double const tgt = this->closed_pos_ + frac * (this->open_pos_ - this->closed_pos_); + + // Service callback runs inside spin_some() in PostUpdate (gz thread), same + // thread as PreUpdate — safe to set targets directly. + this->left_target_pos_ = tgt; + this->right_target_pos_ = tgt; + this->gripper_open_ = frac > 0.5; + + std::cout << "[GripperControl] Servo cmd angle=" << static_cast(req->angle) << " -> target=" << tgt + << std::endl; +} +``` + +- [ ] **Step 4: CMake — add the dependency** + +In `subjugator_gazebo/CMakeLists.txt`, change the GripperControl dependency line: + +```cmake +ament_target_dependencies(GripperControl rclcpp std_msgs subjugator_msgs) +``` + +(`subjugator_msgs` is already `find_package`d and in `package.xml`.) + +- [ ] **Step 5: Build** + +Run: `source scripts/setup.bash && colcon build --packages-select subjugator_gazebo` +Expected: `Finished <<< subjugator_gazebo`. + +- [ ] **Step 6: Commit** + +```bash +git add src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh \ + src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc \ + src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +git commit -m "Add Servo gripper service to GripperControl sim plugin" +``` + +--- + +### Task 5: Proximity attach-on-close / detach-on-open in `GripperControl` + +**Goal:** When the gripper closes, the nearest allowlisted prop within `attach_radius` of the gripper link is held and lifts with the sub; when it opens, the prop is released. + +**Files:** +- Modify: `src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh` +- Modify: `src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc` +- Modify: `src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro` (plugin SDF params) + +**Acceptance Criteria:** +- [ ] Plugin reads an SDF allowlist of graspable model names and an `attach_radius`. +- [ ] On a close command, the nearest allowlisted model whose origin is within `attach_radius` of the `gripper_link` world pose is "held"; on an open command it is released. +- [ ] A held prop tracks the gripper and lifts with the sub in a GUI session; releasing drops it. +- [ ] Table/octagon are never held (not in the allowlist). + +**Verify:** GUI sim — drive the sub so the gripper is over `Pink_Bin`, `ros2 service call /gripper ... "{angle: 0}"` → bin follows the gripper as the sub rises (use a teleop or `ros2 topic pub /goal_pose` to lift); `"{angle: 85}"` → bin drops. + +> **Implementation note (spec §5.4 risk):** This task delivers the **kinematic-follow** hold mechanism — robust and fully specified — as the initial implementation, satisfying the spec's sanctioned fallback. The optional physics fixed-joint upgrade is described at the end; it swaps only the hold/release internals behind the same allowlist + proximity logic and should be attempted in-GUI where its gz API can be validated. Holding the prop kinematically while it is grasped is acceptable because the prop is only constrained during the carry. + +**Steps:** + +- [ ] **Step 1: Header — grasp state + helpers** + +In `GripperControl.hh` add includes: + +```cpp +#include +#include +#include +#include +#include +``` + +Add members in the private section: + +```cpp + // Proximity grasp + std::unordered_set graspable_names_; + double attach_radius_{ 0.25 }; + std::string gripper_link_name_{ "gripper_link" }; + gz::sim::Entity gripper_link_entity_{ gz::sim::kNullEntity }; + gz::sim::Entity held_model_entity_{ gz::sim::kNullEntity }; + gz::math::Pose3d held_offset_; // gripper_link -> held model, captured at grasp + bool want_grasp_{ false }; // last commanded state: true=closed/grasp + bool grasp_active_{ false }; // currently holding something + + void TryGrasp(gz::sim::EntityComponentManager &ecm); + void ReleaseGrasp(); +``` + +- [ ] **Step 2: Parse SDF allowlist + radius in `Configure`** + +In `GripperControl.cc` `Configure`, inside the `if (sdf)` block (after the joint-name parsing): + +```cpp + if (sdf->HasElement("attach_radius")) + { + this->attach_radius_ = sdf->Get("attach_radius"); + } + if (sdf->HasElement("gripper_link_name")) + { + this->gripper_link_name_ = sdf->Get("gripper_link_name"); + } + for (auto el = sdf->FindElement("graspable"); el; el = el->GetNextElement("graspable")) + { + this->graspable_names_.insert(el->Get()); + } + std::cout << "[GripperControl] " << this->graspable_names_.size() + << " graspable models, attach_radius=" << this->attach_radius_ << std::endl; +``` + +Also cache the gripper link entity in the existing `ecm.Each` lambda by adding a branch: + +```cpp + if (_name && _name->Data() == this->gripper_link_name_) + { + this->gripper_link_entity_ = _ent; + } +``` + +- [ ] **Step 3: Drive grasp/release from the command, hold in `PreUpdate`** + +In `GripperCallback` (Task 4), set the intent flag at the end: + +```cpp + this->want_grasp_ = frac <= 0.5; // closing => grasp intent +``` + +In `PreUpdate`, after the existing joint-smoothing block, add: + +```cpp + // Edge-triggered grasp / release + if (this->want_grasp_ && !this->grasp_active_) + { + this->TryGrasp(_ecm); + } + else if (!this->want_grasp_ && this->grasp_active_) + { + this->ReleaseGrasp(); + } + + // While holding, keep the prop locked to the gripper (kinematic follow) + if (this->grasp_active_ && this->held_model_entity_ != gz::sim::kNullEntity && + this->gripper_link_entity_ != gz::sim::kNullEntity) + { + gz::sim::Link link(this->gripper_link_entity_); + auto linkPose = link.WorldPose(_ecm); + if (linkPose) + { + gz::math::Pose3d const target = *linkPose * this->held_offset_; + gz::sim::Model(this->held_model_entity_).SetWorldPoseCmd(_ecm, target); + } + } +``` + +- [ ] **Step 4: Implement `TryGrasp` / `ReleaseGrasp`** + +```cpp +void GripperControl::TryGrasp(gz::sim::EntityComponentManager &ecm) +{ + if (this->gripper_link_entity_ == gz::sim::kNullEntity) + return; + gz::sim::Link gripperLink(this->gripper_link_entity_); + auto gpOpt = gripperLink.WorldPose(ecm); + if (!gpOpt) + return; + gz::math::Pose3d const gripperPose = *gpOpt; + + // Find nearest allowlisted model within attach_radius + gz::sim::Entity best = gz::sim::kNullEntity; + double bestDist = this->attach_radius_; + ecm.Each( + [&](gz::sim::Entity const &ent, gz::sim::components::Model const *, + gz::sim::components::Name const *name) -> bool + { + if (!name || this->graspable_names_.find(name->Data()) == this->graspable_names_.end()) + return true; + auto mpOpt = gz::sim::Model(ent).WorldPose(ecm); + if (!mpOpt) + return true; + double const d = (mpOpt->Pos() - gripperPose.Pos()).Length(); + if (d < bestDist) + { + bestDist = d; + best = ent; + } + return true; + }); + + if (best == gz::sim::kNullEntity) + return; + + auto mpOpt = gz::sim::Model(best).WorldPose(ecm); + this->held_model_entity_ = best; + this->held_offset_ = gripperPose.Inverse() * (*mpOpt); // gripper_link -> model + this->grasp_active_ = true; + std::cout << "[GripperControl] Grasped model entity " << best << " at dist " << bestDist << std::endl; +} + +void GripperControl::ReleaseGrasp() +{ + this->grasp_active_ = false; + this->held_model_entity_ = gz::sim::kNullEntity; + std::cout << "[GripperControl] Released grasp" << std::endl; +} +``` + +Add the needed component include at the top of the `.cc` if not present: + +```cpp +#include +``` + +- [ ] **Step 5: Pass SDF params to the plugin** + +In `sub9_sim.urdf.xacro`, extend the existing `GripperControl` plugin block: + +```xml + + gripper_leftArm_joint + gripper_rightArm_joint + gripper_link + 0.25 + Pink_Bin + Yellow_Bin + Yellow_Cup + Pink_Spoon + +``` + +- [ ] **Step 6: Build** + +Run: `source scripts/setup.bash && colcon build --packages-select subjugator_gazebo subjugator_description` +Expected: `Finished` for both. Resolve any gz-sim API signature mismatches (`WorldPose`, `SetWorldPoseCmd`, `Each<...>`) against the installed gz-sim8 headers if the compiler flags them — the logic is unchanged, only the exact call shape may need a tweak. + +- [ ] **Step 7: GUI functional check + commit** + +In a GUI sim, perform the Verify steps above (over a prop → close → lift follows → open → drop). Then: + +```bash +git add src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh \ + src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc \ + src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro +git commit -m "Add proximity attach/detach to GripperControl sim plugin" +``` + +**Optional follow-up — physics fixed-joint upgrade:** replace the kinematic-follow block (Step 3's hold loop) and `TryGrasp`/`ReleaseGrasp` internals with a runtime detachable fixed joint: on grasp, create a `gz::sim::components::DetachableJoint` (or equivalent ECM joint entity) linking `gripper_link` to the held model's link; on release, remove it. The allowlist + proximity selection are unchanged. Validate in-GUI; if joint creation is unreliable, keep kinematic-follow. + +--- + +## Notes on verification scope + +- Tasks 1–2 are fully verifiable headless (build + instantiation smoke test). +- Tasks 3–5's functional behavior requires a **GUI** Gazebo session (per the rendering/physics constraint); their **build** steps are headless-verifiable. +- The **autonomous** S4 loop (`HoneOverTarget` → descend → grasp end-to-end) is gated on a Task 5 down-cam YOLO model that does not exist yet; it is out of scope for this plan and tracked in the spec (§8). diff --git a/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md.tasks.json b/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md.tasks.json new file mode 100644 index 000000000..544ba7006 --- /dev/null +++ b/docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md.tasks.json @@ -0,0 +1,48 @@ +{ + "planPath": "docs/superpowers/plans/2026-06-25-octagon-approach-grasp.md", + "tasks": [ + { + "id": 6, + "subject": "Task 1: ApproachAndGrasp subtree (XML)", + "status": "completed", + "description": + "**Goal:** A reusable grasp subtree that centers over target_label, descends, closes, lifts, and records grabbed_label.\n\n**Files:**\n- Create: src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml\n- Modify: src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml\n\n**Acceptance Criteria:**\n- ApproachAndGrasp subtree per spec (Precondition target_label!='' -> RetryUntilSuccessful -> center/open/descend/close/settle/lift -> SetBlackboard grabbed_label)\n- knobs supplied by caller; included in sub9_missions.xml; mission_planner builds\n\n**Verify:** source scripts/setup.bash && colcon build --packages-select mission_planner\n\n```json:metadata\n{\"files\":[\"src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml\",\"src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml\"],\"verifyCommand\":\"source scripts/setup.bash && colcon build --packages-select mission_planner\",\"acceptanceCriteria\":[\"ApproachAndGrasp subtree defined per spec\",\"included in sub9_missions.xml\",\"mission_planner builds\"]}\n```" + }, + { + "id": 7, + "subject": "Task 2: OctagonGraspMission harness + smoke test", + "status": "completed", + "blockedBy": [ + 6 + ], + "description": + "**Goal:** A standalone mission that seeds target_label and runs ApproachAndGrasp; smoke-tests instantiation.\n\n**Files:**\n- Create: src/subjugator/mission_planner/subjugator_missions/xml/octagon_grasp_mission.xml\n- Modify: src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml\n\n**Acceptance Criteria:**\n- OctagonGraspMission seeds target_label, calls ApproachAndGrasp with knob literals; included; reaches Ticking tree with no parse/registration error\n\n**Verify:** ros2 run mission_planner mission_planner_node --ros-args -p mission:=OctagonGraspMission ; publish one /odometry/filtered ; see 'Ticking tree', no 'Unknown mission'\n\n```json:metadata\n{\"files\":[\"src/subjugator/mission_planner/subjugator_missions/xml/octagon_grasp_mission.xml\",\"src/subjugator/mission_planner/subjugator_missions/xml/sub9_missions.xml\"],\"verifyCommand\":\"ros2 run mission_planner mission_planner_node --ros-args -p mission:=OctagonGraspMission (publish one /odometry/filtered)\",\"acceptanceCriteria\":[\"OctagonGraspMission seeds target_label and calls ApproachAndGrasp with knob literals\",\"included in sub9_missions.xml\",\"reaches Ticking tree with no parse/registration error\"]}\n```" + }, + { + "id": 9, + "subject": "Task 3: Make Task 5 props dynamic (world)", + "status": "completed", + "description": + "**Goal:** Pink_Bin, Yellow_Bin, Yellow_Cup, Pink_Spoon become non-static with inertia so the gripper can lift them; table/octagon stay static.\n\n**Files:**\n- Modify: src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world\n\n**Acceptance Criteria:**\n- 4 props static=false + inertial (mass 0.3, ixx/iyy/izz 0.0005); table/octagon stay static; subjugator_gazebo builds\n\n**Verify:** source scripts/setup.bash && colcon build --packages-select subjugator_gazebo ; GUI: props rest on table, movable\n\n```json:metadata\n{\"files\":[\"src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2025.world\"],\"verifyCommand\":\"source scripts/setup.bash && colcon build --packages-select subjugator_gazebo\",\"acceptanceCriteria\":[\"4 props static=false + inertial\",\"table/octagon stay static\",\"subjugator_gazebo builds\"]}\n```" + }, + { + "id": 8, + "subject": "Task 4: GripperControl advertises Servo \"gripper\" service", + "status": "completed", + "description": + "**Goal:** The sim gripper responds to the same /gripper Servo service ActuateServo calls, mapping angle -> joint open/closed.\n\n**Files:**\n- Modify: src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh\n- Modify: src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc\n- Modify: src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt\n\n**Acceptance Criteria:**\n- advertises subjugator_msgs/srv/Servo 'gripper'; angle 0->closed, 85->open (OPEN_ANGLE=85, clamp); callback sets targets directly; subjugator_msgs added to CMake dep; builds\n\n**Verify:** colcon build --packages-select subjugator_gazebo ; GUI: ros2 service call /gripper subjugator_msgs/srv/Servo '{angle: 85}' opens, '{angle: 0}' closes\n\n```json:metadata\n{\"files\":[\"src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh\",\"src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc\",\"src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt\"],\"verifyCommand\":\"source scripts/setup.bash && colcon build --packages-select subjugator_gazebo\",\"acceptanceCriteria\":[\"advertises /gripper Servo service\",\"angle 0->closed, 85->open with interpolation\",\"subjugator_msgs added to CMake deps\",\"builds\"]}\n```" + }, + { + "id": 10, + "subject": "Task 5: Proximity attach/detach in GripperControl", + "status": "completed", + "blockedBy": [ + 8, + 9 + ], + "description": + "**Goal:** On close, hold the nearest allowlisted prop within attach_radius of the gripper link so it lifts with the sub; on open, release it.\n\n**Files:**\n- Modify: src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh\n- Modify: src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc\n- Modify: src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro\n\n**Acceptance Criteria:**\n- SDF allowlist + attach_radius + gripper_link_name parsed; nearest allowlisted model within radius held on close, released on open (edge-triggered); held prop lifts with sub in GUI (kinematic-follow via SetWorldPoseCmd); table/octagon never held; builds\n\n**Verify:** GUI: gripper over Pink_Bin, '{angle: 0}' -> bin follows as sub rises; '{angle: 85}' -> drops\n\n```json:metadata\n{\"files\":[\"src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh\",\"src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc\",\"src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro\"],\"verifyCommand\":\"source scripts/setup.bash && colcon build --packages-select subjugator_gazebo subjugator_description\",\"acceptanceCriteria\":[\"SDF allowlist + attach_radius parsed\",\"nearest allowlisted prop held on close, released on open\",\"held prop lifts with sub in GUI\",\"table/octagon never held\",\"builds\"]}\n```" + } + ], + "lastUpdated": "2026-06-25" +} diff --git a/docs/superpowers/specs/2026-06-25-octagon-approach-grasp-design.md b/docs/superpowers/specs/2026-06-25-octagon-approach-grasp-design.md new file mode 100644 index 000000000..f5eb7705b --- /dev/null +++ b/docs/superpowers/specs/2026-06-25-octagon-approach-grasp-design.md @@ -0,0 +1,284 @@ +# S4 — Approach & Grasp (RoboSub Task 5, Restore/Octagon) + +**Date:** 2026-06-25 +**Branch:** `gripper-task-5` +**Status:** Design approved; ready for implementation plan. + +## 1. Context & goal + +Task 5 (Restore/Octagon) has SubjuGator surface inside an octagon, retrieve +objects from a table, place them in baskets, and orient toward images. The +mission is decomposed into stages S0–S7: + +- S1 pinger approach → **S2** surface & center over the table → **S3** + detect/select object → **S4 approach & grasp** → S5 transport & place → + S6 second-object loop → S7 face image + yaw. + +S2 and S3 are already built on this branch. **S4 is this design.** Its job: +consume the `target_label` chosen by S3, approach and grasp that object, lift it +clear of the table, and hand a held object to S5 — and to be **functionally +runnable in the GUI sim end-to-end** (not just structurally complete). + +The mission planner is BehaviorTree.CPP v4: C++ leaf nodes in +`mission_planner/subjugator_operations/{include,src}`, reusable subtrees and +missions as XML in `mission_planner/subjugator_missions/xml/`, registered via +`` in `sub9_missions.xml` and `registerNodeType` in +`mission_planner/src/mission_planner_node.cpp`. + +## 2. Scope + +**In scope (S4):** +- Center over the selected object (reuse `HoneOverTarget` with + `label={target_label}`). +- Dead-reckon descent to grasp depth. +- Gripper actuation (close to grasp, open to release) via the existing + `ActuateServo` leaf. +- Lift clear of the table (terminal action of S4). +- Record `grabbed_label` for the S6 two-object loop. +- Make the grasp *functional in sim*: a `Servo` command path into the sim + gripper, an attach mechanism so a closed gripper actually holds an object, + and dynamic (pick-uppable) objects. + +**Out of scope (deferred):** +- S3 selection logic (already built; S4 only consumes `target_label`). +- S5 transport & placement, and the release-into-basket (S5 opens the gripper). +- A Task 5 down-cam YOLO model (does not exist on any branch; see §8). +- Sim command paths for the dropper/torpedo `Servo` services (Task 3/4 work). + +## 3. Key decisions + +| # | Decision | Rationale | +|---|---|---| +| Scope | **Full functional grasp in sim** (not BT-layer-only). | User chose the most complete option; makes the command seam and attach physics real design problems. | +| Command seam | **Preserve the real chain**: BT → `Servo "gripper"` service → provider. Sim provider = `GripperControl` plugin; real provider = `driver.py`. | Byte-identical command path real vs sim; only the service provider differs. The C++ stack calls the `Servo` service **directly** — the `mechanism` action + `mechanisms_server.py` is the legacy Python path and is **not** used. | +| Actuation leaf | **Reuse `ActuateServo`** (generic over `target` = dropper/gripper/torpedo + `angle`). No new actuation leaf. | The leaf already exists (Ethan mechanisms, #500) on `task5-work`; cherry-picked onto this branch. Serves Tasks 3/4 too. | +| Grasp physics | **Proximity fixed-joint attach** (primary), **kinematic-follow** (fallback). | Physically real grasp; runtime joint creation is the one fiddly piece, so keep a robust escape hatch with the same external interface. | +| Descent | **Dead-reckon to grasp depth** via `RelativeMove`. | Down-cam loses the object when close; objects sit on the table at a known z; the attach radius absorbs small error. Matches existing `RelativeMove` placeholder pattern. | +| Confirm + handoff | **Close → lift to clear → open-loop success**, set `grabbed_label = target_label`. | The `Servo` call has no grasp feedback (matches real hardware). The lift is part of S4; S5 owns transport/place. | +| Failure stance | **Fail-fast with bounded retry**: require `target_label` (FAIL if empty), retry the cycle up to N times, return FAILURE if still unable. Object-centering still degrades gracefully internally. | Grabbing without a decision, or transporting nothing, is worse than stopping (consistent with S3). | +| Graspable set | **All four props dynamic**; plugin attaches the single nearest allowlisted model within the attach radius on close. | General, supports the S6 two-object loop, label-agnostic (keeps the command seam action/service-only; the plugin needs no mission state). | + +## 4. Architecture + +Two layers joined only by the `/gripper` `Servo` service: + +``` +BT layer (mission_planner, C++/XML) Sim layer (subjugator_gazebo + world) +───────────────────────────────── ──────────────────────────────────── +ApproachAndGrasp (subtree XML) + Precondition: target_label != '' + RetryUntilSuccessful(grasp_attempts): + Sequence: + HoneOverTarget(label={target_label}) ── down-cam YOLO (existing, gated on model) + ActuateServo(gripper, open) ──────────┐ + RelativeMove(z=descend) │ + ActuateServo(gripper, close) ─────────┤ /gripper (Servo srv) + Delay(settle) │ ↓ + RelativeMove(z=lift) └─> GripperControl plugin (sim): + SetBlackboard grabbed_label := target_label - advertises Servo "gripper" + - angle → joint open/closed + - attach nearest allowlisted prop + within attach_radius on close; + detach on open + World: 4 props now dynamic +``` + +`target_label` (blackboard, from S3) feeds `HoneOverTarget`'s `label` and, on +success, is copied to `grabbed_label` (blackboard), which S3's `exclude` reads +to skip the already-grabbed object on the S6 loop. The attach is +**label-agnostic** (nearest allowlisted prop), so the plugin never needs mission +state. + +## 5. Components + +### 5.1 `ActuateServo` leaf — DONE (cherry-picked) + +`BT::StatefulActionNode`, ports `target` (dropper|gripper|torpedo), `angle` +(int → uint16), `ctx`. Calls the `Servo` service for the chosen target via +clients held in `Context`; RUNNING until the response arrives, then SUCCESS; +FAILURE on missing ctx/target, unknown target, or service-not-ready. + +Ported byte-identical from `task5-work` (so the branches reconcile cleanly when +they converge), plus minimal infra: three `Servo` clients in `Context`, their +creation + `registerNodeType` in `mission_planner_node.cpp`, the +`subjugator_msgs` dependency in `CMakeLists.txt`/`package.xml`, and +`Servo.srv` bumped `uint8`→`uint16` to match upstream. Builds clean +(`colcon build --packages-select subjugator_msgs mission_planner`). + +### 5.2 `ApproachAndGrasp` subtree — NEW (XML, no C++) + +`subjugator_missions/xml/approach_and_grasp.xml`. Composes `HoneOverTarget`, +`RelativeMove`, and `ActuateServo` with BTCPP-v4 built-ins (`Precondition`, +`RetryUntilSuccessful`, `SetBlackboard`, `Delay`). Cycle ordering: +center → open → descend → close → settle → lift; then record `grabbed_label`. + +```xml + + + + + + + + + + + + + + + + + + + +``` + +Tunable knobs (placeholders, measured in GUI sim like `octagon_table_mission`'s +distances): + +| Knob | Placeholder | Meaning | +|---|---|---| +| `grasp_attempts` | `2` | bounded retries of the cycle | +| `center_tol_norm` / `center_timeout_msec` | `0.05` / `20000` | object-centering tolerance / timeout | +| `descend_z` | `-0.6` **TODO** | descent from hover to grasp range | +| `lift_z` | `0.6` **TODO** | lift to clear the table | +| `open_angle` / `close_angle` | `85` / `0` | gripper open / closed (mapping in §5.4) | +| `grip_settle_msec` | `1500` | let the close + attach take effect | +| `move_timeout_msec` | `30000` | per-`RelativeMove` timeout | + +**Notes:** +- Fail-fast is **open-loop**: the cycle FAILs (→ retry → eventual FAILURE) on a + move/actuation failure (`RelativeMove` timeout, gripper service not ready) or + empty `target_label` — not on "object not actually held," since the grab is + not verified (matches the open-loop confirmation decision). +- `Precondition` uses BTCPP scripting (built into v4) and assumes `target_label` + *exists* on the blackboard (seeded by S3 or the test mission). +- Retry compounds depth: attempt 2 re-centers and descends again from wherever + attempt 1 ended. Acceptable for move/service failures; documented limitation, + no bookkeeping added now. + +### 5.3 `OctagonGraspMission` harness — NEW (XML) + +`subjugator_missions/xml/octagon_grasp_mission.xml`. Standalone S4 tuning +harness (as `CenterCameraTest` is for S2): no upstream S3, so it seeds +`target_label` and runs `ApproachAndGrasp`. + +```xml + + + + + + + + +``` + +Both new XML files registered via `` in `sub9_missions.xml`. The +production full-Task-5 mission (S2→S3→S4 chained) is a separate, later file; +`ApproachAndGrasp` is built to drop into it. + +### 5.4 `GripperControl` plugin — EXTENDED (`subjugator_gazebo`) + +Currently keyboard-only (`/keyboard/keypress` `'u'` toggle), position-only, no +attach. Three additions: + +1. **Advertise the `Servo "gripper"` service** on the plugin's existing + `rclcpp::Node` (resolves to `/gripper`, matching `ActuateServo`'s client and + real `driver.py`). The callback runs inside `spin_some` in `PostUpdate`, i.e. + on the gz thread — same thread as `PreUpdate`, so it sets member targets + directly with no cross-thread races. Mapping: + `target = closed_pos_ + clamp(angle/OPEN_ANGLE, 0, 1) * (open_pos_ − closed_pos_)`, + `OPEN_ANGLE = 85` ⇒ `angle=0` fully closed (grasp), `angle=85` fully open. + Keyboard `'u'` stays as a manual fallback. + +2. **Proximity attach-on-close / detach-on-open.** On a close command (target + crosses below a grip threshold), find the nearest **allowlisted** model + within `attach_radius` of `gripper_link`'s world pose and create a fixed + joint (driving the physics `AttachFixedJoint` primitive via the ECM — the + same primitive gz's `DetachableJoint` system uses); break it on open. The + allowlist is SDF-configured (`Pink_Bin` ×4) so the + plugin can never attach the sub, table, or octagon. + **Fallback (same external interface):** if runtime joint creation is + unreliable in gz Harmonic, swap the attach/detach internals for + *kinematic-follow* (write the held model's pose to track `gripper_link` each + `PreUpdate`). Implement fixed-joint first; keep kinematic-follow as the + escape hatch. Only the ~30 lines that "hold the object" differ; the service, + BT, subtree, and props are identical either way. + + SDF knobs: `attach_radius` (≈`0.25 m` **TODO**), grip threshold. + +### 5.5 World — props go dynamic (`robosub_2025.world`) + +`Pink_Bin`, `Yellow_Bin`, `Yellow_Cup`, `Pink_Spoon`: `true` → +`false`, add `` (mass ≈`0.3 kg` **TODO** + simple box inertia, tuned so +they rest on the table and lift cleanly). `Task5_Table` and `Octagon_2025` stay +static (and are excluded from the attach allowlist). + +## 6. Data flow (end to end) + +``` +ApproachAndGrasp → ActuateServo(target=gripper, angle=0) + → /gripper (Servo srv) → GripperControl callback + → joints close + attach nearest allowlisted prop within radius +RelativeMove(lift_z) → sub rises → fixed joint carries the prop up +SetBlackboard grabbed_label := target_label +[S5 later] ActuateServo(angle=85) → joints open + detach → prop released +``` + +## 7. Error handling + +- **No `target_label`:** `Precondition` returns FAILURE immediately (no blind + grab). +- **Centering can't see the object:** `HoneOverTarget` degrades to dead-reckon + (graceful, internal) — the cycle continues to descend/grasp. +- **Move/actuation failure** (`RelativeMove` timeout, gripper service not + ready): the cycle FAILs → `RetryUntilSuccessful` re-attempts up to + `grasp_attempts` → FAILURE if still unable. S4 returns FAILURE (not masked). +- **Grab not actually achieved:** not detected (open-loop). Accepted tradeoff; + a vision/again-detect confirmation is a future improvement. + +## 8. Dependencies, assumptions, risks + +- **No Task 5 down-cam YOLO model exists** on any branch (committed models are + front-cam Task 1/2 only). The grasp *plumbing* (§5.4, §5.5 + `ActuateServo`) + is buildable and testable now via manual driving; the **autonomous** + `HoneOverTarget → descend → grasp` loop is gated on that model, exactly like + S2/S3. +- **Branch divergence:** `gripper-task-5` (S2/S3) and `task5-work` (mechanisms) + have diverged; S4 needs pieces from both. Resolved by a **surgical + cherry-pick** of `ActuateServo` onto this branch (done), not a full merge — + `task5-work` carries no S5/S6/S7 logic and no Task 5 vision, so a full merge + would add unrelated Task 2/4/start-gate/CI churn for one leaf. +- **Runtime fixed-joint creation** is the highest-risk implementation step; + mitigated by the kinematic-follow fallback (§5.4). +- **Cameras render only in a GUI Gazebo session**, not agent-headless — the + autonomous functional run requires a GUI session. + +## 9. Verification + +- **Build:** `colcon build --packages-select subjugator_msgs mission_planner` + and the `subjugator_gazebo` plugin. +- **Instantiate:** `ros2 run mission_planner mission_planner_node --ros-args -p + mission:=OctagonGraspMission`, publish one `/odometry/filtered`, confirm it + reaches "Ticking tree" with no parse/registration error (now exercising + `ActuateServo`). +- **Plumbing (no YOLO), GUI sim:** drive the sub over a prop manually (teleop or + scripted `RelativeMove`), call `/gripper angle:=0` → prop attaches and lifts + with the sub; `angle:=85` → prop releases. +- **Autonomous (gated on a Task 5 model):** the full center→descend→grasp loop. + +## 10. Conventions + +- Leaf nodes named by mechanism (`ActuateServo`), subtrees by intent + (`ApproachAndGrasp`); mission/subtree XML named by behavior, no `S4` shorthand + in shared files. +- Commits carry **no** AI self-attribution. diff --git a/docs/superpowers/specs/2026-06-25-pool-test-protocol-design.md b/docs/superpowers/specs/2026-06-25-pool-test-protocol-design.md new file mode 100644 index 000000000..c08420e00 --- /dev/null +++ b/docs/superpowers/specs/2026-06-25-pool-test-protocol-design.md @@ -0,0 +1,339 @@ +# Pool Test Protocol (S2 + S3) — Design + +**Date:** 2026-06-25 +**Branch:** gripper-task-5 +**Status:** Approved design, ready for implementation plan + +## Goal + +A single guided shell script the team runs poolside to test the Task 5 down-cam +stack modularly — smallest whole unit first, each stage a superset of the last — +while capturing the pool-specific information we can only get in the water +(sign/axis calibration, gains, detection confidence/range, settle behavior, and +optionally the dead-reckon constants). The required milestone is an end-to-end +**S2 + S3** run: center over the table, then select the role target. + +## Scope + +- **Tier A (required):** preflight → calibrate → hone → select → combined. + The combined run (center → lock target in one mission process) is the S2 + S3 + milestone. +- **Tier B (optional):** measure the four dead-reckon constants, then run full S2 + (`OctagonTableMission`) with them. Gated on having the task objects/models; + not required for the S2 + S3 milestone. + +## Architecture + +One guided shell script, `pooltest.sh`, run on the sub's onboard machine. It +walks an ordered ladder of stages. For each stage it: + +1. Prints the **precondition** (how the sub must be placed) and waits for the + operator to type `GO` (so a diver/teleop can position the sub first). +2. **Counts down a configurable autonomous-start delay (default 30 s)** before + launching, so the operator can disconnect and clear the sub. The sub runs the + mission untethered — the operator is not connected during autonomous motion. +3. Launches the stage's **mission** (real-robot topic defaults — no sim + overrides). +4. Records a **bag** of the relevant topics and **tees** the mission console to + a log. +5. Prints a **results form** the operator fills in (the eyeball observations a + machine can't easily judge from a bag). + +The script runs on the sub's onboard machine and **must survive operator +disconnect** (launched detached via `tmux`/`nohup`/`setsid`), so the mission, the +bag, and the teed log keep running after the operator closes their connection +during the autonomous-start delay. The autonomous-start delay applies to stages +that command motion (1, 2, 4, 6). Stage 3 (select, no motion) tolerates it +harmlessly; Stage 5 (dead-reckon) is the exception — it is manual teleop, so the +operator stays connected and the delay is skipped. + +The script supports running a single stage (`./pooltest.sh calib`) or the guided +sequence (`./pooltest.sh`). + +### Capture mechanisms + +- **Bag** (`ros2 bag record`): flight recorder of the data topics + (`/odometry/filtered`, `/goal_pose`, `/yolo_down/detections`) so convergence, + timing, and detection confidence can be measured offline. Raw images excluded + by default (large). +- **tee**: splits the mission's stdout to screen *and* `console.log`, preserving + the node's own narration (e.g. `SelectTarget: locked '' (N frames)`). +- **Results form**: a short fill-in template printed at the end of each stage for + human observations (converged? oscillated? signs flipped?), appended to + `results.txt`. + +## Stage ladder + +### Tier A — required (S2 + S3 milestone) + +| # | Stage | Mission | New? | Proves / extracts | +|---|-------|---------|------|-------------------| +| 0 | Preflight | *(checks only, no motion)* | — | down image + `/yolo_down/detections` flowing; odom healthy; real topic names + image WxH; detection rate; classes the down model produces | +| 1 | Calibrate | `CenterCameraTest` | exists | converging `swap_axes`/`map_x_sign`/`map_y_sign`; tuned `kp`/`max_step`; achievable `tol_norm`; settle behavior. Smallest whole unit. | +| 2 | Hone | `HoneOverTableOnly` | new | degrade-wrapped subtree holds center over `table`; table detection confidence + altitude/range | +| 3 | Select | `SelectOnly` | new | role → `{target_label}` lock on real detections; good `min_conf`/`consecutive_frames`; per-class confidence; false picks | +| 4 | **Combined** | `HoneOverTableSelect` | new (fused) | center → lock in one process, exercising the in-tree `{target_label}` handoff. **S2 + S3 milestone.** | + +### Tier B — optional (full S2 with real constants) + +| # | Stage | Mission | New? | Extracts | +|---|-------|---------|------|----------| +| 5 | Measure dead-reckon | *(guided odom-snapshot procedure)* | — | `surface_dz`, `hover_dz`, `octagon_dx`, `octagon_dy` | +| 6 | Full S2 | `OctagonTableMission` | exists | full surface → descend → dead-reckon → hone with measured constants | + +### Boundaries + +- Stages 1–3 are isolated; the sub is repositioned between them. Stage 4 chains + internally with no reposition. +- Stage 4 is the deliverable milestone. Tier B is explicitly optional. +- **Dependency:** Stages 3–4 need trained YOLO models for the role's object + classes (`survey_repair` → nut_bolt/plug; `search_rescue` → pill/bandage) and + the `role` param set; Stage 2 needs a `table` model. A missing model degrades + that stage to "topics connect, no detection" — the script detects and reports + this rather than silently passing. + +## Per-stage capture spec + +Common: launch uses real-robot topic defaults (no `down_image_topic` / +`down_detect_topic` overrides — those are sim-only). Each stage records a bag of +`/odometry/filtered`, `/goal_pose`, `/yolo_down/detections` and tees the node +console to `console.log`. Output folder: `pooltest_runs/_/`. + +- **Stage 0 — Preflight** (no motion, gates only). Checks `/yolo_down/detections` + publishing + rate, down image topic publishing + actual WxH, `/odometry/filtered` + healthy, `yolo_down` node alive; prints classes the down model currently + produces. **Pass:** all topics live, detection rate ≥ ~5 Hz, expected classes + present. Fails loud if any missing. + +- **Stage 1 — Calibrate** · `mission:=CenterCameraTest`. Precondition: sub + hovering ~1 m over table, roughly level. Signs/gains are literals in the XML; + operator edits them between attempts. Form: converged toward target (y/n)? · + signs used · `kp`/`max_step` used · final tightness (tight/oscillated/drifted) · + approx settle time · notes. **Pass:** error shrinks within `tol_norm` and holds. + Headline output: the converging sign triple + gains. + +- **Stage 2 — Hone** · `mission:=HoneOverTableOnly`. Precondition: same as Stage 1. + Form: held center (y/n)? · table detection confidence range · approx altitude · + detection stable/flickery · did it fall back to AlwaysSuccess (lost table)? + **Pass:** stays centered using Stage-1 signs via the real subtree wrapper. + +- **Stage 3 — Select** · `mission:=SelectOnly` · `-p role:=`. + Precondition: sub positioned so the task objects are in the down-cam frame; role + chosen (script prompts/arg). Form: locked label · correct? · lock time/frames · + per-class confidence · false picks · `min_conf`/`consecutive_frames` used. + **Pass:** locks the correct role-class. Output: tuned `min_conf`/`consecutive_frames`. + +- **Stage 4 — Combined** · `mission:=HoneOverTableSelect` · `-p role:=…`. + Precondition: sub hovering over table with objects present. Form: centered then + locked (y/n)? · locked label · total time · handoff worked (`{target_label}` + populated)? **Pass:** one process centers over the table then locks the target. + +- **Stage 5 — Measure dead-reckon** (Tier B). See procedure below. Writes + `constants.txt`. + +- **Stage 6 — Full S2** · `mission:=OctagonTableMission` with measured constants + edited in. + +## Operator guidance (self-serve) + +The script must be runnable by a poolside tester who did **not** design the test +and may not know ROS. All guidance is printed by the script itself; no external +notes required. Plain language, no bare `ros2` commands shown to the operator. + +### At launch (`./pooltest.sh` with no args) + +Before anything else, print a briefing: + +- One sentence on what this is. +- The **ladder** (each stage one line + rough time). +- A **"What you need" checklist**: sub powered and in the water; the down-cam + YOLO node running with the required model(s) loaded (names listed); the + **role** for this run; a diver/teleop to position the sub; clear pool space. +- The **role values** spelled out: `survey_repair` (objects: nut_bolt, plug) or + `search_rescue` (objects: pill, bandage). +- Where outputs are written (`pooltest_runs/...`). +- How to run: single stage vs. guided sequence. +- The post-`GO` rule: "After you type GO you have 30 s to disconnect and clear + the sub; it then runs on its own." + +### Before each stage (printed above the `GO` gate) + +A numbered **DO THIS** block, every stage: + +1. **MANUAL — position the sub:** exact placement, e.g. "Diver: hold the sub + level, ~1 m above the table, with the table roughly centered beneath it." +2. **TYPE:** the literal inputs expected this stage (e.g. the role, then `GO`), + each with its valid values. +3. **WHAT WILL HAPPEN:** e.g. "After the 30 s delay the sub moves on its own to + center over the table (~20 s)." +4. **SUCCESS LOOKS LIKE:** plain-language description (e.g. "the sub settles and + holds steady over the table"). + +### What the operator types vs. does by hand + +The plan must implement prompts/printouts covering exactly these: + +- **Types:** stage selection or guided continue (`y`/`n`); `GO` at each gate; + `role` for Stages 3–4 (`survey_repair`/`search_rescue`); each results-form + value; `SNAP` at each Stage-5 checkpoint. +- **Does by hand:** positions the sub before every motion stage; disconnects and + clears the sub during the post-`GO` delay; for **Stage 1**, edits the sign/gain + literals in `center_camera_test_mission.xml` between attempts and rebuilds — + the script prints the exact file, which values to change (flip `map_x_sign` / + `map_y_sign` or set `swap_axes="true"` if the sub drove *away* from the + target), and the rebuild command; for **Stage 5**, teleops the sub to each + named checkpoint. + +### Results form fields + +Every field is labeled with its format (y/n, number + unit, or free text) and a +worked example, so it is answerable by someone who didn't design the test. + +### Fail-loud preconditions + +If a precondition is not met (e.g. no down-cam detections arriving), the script +stops with a plain-language message and the likely cause ("Is the yolo_down node +running with a model loaded?") instead of launching a motion stage blind. + +### End-of-run summary and README + +- After each stage (and at the end of a guided run) the script prints a + **summary of the key captured values** (signs, gains, `min_conf`/frames, + constants) so the tester can report them back. +- `pool_tests/README.md` includes a **one-page printable checklist** (per-stage: + position → type → expect → record) and a short **glossary** (hone, role, + `tol_norm`) and the required model name(s) per stage. + +## New mission XML + +Three thin wrappers around already-registered nodes/subtrees — **no new C++**. +Each gets an `` in `sub9_missions.xml`. + +`hone_over_table_only_mission.xml` (Stage 2): + +```xml + + + + + +``` + +`select_only_mission.xml` (Stage 3 — wrapper binds the subtree's ports so it runs +as a root): + +```xml + + + + + +``` + +`hone_over_table_select_mission.xml` (Stage 4 — fused milestone; remaps +`{target_label}` to root scope so the handoff is real and observable): + +```xml + + + + + + + + +``` + +The locked class is already logged by `SelectTarget`, so it lands in +`console.log` — no extra logging node needed. + +## Dead-reckon measurement procedure (Stage 5, Tier B) + +The four constants are *relative* moves from wherever S1 leaves the sub, so the +procedure pins a start pose and snapshots odom at labeled checkpoints; the script +computes the deltas and writes `constants.txt`. The operator does the moving +(teleop/diver); the script snapshots one `/odometry/filtered` on command. + +1. Place sub at the designated **S1-arrival start pose** (a pool marker). Snapshot + → `A = (xA, yA, zA)`. +2. Surface the sub. Snapshot → `zS`. ⇒ `surface_dz = zS − zA`. +3. Descend to down-cam hover depth (table fills frame). Snapshot → `zH`. ⇒ + `hover_dz = zH − zS`. +4. Translate until centered over the table. Snapshot → `(xC, yC)`. ⇒ + `octagon_dx = xC − xA`, `octagon_dy = yC − yA`. + +**Synergy:** after a Stage 4 run the sub is already centered over the table — its +end pose *is* checkpoint 4, so Stage 5 can offer to reuse it. + +**Validity flag (written into `constants.txt`):** these values are valid only for +that start pose; re-measure at competition against the real S1 arrival. + +## File layout + +``` +src/subjugator/mission_planner/subjugator_missions/xml/ + hone_over_table_only_mission.xml (new — Stage 2) + select_only_mission.xml (new — Stage 3) + hone_over_table_select_mission.xml (new — Stage 4 fused) + + sub9_missions.xml (3 new lines) + +src/subjugator/subjugator_bringup/pool_tests/ + pooltest.sh (the guided runner) + README.md (ladder + how to run + how to read outputs) +``` + +Output per run, in `./pooltest_runs/_/` (gitignored, path +overridable via env var): + +``` +_/ + bag/ ros2 bag of odom, goal_pose, yolo_down detections + console.log teed mission stdout + results.txt the filled-in results form + constants.txt (Stage 5 only) the four measured dead-reckon values +``` + +## Script internals (bash) + +- Stage registry: each stage = precondition text + launch command + bag topic + list + form fields. +- Arg parsing: `./pooltest.sh [stage|all]`. +- `GO` gate before each stage, then a configurable autonomous-start countdown + (default 30 s, `--delay` / env override; skipped for Stage 5 teleop) before the + mission launches, giving the operator time to disconnect and clear the sub. +- Detached execution (`tmux`/`nohup`/`setsid`) so the mission, bag, and teed log + survive operator disconnect during the autonomous-start delay. +- Interactive `role` prompt for Stages 3–4 (or `--role` arg). +- Bag start/stop wrapping each run; `tee` for console. +- `read`-driven form appended to `results.txt`. +- Odom-snapshot helper (`ros2 topic echo --once /odometry/filtered`) for Stage 5. +- Preflight via `ros2 topic list` / `ros2 topic hz`. +- Launch briefing + per-stage **DO THIS** blocks + end-of-run value summary, per + the Operator guidance section (self-serve for testers running without the + author). + +### `--sim` rehearsal flag + +Same ladder, but adds the sim overrides (`down_image_topic:=/down_cam/image_raw`) +and reminds the operator to launch the `yolo_down` node. Lets the whole harness — +staging, GO gates, bagging, forms, and that all four missions instantiate — be +rehearsed in a GUI Gazebo session before pool day. `--sim` only rehearses the +harness; real runs use the real-robot topic defaults. + +## Non-goals + +- Does **not** manually position the sub (initial placement or repositioning + between stages) — a diver/teleop does that. The script *does* launch missions + that command autonomous motion (centering, dead-reckon), which is the behavior + under test. Stage 5 is the exception: there all motion is manual teleop and the + script only snapshots odom. +- Does **not** train or load YOLO models — assumes they are present; reports + clearly if a class never appears. +- Does **not** auto-tune gains/signs — operator edits `CenterCameraTest` literals + between calibration attempts (auto-parse deferred). +- Does **not** cover S1 pinger approach or S4 grasp. +- Not the sim test path itself — `--sim` only rehearses the harness. +- Does **not** bag raw images by default (large; Stage 0 may grab a few throttled + frames if a visual record is wanted). diff --git a/run_task b/run_task new file mode 100755 index 000000000..19e9f5de9 --- /dev/null +++ b/run_task @@ -0,0 +1,39 @@ +#!/usr/bin/env bash +# Thin wrapper so teammates can run `./run_task 5` from the repo root without +# remembering the ros2 run incantation. Identical behaviour either way. +# +# Every failure this script detects is a rig failure, so it exits 2. Exit 1 is +# reserved for "the mission ran and failed" and must only ever come from +# run_task itself -- hence the checks below, which would otherwise surface as a +# bare exit 1 out of ros2 run. +set -euo pipefail + +here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +if [ ! -f "$here/install/setup.bash" ]; then + echo "run_task: $here/install/setup.bash is missing -- build the workspace first (cb)." >&2 + exit 2 +fi + +# colcon's prefix chain is not written for `set -eu`: an unset variable or a +# non-zero return inside it would otherwise kill this script with no message. +set +eu +# shellcheck disable=SC1091 +source "$here/install/setup.bash" +set -eu + +if ! command -v ros2 >/dev/null 2>&1; then + echo "run_task: ros2 is not on PATH after sourcing the workspace." >&2 + exit 2 +fi + +# Not `ros2 pkg prefix`: that passes for any installed package, and a workspace +# built before this feature existed has the package but not the executable -- +# where `ros2 run` would exit 1 and masquerade as a failed mission. +if ! ros2 pkg executables subjugator_bringup 2>/dev/null | + grep -qx "subjugator_bringup run_task"; then + echo "run_task: subjugator_bringup has no run_task executable -- run cb." >&2 + exit 2 +fi + +exec ros2 run subjugator_bringup run_task "$@" diff --git a/scripts/setup.bash b/scripts/setup.bash index 610d3ddeb..2c4bf1bb8 100644 --- a/scripts/setup.bash +++ b/scripts/setup.bash @@ -252,9 +252,53 @@ function pub_wrench() { local tz=$6 local duration=$7 - echo "Publishing force for ${duration} seconds..." - - ros2 topic pub --once /cmd_wrench geometry_msgs/msg/Wrench "force: + # One-shot `ros2 topic pub` can silently lose its message under rmw_zenoh + # (publish lands before the writer->router->reader route is ready, or the + # process exits while the sample is still in flight). Losing the STOP + # message leaves the thrusters holding force. So both publishes: send the + # sample twice 1s apart (-r 1 --times 2), wait for a matched subscriber + # first (-w 1, bounded so a dead stack fails loud instead of hanging), and + # keep the session alive 2s after the last publish (--keep-alive 2). + # Side effect: actual thrust runs ~3s LONGER than (the extra + # sample + keep-alive happen between force-on and the sleep starting). + echo "Publishing force for ${duration} seconds (thrust runs ~3s longer than requested)..." + + # Subshell so the EXIT trap is scoped to this call, not the caller's shell. + # The trap fires on EVERY exit path — normal completion, Ctrl-C during the + # sleep (previously the remaining thrusters-stuck-on path), TERM/HUP — so + # the STOP wrench is sent exactly once no matter how we leave. It is armed + # BEFORE the force publish: an interrupt mid-publish may already have put + # force samples on the wire. + ( + stop_wrench() { + # Shield the STOP publish from a second Ctrl-C; it is bounded + # by --max-wait-time-secs anyway (worst case ~13s). + trap '' INT TERM HUP + echo "Stopping force..." + if ! ros2 topic pub --times 2 -r 1 -w 1 --max-wait-time-secs 10 --keep-alive 2 \ + /cmd_wrench geometry_msgs/msg/Wrench "force: + x: 0.0 + y: 0.0 + z: 0.0 +torque: + x: 0.0 + y: 0.0 + z: 0.0 +"; then + echo "WARNING: STOP wrench NOT delivered (no /cmd_wrench subscriber within 10s)." + echo " Thrusters may STILL BE APPLYING FORCE — check the stack NOW." + exit 1 + fi + } + trap stop_wrench EXIT + # Fatal signals must EXIT the subshell (running the EXIT trap), not + # kill it trap-less. A ^C in the terminal also kills the foreground + # `sleep`, so the trap runs immediately, not after the full duration. + trap 'exit 130' INT + trap 'exit 143' TERM HUP + + if ! ros2 topic pub --times 2 -r 1 -w 1 --max-wait-time-secs 10 --keep-alive 2 \ + /cmd_wrench geometry_msgs/msg/Wrench "force: x: ${fx} y: ${fy} z: ${fz} @@ -262,21 +306,16 @@ torque: x: ${tx} y: ${ty} z: ${tz} -" - - sleep "${duration}" - - echo "Stopping force..." +"; then + echo "WARNING: force command NOT delivered (no /cmd_wrench subscriber within 10s)." + echo " Nothing is thrusting; check the stack, then retry." + # The EXIT trap still sends the STOP wrench: cheap if truly nothing + # is thrusting, essential if the failure was anything else. + exit 1 + fi - ros2 topic pub --once /cmd_wrench geometry_msgs/msg/Wrench "force: - x: 0.0 - y: 0.0 - z: 0.0 -torque: - x: 0.0 - y: 0.0 - z: 0.0 -" + sleep "${duration}" + ) } function move_rel() { diff --git a/src/subjugator/drivers/down_cam/down_cam/down_cam_node.py b/src/subjugator/drivers/down_cam/down_cam/down_cam_node.py index 5598572fb..c082e5cc3 100644 --- a/src/subjugator/drivers/down_cam/down_cam/down_cam_node.py +++ b/src/subjugator/drivers/down_cam/down_cam/down_cam_node.py @@ -36,7 +36,7 @@ def rotate_front_cam(frame: MatLike) -> MatLike: class DownCamDriver(Node): def __init__(self): - super().__init__("front_cam_driver") + super().__init__("down_cam_driver") # cam_path = "/dev/v4l/by-id/usb-Chicony_Tech._Inc._Dell_Webcam_WB7022_77A8ADD45565-video-index0" # cam_path = "/dev/v4l/by-id/usb-H264_USB_Camera_H264_USB_Camera_2020032801-video-index0" @@ -60,6 +60,7 @@ def get_frame_and_publish(self): ret, frame = self.cap.read() if not ret: self.get_logger().warn("no frame from camera") + continue frame = rotate_front_cam(frame) diff --git a/src/subjugator/drivers/front_cam/front_cam/front_cam_driver.py b/src/subjugator/drivers/front_cam/front_cam/front_cam_driver.py index 5c5adbf0f..08357f9fa 100644 --- a/src/subjugator/drivers/front_cam/front_cam/front_cam_driver.py +++ b/src/subjugator/drivers/front_cam/front_cam/front_cam_driver.py @@ -1,3 +1,5 @@ +import time + import cv2 import rclpy from cv2.typing import MatLike @@ -81,6 +83,11 @@ def publish_forever(self): ret, frame = self.cap.read() if not ret: self.get_logger().warn("no frame from camera") + # This loop has no spin_once/rate cap, so a persistently failing + # read (camera unplugged) would busy-spin a core and flood the + # logs. Back off briefly before retrying. + time.sleep(0.1) + continue # frame = rotate_front_cam(frame) diff --git a/src/subjugator/gnc/subjugator_controller/config/sim_pid_controller.yaml b/src/subjugator/gnc/subjugator_controller/config/sim_pid_controller.yaml index 145f4ff48..de0c6cea1 100644 --- a/src/subjugator/gnc/subjugator_controller/config/sim_pid_controller.yaml +++ b/src/subjugator/gnc/subjugator_controller/config/sim_pid_controller.yaml @@ -3,9 +3,17 @@ ros__parameters: # vector indices correspond to x,y,z position, # then roll,pitch,yaw orientation gains + # Pitch (index 4) was undamped with no integral (kd=ki=imax=0) -- the + # same class of bug as the sway kd=0 runaway. On surge the sub develops + # a pitching moment; undamped it wandered to 15-45 deg, tilting the + # down_cam and corrupting the LockTargetXY ray-cast, which uses + # orientation as its lever arm. Mirror the working roll axis (index 3): + # kd=10 to damp, ki=2.5 + imax/imin=+-10 to null the steady + # surge-induced pitch, so the camera stays nadir-down while the sub + # repositions. kp: [80.0, 30.0, 45.0, 65.0, 35.0, 30.0] - ki: [0.0, 0.0, 1.6, 2.5, 0.0, 1.0] - kd: [135.0, 0.0, 2.0, 10.0, 0.0, 5.0] - imax: [0.0, 0.0, 5.0, 10.0, 0.0, 6.0] - imin: [0.0, 0.0, -5.0, -10.0, 0.0, -6.0] + ki: [0.0, 0.0, 1.6, 2.5, 2.5, 1.0] + kd: [135.0, 50.0, 2.0, 10.0, 10.0, 5.0] + imax: [0.0, 0.0, 5.0, 10.0, 10.0, 6.0] + imin: [0.0, 0.0, -5.0, -10.0, -10.0, -6.0] antiwindup: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/src/subjugator/gnc/subjugator_vision/models/octagon_sim.pt b/src/subjugator/gnc/subjugator_vision/models/octagon_sim.pt new file mode 100644 index 000000000..b29a91861 Binary files /dev/null and b/src/subjugator/gnc/subjugator_vision/models/octagon_sim.pt differ diff --git a/src/subjugator/gnc/subjugator_vision/scripts/save_down_images.py b/src/subjugator/gnc/subjugator_vision/scripts/save_down_images.py index 6f876fc11..99e5c7f33 100755 --- a/src/subjugator/gnc/subjugator_vision/scripts/save_down_images.py +++ b/src/subjugator/gnc/subjugator_vision/scripts/save_down_images.py @@ -1,9 +1,7 @@ #!/usr/bin/env python3 -""" -Take a single image from /down_cam/image_raw, save it, and exit. -""" import os +import threading import cv2 import rclpy @@ -18,38 +16,79 @@ def __init__(self): default_dir = os.path.join(os.path.expanduser("~"), "sim_images/down") self.declare_parameter("save_dir", default_dir) + self.declare_parameter( + "save_every", + 0, + ) # 0 is save on Enter, >0 also auto saves every Nth frame + + self.save_every = self.get_parameter("save_every").value + self.count = 0 - topic = "/down_cam/image_raw" self.dir = os.path.abspath( os.path.expanduser(self.get_parameter("save_dir").value), ) os.makedirs(self.dir, exist_ok=True) self.bridge = CvBridge() + self.latest_msg = None + self.lock = threading.Lock() + self.saved = 0 + topic = "/down_cam/image_raw" self.create_subscription(Image, topic, self.callback, 10) + + mode = ( + f"burst every {self.save_every} frames + Enter" + if self.save_every > 0 + else "Enter only" + ) self.get_logger().info( - f"DownCamSaver: waiting for image on {topic}, saving to {self.dir}", + f"DownCamSaver: subscribed to {topic}, saving to {self.dir}. " + f"Mode: {mode}. Press Enter to save current frame, Ctrl-C to quit.", ) def callback(self, msg: Image): + with self.lock: # Cache most recent frame + self.latest_msg = msg + + # Burst path if enabled + if self.save_every > 0: + self.count += 1 + if self.count % self.save_every == 0: + self.save_msg(msg, "burst") + + def save_latest(self): + # Manual path + with self.lock: + msg = self.latest_msg + if msg is None: + self.get_logger().warn("No frame received yet; nothing to save.") + return + self.save_msg(msg, "manual") + + def save_msg(self, msg: Image, reason: str): + # Shared write logic used by both the burst and manual path img = self.bridge.imgmsg_to_cv2(msg, "bgr8") - - # time for file name stamp = self.get_clock().now().to_msg() filename = f"down_cam_{stamp.sec}_{stamp.nanosec}.png" cv2.imwrite(os.path.join(self.dir, filename), img) - - self.get_logger().info(f"Saved {filename}") - - rclpy.shutdown() + self.saved += 1 + self.get_logger().info(f"Saved {filename} ({reason}, total: {self.saved})") def main(): rclpy.init() node = DownCamSaver() + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + spin_thread.start() + try: - rclpy.spin(node) + while True: + input() + node.save_latest() + except (KeyboardInterrupt, EOFError): + pass finally: node.destroy_node() if rclpy.ok(): diff --git a/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov11.launch.py b/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov11.launch.py index d7e9b0b20..a601a4950 100644 --- a/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov11.launch.py +++ b/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov11.launch.py @@ -24,7 +24,11 @@ def generate_launch_description(): - + # NOTE: This wrapper is intentionally near-duplicated with yolov26.launch.py. + # yolo_bringup is an ament_cmake package that only installs launch/ into + # share/, so there is no importable Python module to hold a shared helper. + # Adding a fragile import from the installed share/ path would be worse than + # the small duplication, so the two files are kept in sync by hand. return LaunchDescription( [ IncludeLaunchDescription( diff --git a/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov26.launch.py b/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov26.launch.py index e7a03a96b..01c15718e 100644 --- a/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov26.launch.py +++ b/src/subjugator/gnc/yolo_ros/yolo_bringup/launch/yolov26.launch.py @@ -25,6 +25,12 @@ def generate_launch_description(): + # NOTE: This wrapper is intentionally near-duplicated with yolov11.launch.py. + # yolo_bringup is an ament_cmake package that only installs launch/ into + # share/, so there is no importable Python module to hold a shared helper. + # Adding a fragile import from the installed share/ path would be worse than + # the small duplication, so the two files are kept in sync by hand. + # Absolute path to the installed weights. Relative paths break because the # yolo_node resolves them against its runtime cwd, not this launch file. # Requires `colcon build --packages-select subjugator_vision` so the .pt is diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 6426174e0..72ae57b2c 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/src/subjugator/mission_planner/CMakeLists.txt @@ -17,8 +17,10 @@ find_package(sensor_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(subjugator_msgs REQUIRED) find_package(yolo_msgs REQUIRED) +find_package(subjugator_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(yaml-cpp REQUIRED) find_package(subjugator_msgs REQUIRED) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include @@ -31,19 +33,31 @@ add_library( subjugator_operations/src/at_goal_pose.cpp subjugator_operations/src/detect_target.cpp subjugator_operations/src/hone_bearing.cpp + subjugator_operations/src/center_camera.cpp + subjugator_operations/src/lock_target_xy.cpp subjugator_operations/src/check_yolo_model.cpp subjugator_operations/src/track_largest_poles.cpp subjugator_operations/src/poles_big_enough.cpp subjugator_operations/src/determine_channel_side.cpp + subjugator_operations/src/select_target.cpp + subjugator_operations/src/select_basket.cpp + subjugator_operations/src/select_face_symbol.cpp subjugator_operations/src/any_poles_detected.cpp subjugator_operations/src/has_found_pair.cpp + subjugator_operations/src/actuate_servo.cpp + subjugator_operations/src/record_target_scale.cpp + subjugator_operations/src/confirm_grasp_by_scale.cpp subjugator_operations/src/track_best_pair.cpp subjugator_operations/src/hone_midpoint.cpp subjugator_operations/src/log_to_file.cpp subjugator_operations/src/nav_channel_control.cpp subjugator_operations/src/align_depth.cpp subjugator_operations/src/align_yaw.cpp - subjugator_operations/src/actuate_servo.cpp) + subjugator_operations/src/go_to_pinger.cpp + subjugator_operations/src/descend_until_detected.cpp + subjugator_operations/src/search_for_target.cpp + subjugator_operations/src/ros_timeout.cpp + subjugator_operations/src/ros_delay.cpp) ament_target_dependencies( operations @@ -64,7 +78,7 @@ ament_target_dependencies( # Node add_executable(mission_planner_node src/mission_planner_node.cpp) -target_link_libraries(mission_planner_node operations) +target_link_libraries(mission_planner_node operations yaml-cpp) ament_target_dependencies( mission_planner_node rclcpp @@ -86,6 +100,52 @@ install( install(DIRECTORY include/ DESTINATION include) install(DIRECTORY subjugator_operations/include/ DESTINATION include) install(DIRECTORY subjugator_missions/xml/ DESTINATION share/${PROJECT_NAME}/bt) +install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../gnc/subjugator_vision/models/ DESTINATION share/${PROJECT_NAME}/models) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_select_target_logic test/test_select_target_logic.cpp) + target_include_directories( + test_select_target_logic + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_select_basket_logic test/test_select_basket_logic.cpp) + target_include_directories( + test_select_basket_logic + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_select_face_symbol_logic + test/test_select_face_symbol_logic.cpp) + target_include_directories( + test_select_face_symbol_logic + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_search_pattern test/test_search_pattern.cpp) + target_include_directories( + test_search_pattern + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_detection_gate test/test_detection_gate.cpp) + target_include_directories( + test_detection_gate + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_target_projection test/test_target_projection.cpp) + target_include_directories( + test_target_projection + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_ros_time_budget test/test_ros_time_budget.cpp) + target_include_directories( + test_ros_time_budget + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + + ament_add_gtest(test_quat_math test/test_quat_math.cpp) + target_include_directories( + test_quat_math + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/subjugator_operations/include) + ament_target_dependencies(test_quat_math geometry_msgs) +endif() + ament_package() diff --git a/src/subjugator/mission_planner/config/grasp_targets.yaml b/src/subjugator/mission_planner/config/grasp_targets.yaml new file mode 100644 index 000000000..758de326f --- /dev/null +++ b/src/subjugator/mission_planner/config/grasp_targets.yaml @@ -0,0 +1,19 @@ +--- +# Canonical Task-5 grasp-target names: the single source of truth for +# - the mission target labels (loaded onto the blackboard as grasp_targets / +# grasp_target by mission_planner_node, used by the grasp missions) +# - the sim GripperControl allowlist (sub9_sim.urdf.xacro loads +# this same file via xacro.load_yaml) +# +# These names MUST match the prop entries in +# robosub_2025.world (the Gazebo entity names) and the down-cam YOLO class +# strings (octagon_sim.pt). Edit the list here only. +# +# These are the real Task-5 objects, using Jack's table_2026 meshes. The world +# props in robosub_2025.world carry these same names (static=false, box +# collision, so the gripper can grab them) and octagon_sim.pt detects them. +grasp_targets: + - electric_box + - nut_cylinder + - pill_cylinder + - bandaid_box diff --git a/src/subjugator/mission_planner/include/context.hpp b/src/subjugator/mission_planner/include/context.hpp index 6c5a16944..62fecb030 100644 --- a/src/subjugator/mission_planner/include/context.hpp +++ b/src/subjugator/mission_planner/include/context.hpp @@ -1,6 +1,9 @@ #pragma once +#include +#include #include #include +#include #include #include @@ -26,10 +29,17 @@ struct Context rclcpp::Subscription::SharedPtr targets_sub; rclcpp::Subscription::SharedPtr image_sub; - // Service clients to actuate servos (driver.py services) + // Down-cam perception (Task 5). Separate streams so both YOLO nodes + // (front + down) can run concurrently; consumers pick a camera below. + rclcpp::Subscription::SharedPtr down_targets_sub; + rclcpp::Subscription::SharedPtr down_image_sub; + + // Servo service clients (matched to services exposed by servo_controller/driver.py + // on the real robot, or the GripperControl plugin in sim). Used by ActuateServo. rclcpp::Client::SharedPtr dropper_client; rclcpp::Client::SharedPtr gripper_client; rclcpp::Client::SharedPtr torpedo_client; + // Service clients to actuate servos (driver.py services) // services and clients rclcpp::Client::SharedPtr controller_enable_client; @@ -47,8 +57,99 @@ struct Context uint32_t img_width{ 0 }; uint32_t img_height{ 0 }; + std::mutex down_detections_mx; + std::optional latest_down_detections; + + std::mutex down_img_mx; + uint32_t down_img_width{ 0 }; + uint32_t down_img_height{ 0 }; + + // Robot role (Task 5). Cross-task state: written by Task 1 (gate side) at + // runtime via set_role; read by SelectTarget. Seeded by a startup param + // until Task 1 wiring exists. "" = unknown. + std::mutex role_mx; + std::string role; + + std::string get_role() + { + std::scoped_lock lk(role_mx); + return role; + } + void set_role(std::string const& r) + { + std::scoped_lock lk(role_mx); + role = r; + } + inline rclcpp::Logger logger() const { return node->get_logger(); } + + // Publish an orientation/position goal and cache it as last_goal in one + // step, so the "publish, then remember what we asked for" invariant can + // never drift apart. Every motion node that emits a goal (CenterCamera, + // HoneBearing, SearchForTarget, DescendUntilDetected, AlignDepth, AlignYaw, + // YawStyle) previously repeated this publish + scoped_lock pair by hand. + inline void command_goal(geometry_msgs::msg::Pose const& goal) + { + goal_pub->publish(goal); + std::scoped_lock lk(last_goal_mx); + last_goal = goal; + } + + // Latest detections for the requested camera ("down" selects the down-cam + // stream; anything else falls back to the front-cam stream). Returns a copy + // so callers don't hold the mutex while iterating. + inline std::optional detections_for(std::string const& camera) + { + if (camera == "down") + { + std::scoped_lock lk(down_detections_mx); + return latest_down_detections; + } + std::scoped_lock lk(detections_mx); + return latest_detections; + } + + // Image size for the requested camera. Fills w/h and returns true once a + // frame has been seen (both nonzero); false on cold start. + inline bool image_size_for(std::string const& camera, uint32_t& w, uint32_t& h) + { + if (camera == "down") + { + std::scoped_lock lk(down_img_mx); + w = down_img_width; + h = down_img_height; + } + else + { + std::scoped_lock lk(img_mx); + w = img_width; + h = img_height; + } + return w != 0 && h != 0; + } }; + +// Fetch the shared Context from the "ctx" blackboard port into `ctx` if it is +// not already set, logging a uniform error on failure. Returns true when `ctx` +// is usable. Collapses the guard every BT node repeated verbatim at the top of +// onStart/onRunning/tick: +// if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) { RCLCPP_ERROR(...); return FAILURE; } +// Templated on the node type so this header needs no BehaviorTree dependency -- +// node.getInput is only instantiated at the (BT-aware) call site. +template +inline bool require_ctx(Node& node, std::shared_ptr& ctx, char const* who) +{ + if (ctx) + { + return true; + } + if (!node.template getInput>("ctx", ctx) || !ctx) + { + RCLCPP_ERROR(rclcpp::get_logger("mission_planner"), "%s: missing ctx", who); + return false; + } + return true; +} diff --git a/src/subjugator/mission_planner/package.xml b/src/subjugator/mission_planner/package.xml index 8ae26188c..06b0ffb06 100644 --- a/src/subjugator/mission_planner/package.xml +++ b/src/subjugator/mission_planner/package.xml @@ -19,11 +19,14 @@ std_srvs subjugator_msgs tf2 + yaml-cpp yolo_msgs sensor_msgs sensor_msgs + ament_cmake_gtest + ament_cmake diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index e504438e2..5e4231204 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include @@ -12,17 +13,28 @@ #include "align_yaw.hpp" #include "any_poles_detected.hpp" #include "at_goal_pose.hpp" +#include "center_camera.hpp" #include "check_yolo_model.hpp" +#include "confirm_grasp_by_scale.hpp" #include "context.hpp" +#include "descend_until_detected.hpp" #include "detect_target.hpp" #include "determine_channel_side.hpp" #include "has_found_pair.hpp" #include "hone_bearing.hpp" #include "hone_midpoint.hpp" +#include "lock_target_xy.hpp" #include "log_to_file.hpp" #include "nav_channel_control.hpp" #include "poles_big_enough.hpp" #include "publish_goal.hpp" +#include "record_target_scale.hpp" +#include "ros_delay.hpp" +#include "ros_timeout.hpp" +#include "search_for_target.hpp" +#include "select_basket.hpp" +#include "select_face_symbol.hpp" +#include "select_target.hpp" #include "std_srvs/srv/set_bool.hpp" #include "subjugator_msgs/msg/thruster_efforts.hpp" #include "track_best_pair.hpp" @@ -31,8 +43,7 @@ #include #include #include -#include -#include +#include // RollStyle + PitchStyle #include #include #include @@ -46,6 +57,22 @@ int main(int argc, char** argv) node->declare_parameter("mission", "SonarFollowerTest"); std::string mission_to_run = node->get_parameter("mission").as_string(); + node->declare_parameter("target_freq", 0); + node->declare_parameter("target_freq_tol", 500); + auto target_freq = static_cast(node->get_parameter("target_freq").as_int()); + auto target_freq_tol = static_cast(node->get_parameter("target_freq_tol").as_int()); + + // Role for Task 5 selection. "" on the robot (Task 1 sets it at runtime via + // ctx->set_role); set in sim launch until that wiring exists. + node->declare_parameter("role", ""); + ctx->set_role(node->get_parameter("role").as_string()); + + // Task-5 capstone dials (read by OctagonMission). Defaults run the FULL + // competition sequence so a bare `mission:=OctagonMission` needs no flags. + node->declare_parameter("score_level", 6); // cumulative ceiling 0..6 + node->declare_parameter("do_pinger", 1); // 1 = run S1 acoustic homing + int const score_level = node->get_parameter("score_level").as_int(); + int const do_pinger = node->get_parameter("do_pinger").as_int(); // Topics to subscribe/publish to ctx->goal_pub = node->create_publisher("/goal_pose", 10); @@ -74,10 +101,40 @@ int main(int argc, char** argv) ctx->img_height = msg->height; }); - // Servo service clients (matched to services exposed by servo_controller/driver.py) + // Down-cam perception (Task 5). Defaults are the REAL-robot topics on + // purpose: a forgotten sim override then breaks the local sim run (cheap to + // catch) instead of a pool test someone else is running. + // For sim, override down_image_topic:=/down_cam/image_raw (the gz bridge + // topic). down_detect_topic already matches if the down YOLO node is + // launched with namespace:=yolo_down (-> /yolo_down/detections). + node->declare_parameter("down_detect_topic", "/yolo_down/detections"); + node->declare_parameter("down_image_topic", "/down_camera/rgb/image_raw"); + std::string const down_detect_topic = node->get_parameter("down_detect_topic").as_string(); + std::string const down_image_topic = node->get_parameter("down_image_topic").as_string(); + + ctx->down_targets_sub = + node->create_subscription(down_detect_topic, 10, + [ctx](yolo_msgs::msg::DetectionArray::SharedPtr msg) + { + std::scoped_lock lk(ctx->down_detections_mx); + ctx->latest_down_detections = *msg; + }); + + ctx->down_image_sub = + node->create_subscription(down_image_topic, 10, + [ctx](sensor_msgs::msg::Image::SharedPtr msg) + { + std::scoped_lock lk(ctx->down_img_mx); + ctx->down_img_width = msg->width; + ctx->down_img_height = msg->height; + }); + + // Servo service clients (matched to services exposed by servo_controller/driver.py + // on the real robot, or the GripperControl plugin in sim). Used by ActuateServo. ctx->dropper_client = node->create_client("dropper"); ctx->gripper_client = node->create_client("gripper"); ctx->torpedo_client = node->create_client("torpedo"); + // Servo service clients (matched to services exposed by servo_controller/driver.py) ctx->controller_enable_client = node->create_client("/pid_controller/enable", 10); ctx->raw_effort_pub = node->create_publisher("/thruster_efforts", 10); @@ -104,18 +161,30 @@ int main(int argc, char** argv) factory.registerNodeType("LogToFile"); factory.registerNodeType("DetectTarget"); factory.registerNodeType("HoneBearing"); + factory.registerNodeType("CenterCamera"); + factory.registerNodeType("LockTargetXY"); + factory.registerNodeType("SelectTarget"); + factory.registerNodeType("SelectBasket"); + factory.registerNodeType("SelectFaceSymbol"); factory.registerNodeType("CheckYoloModel"); factory.registerNodeType("TrackLargestPoles"); factory.registerNodeType("PolesBigEnough"); factory.registerNodeType("DetermineChannelSide"); factory.registerNodeType("AnyPolesDetected"); factory.registerNodeType("HasFoundPair"); + factory.registerNodeType("ActuateServo"); + factory.registerNodeType("RecordTargetScale"); + factory.registerNodeType("ConfirmGraspByScale"); factory.registerNodeType("TrackBestPair"); factory.registerNodeType("HoneMidpoint"); factory.registerNodeType("NavChannelControl"); - factory.registerNodeType("ActuateServo"); factory.registerNodeType("AlignDepth"); factory.registerNodeType("AlignYaw"); + factory.registerNodeType("DescendUntilDetected"); + factory.registerNodeType("SearchForTarget"); + // ROS-time (sim-aware) replacements for the builtin /. + factory.registerNodeType("RosTimeout"); + factory.registerNodeType("RosDelay"); factory.registerNodeType>("TopicTicker"); factory.registerNodeType("CountWhenTicked"); @@ -133,6 +202,43 @@ int main(int argc, char** argv) // Create by name auto blackboard = BT::Blackboard::create(); blackboard->set("ctx", ctx); + blackboard->set("score_level", score_level); + blackboard->set("do_pinger", do_pinger); + // Mirror role onto the blackboard for S7's role port (S3/S5 read it from Context). + blackboard->set("role", node->get_parameter("role").as_string()); + blackboard->set("target_freq", target_freq); + blackboard->set("target_freq_tol", target_freq_tol); + + // Load the canonical Task-5 grasp-target list and expose it on the blackboard, so + // the grasp missions and the sim allowlist (sub9_sim.urdf.xacro reads + // the same file) draw labels from one source instead of hard-coding them. + node->declare_parameter("grasp_targets_file", ""); + std::string grasp_targets_file = node->get_parameter("grasp_targets_file").as_string(); + if (grasp_targets_file.empty()) + { + grasp_targets_file = (std::filesystem::path(pkg_share) / "config" / "grasp_targets.yaml").string(); + } + std::vector grasp_targets; + try + { + YAML::Node const gt = YAML::LoadFile(grasp_targets_file); + for (auto const& n : gt["grasp_targets"]) + { + grasp_targets.push_back(n.as()); + } + } + catch (std::exception const& e) + { + RCLCPP_WARN(node->get_logger(), "Could not load grasp_targets from '%s': %s", grasp_targets_file.c_str(), + e.what()); + } + blackboard->set("grasp_targets", grasp_targets); + blackboard->set("grasp_target", grasp_targets.empty() ? std::string{} : grasp_targets.front()); + RCLCPP_INFO(node->get_logger(), "Loaded %zu grasp target(s) from %s", grasp_targets.size(), + grasp_targets_file.c_str()); + + RCLCPP_INFO(node->get_logger(), "Running mission '%s' (role='%s', score_level=%d, do_pinger=%d)", + mission_to_run.c_str(), node->get_parameter("role").as_string().c_str(), score_level, do_pinger); std::unique_ptr tree_ptr; try diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/acquire_table.xml b/src/subjugator/mission_planner/subjugator_missions/xml/acquire_table.xml new file mode 100644 index 000000000..413f6abf8 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/acquire_table.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml b/src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml new file mode 100644 index 000000000..c8a458eb6 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/approach_and_grasp.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/center_camera_test_mission.xml b/src/subjugator/mission_planner/subjugator_missions/xml/center_camera_test_mission.xml new file mode 100644 index 000000000..e12e2a8e0 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/center_camera_test_mission.xml @@ -0,0 +1,28 @@ + + + + + + + + diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/collect_objects.xml b/src/subjugator/mission_planner/subjugator_missions/xml/collect_objects.xml new file mode 100644 index 000000000..a8bf66a2b --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/collect_objects.xml @@ -0,0 +1,35 @@ + + + + +