Add active vision

AGENTS.md

@@ -43,11 +43,49 @@ package's manifests and nearby code before choosing tools or patterns.
   as they might change later, thus making the documentation outdated.
   Instead, describe the expected behavior or refer to the relevant code sections.
 
+## Failure Handling
+
+Handle unexpected states explicitly and loudly. A robot that behaves subtly
+wrong is far harder to debug than one that says what is missing and does
+nothing. Never invent a value to keep going.
+
+- Never substitute a made up default for missing or malformed input. A
+  fabricated value is indistinguishable from a real one, both to the rest of the
+  system and to whoever debugs it later. Zero, identity, and empty are the most
+  dangerous of these, because they are frequently valid readings.
+- Prefer making the failure unrepresentable. Return `std::optional`, a status,
+  or a validity flag rather than a plausible looking value, so that callers have
+  to acknowledge the failure instead of silently inheriting it.
+- Log at the point where enough context exists to say what is actually wrong,
+  and name the missing input. Prefer throttled logging in periodic code so a
+  persistent fault does not flood the log. Use a warning when the node can
+  continue degraded, an error when it cannot do its job at all.
+- Drop the affected update rather than the whole process. Skipping one cycle and
+  reporting why is almost always better than crashing, and always better than
+  publishing a command derived from data that was not there.
+- Fail early. Validate inputs, parameters and transforms where they enter the
+  system, not at the point where the bad value finally produces a visible
+  symptom.
+- Keep retrying inputs that legitimately arrive late, such as transforms,
+  parameters from other nodes, and latched topics. Do not disable a feature for
+  the rest of the run because of a startup race, and keep reporting the wait.
+- Distinguish "not available yet" from "broken". The first is expected during
+  startup and should be reported as a wait, the second should be reported as an
+  error.
+- Do not let a fallback path quietly replace the intended one. If a degraded
+  mode exists, make entering it visible in the logs and say which mode is
+  running.
+- Treat contract violations, such as an out of range index or a precondition a
+  caller must uphold, as programming errors and assert on them, rather than
+  clamping the input into a range that hides the bug.
+
 ## Development Environment
 
 This ROS 2 workspace is managed by Pixi. Run development commands through the
-repository's Pixi environments; do not invoke `colcon`, ROS 2 tools, or formatters
-directly from the host shell.
+repository's Pixi tasks; do not invoke `colcon`, ROS 2 tools, or formatters
+directly from the host shell, and do not build, clean, or format by hand when a
+task exists for it. The tasks carry flags the workspace depends on, and hand
+written equivalents silently drop them.
 
 - Use the `default` environment for normal development.
   It contains the `ros` and `format` features.
@@ -76,6 +114,12 @@ Common commands:
 - Run one-off tools with `pixi run -e default <command>`.
 - Clean all workspace build artifacts with `pixi run -e default clean`.
 - Clean one package with `pixi run -e default clean <package>`.
+  Prefer this over cleaning everything, because a full clean forces a rebuild of
+  the entire workspace.
+- Never remove `build/`, `install/`, or `log/` by hand, for example with `rm -rf`.
+  Removing the install space breaks every package that is not rebuilt with it,
+  including ones outside the selection being worked on, and recovering requires a
+  full workspace rebuild. Use the clean task, which scopes the removal correctly.
 - Use `pixi clean` only to reset Pixi's local environment data.
   This requires downloading dependencies and rebuilding afterward.
 

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -14,6 +14,7 @@
     HeadMode.DONT_MOVE,
     HeadMode.SEARCH_BALL_PENALTY,
     HeadMode.SEARCH_FRONT,
+    HeadMode.ACTIVE_VISION,
 ]
 
 

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py

@@ -107,3 +107,11 @@ class LookAtFront(AbstractHeadModeElement):
     def perform(self):
         self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FRONT)
         return self.pop()
+
+
+class ActiveVisionHeadMove(AbstractHeadModeElement):
+    """Uses the active vision (has nothing to do with the vision node itself) to look for objects in the environment"""
+
+    def perform(self):
+        self.blackboard.misc.set_head_duty(HeadMode.ACTIVE_VISION)
+        return self.pop()

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -1,8 +1,8 @@
 #SearchBall
 $DoOnce
-    NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:1, @TurnLastSeenBallSide + duration:6, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
+    NOT_DONE --> @ChangeAction + action:searching, @ActiveVisionHeadMove, @WalkInPlace + duration:1, @TurnLastSeenBallSide + duration:6, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
     DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true
-        NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5
+        NO --> @ActiveVisionHeadMove, @GoToAbsolutePositionFieldFraction + x:0.5
         YES --> $DoOnce
             NOT_DONE --> @Turn + duration:15
             DONE --> $DoOnce
@@ -25,17 +25,17 @@ $DoOnce
 
 #RolePositionWithPause
 $DoOnce
-    NOT_DONE --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToRolePosition + blocking:false
+    NOT_DONE --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToRolePosition + blocking:false
     DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.2 + latch:true
         YES -->  #StandAndLook
-        NO --> @LookAtFieldFeatures, @GoToRolePosition
+        NO --> @ActiveVisionHeadMove, @GoToRolePosition
 
 #KickWithAvoidance
 $KickOrDribble + threshold_upfield:2.0 + threshold_downfield:0.5
     KICK --> $DoOnce
-        NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick + blocking:false
+        NOT_DONE --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:rl_kick + blocking:false
         DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:%rl_kick_approach_tolerance_pos + orientation_threshold:%rl_kick_approach_tolerance_deg
-            NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick
+            NO --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:rl_kick
             YES --> $DoOnce
                 NOT_DONE --> @StoreBallMovementDetectionStartPosition
                 DONE --> @ChangeAction + action:kicking, @Stand + duration:0.1 + r:false, @LookAtBallPenalty + r:false, @RLKickTowardsGoal + strength:3.0 + r:false, @ForgetBall + r:false
@@ -45,46 +45,46 @@ $KickOrDribble + threshold_upfield:2.0 + threshold_downfield:0.5
                 NEAR --> $DoOnce
                     NOT_DONE --> @StoreBallMovementDetectionStartPosition
                     DONE --> #Dribble
-                FAR --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map
-            NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist
+                FAR --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:map
+            NO --> @ChangeAction + action:going_to_ball + r:false, @ActiveVisionHeadMove + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist
         YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh
             YES --> @AvoidBallInactive
-            NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map + distance:%ball_far_approach_dist
+            NO --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:map + distance:%ball_far_approach_dist
 
 #PositioningReady
 $GoalScoreRecently
     YES --> $ConfigRole
         GOALIE -->  $RobotInOwnPercentOfField + p:40
             YES --> @Stand + duration:1.0 + r:false, @PlayAnimationCheering + r:false, @Stand
-            NO -->  @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
-        ELSE --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
-    NO --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
+            NO -->  @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
+        ELSE --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
+    NO --> @ChangeAction + action:positioning, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition
 
 #SupporterRole
 $BallSeen
     YES --> $PassStarted
         YES --> @TrackBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition
-        NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation
-    NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation
+        NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation
+    NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation
 
 
 #PenaltyShootoutBehavior
 $SecondaryStateTeamDecider
-    OUR --> @StandAndWaitRandom + min:10 + max:25, @ChangeAction + action:kicking, @LookAtFieldFeatures, @WalkInPlace + duration:2, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @WalkInPlace + duration:1 + r:false, @Stand
+    OUR --> @StandAndWaitRandom + min:10 + max:25, @ChangeAction + action:kicking, @ActiveVisionHeadMove, @WalkInPlace + duration:2, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @WalkInPlace + duration:1 + r:false, @Stand
     ELSE --> $BallDangerous + radius:1.3
         LEFT --> @PlayAnimationGoalieFallLeft, @Stand
         RIGHT --> @PlayAnimationGoalieFallRight, @Stand
         CENTER --> @PlayAnimationGoalieFallCenter, @Stand
         ELSE --> $BallSeen
             YES --> @TrackBall, @Stand
-            NO --> @LookAtFieldFeatures, @Stand
+            NO --> @ActiveVisionHeadMove, @Stand
 
 #Init
 @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand
 
 #NormalBehavior
 $SecondBallTouchAllowed
-    NO --> @SetNoSecondBallContactVariable + value:false + r:false, @LookAtFieldFeatures + r:false, @ChangeAction + action:passive + r:false, @AvoidBallActive + r:false, @GoToFormationPosition //defender variables
+    NO --> @SetNoSecondBallContactVariable + value:false + r:false, @ActiveVisionHeadMove + r:false, @ChangeAction + action:passive + r:false, @AvoidBallActive + r:false, @GoToFormationPosition //defender variables
     YES --> $BallSeen
         NO --> $ConfigRole
             GOALIE --> $CountActiveRobotsWithoutGoalie
@@ -95,14 +95,14 @@ $SecondBallTouchAllowed
             STRIKER --> #KickWithAvoidance
             SUPPORTER --> #SupporterRole
             ELSE --> $BallInOwnPercent + p:40
-                YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToFormationPosition
-                NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToFormationPosition + stand:true + enter_position:%defender_enter_position + leave_position:%defender_leave_position + enter_orientation:%defender_enter_orientation + leave_orientation:%defender_leave_orientation
+                YES --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToFormationPosition
+                NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToFormationPosition + stand:true + enter_position:%defender_enter_position + leave_position:%defender_leave_position + enter_orientation:%defender_enter_orientation + leave_orientation:%defender_leave_orientation
 
 #ActivateDoubleTouchProtection
 @SetNoSecondBallContactVariable + value:true + r:false, @ForgetBallStartPosition + r:false
 
 #DefensiveSetPlay
-@ChangeAction + action:positioning, @ForgetBall, @LookAtFieldFeatures, @GoToFormationPosition + set_play:true + stand:true + enter_position:%placing_enter_position + leave_position:%placing_leave_position + enter_orientation:%placing_enter_orientation + leave_orientation:%placing_leave_orientation
+@ChangeAction + action:positioning, @ForgetBall, @ActiveVisionHeadMove, @GoToFormationPosition + set_play:true + stand:true + enter_position:%placing_enter_position + leave_position:%placing_leave_position + enter_orientation:%placing_enter_orientation + leave_orientation:%placing_leave_orientation
 
 #SetPlaySituation
 $DoOnce
@@ -135,7 +135,7 @@ $IsPenalized
                     YES --> #StandAndLook
                     NO --> #PositioningReady
                 NO --> $DoOnce
-                    NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
+                    NOT_DONE --> @ChangeAction + action:waiting + r:false, @ActiveVisionHeadMove + r:false, @Stand + duration:2
                     DONE --> $ReachedAndAlignedToConfigRolePosition
                         YES --> #StandAndLook
                         NO --> #PositioningReady
@@ -151,7 +151,7 @@ $IsPenalized
                     OUR --> @Stand + duration:5.0 + r:false, @LookForward + r:false, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @Stand
                     OTHER --> $BallSeen
                         YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball
-                        NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
+                        NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @ActiveVisionHeadMove + r:false, @PlayAnimationGoalieArms + r:false, @Stand
                 ELSE --> #StandAndLook
         FINISHED --> $CurrentScore
             AHEAD --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand

src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py

@@ -50,6 +50,7 @@
 3: Don't move the head
 4: Ball Mode adapted for Penalty Kick
 5: Do a pattern which only looks in front of the robot
+6: Active vision (ball tracking, field coverage, ...)
 
 Simulation only:
 r: reset robot in simulation
@@ -250,6 +251,10 @@ def loop(self):
                     # Do a pattern which only looks in front of the robot
                     self.head_mode_msg.head_mode = HeadMode.SEARCH_FRONT
                     assert int(key) == HeadMode.SEARCH_FRONT
+                elif key == "6":
+                    # Active vision (ball tracking, field coverage, ...)
+                    self.head_mode_msg.head_mode = HeadMode.ACTIVE_VISION
+                    assert int(key) == HeadMode.ACTIVE_VISION
                 elif key == "F":
                     # play walkready animation
                     self.get_walkready()