Fix / Clean up look at handling (#723)

Author: Flova

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -8,7 +8,7 @@
 from bitbots_msgs.msg import Audio, HeadMode, RobotControlState
 
 THeadMode: TypeAlias = Literal[  # type: ignore[valid-type]
-    HeadMode.SEARCH_BALL,
+    HeadMode.TRACK_BALL,
     HeadMode.SEARCH_FIELD_FEATURES,
     HeadMode.LOOK_FORWARD,
     HeadMode.DONT_MOVE,

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

@@ -1,8 +1,8 @@
-import rclpy
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+from rclpy.task import Future
 
-from bitbots_msgs.action import LookAt
+from bitbots_msgs.action import LookAt as LookAtROSAction
 from bitbots_msgs.msg import HeadMode
 
 
@@ -12,40 +12,60 @@ class AbstractHeadModeElement(AbstractActionElement):
     blackboard: BodyBlackboard
 
 
-class LookAtBall(AbstractHeadModeElement):
-    """Search for Ball and track it if found"""
+class LookAt(AbstractHeadModeElement):
+    """Looks at a given point in a given frame. Pass x, y, z and frame as parameters."""
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
+        self.blocking = parameters.get("blocking", True)
+        self.finished = not self.blocking
 
-    def perform(self):
-        ball_position = self.blackboard.world_model.get_best_ball_point_stamped()
-        server_running = self.blackboard.animation.lookat_action_client.wait_for_server(timeout_sec=1.0)
+        # Check if action server is running
+        server_running = self.blackboard.animation.lookat_action_client.wait_for_server(timeout_sec=0.1)
         if not server_running:
-            while not server_running and rclpy.ok():
-                self.blackboard.node.get_logger().warn(
-                    "Lookat Action Server not running! Lookat cannot work without lookat server!"
-                    "Will now wait until server is accessible!",
-                    throttle_duration_sec=10.0,
-                )
-                server_running = self.blackboard.animation.lookat_action_client.wait_for_server(timeout_sec=1)
-            if server_running:
-                self.blackboard.node.get_logger().warn("Lookat server now running, 'look_at_ball' action will go on.")
-            else:
-                self.blackboard.node.get_logger().warn("Lookat server did not start. Did not send action.")
-                return self.pop()
-
-        goal = LookAt.Goal()
-        goal.look_at_position = ball_position
-        self.blackboard.animation.lookat_action_client.send_goal_async(goal)
-        return self.pop()
+            self.blackboard.node.get_logger().error("LookAt action server not running! Failed to look at ball.")
+            self.finished = True
+            return
+
+        # Validate parameter
+        assert "frame" in parameters, "LookAt action missing frame parameter"
+
+        # Create look at goal
+        goal = LookAtROSAction.Goal()
+        goal.look_at_position.point.x = parameters.get("x", 0.0)
+        goal.look_at_position.point.y = parameters.get("y", 0.0)
+        goal.look_at_position.point.z = parameters.get("z", 0.0)
+        goal.look_at_position.header.frame_id = parameters["frame"]
+
+        # Handle action server responses
+        def goal_response_callback(future: Future):
+            goal_handle = future.result()
+            if not goal_handle.accepted:
+                self.blackboard.node.get_logger().warn("Can not perform look at, goal rejected")
+                self.finished = True
+                return
+
+            def done_callback(future: Future):
+                result = future.result().result
+                if not result.success:
+                    self.blackboard.node.get_logger().warn("Looking at failed")
+                self.finished = True
+
+            goal_handle.get_result_async().add_done_callback(done_callback)
+
+        # Call the action server
+        self.blackboard.animation.lookat_action_client.send_goal_async(goal).add_done_callback(goal_response_callback)
+
+    def perform(self):
+        if self.finished:
+            return self.pop()
 
 
-class SearchBall(AbstractHeadModeElement):
-    """Look for ball"""
+class TrackBall(AbstractHeadModeElement):
+    """Tracks the last known ball position"""
 
     def perform(self):
-        self.blackboard.misc.set_head_duty(HeadMode.SEARCH_BALL)
+        self.blackboard.misc.set_head_duty(HeadMode.TRACK_BALL)
         return self.pop()
 
 

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd

@@ -14,11 +14,11 @@ $AvoidBall
             NEAR --> $FootSelection
                 LEFT --> #PerformKickLeft
                 RIGHT --> #PerformKickRight
-            FAR --> @ChangeAction + action:going_to_ball, @LookAtFront, @GoToBall + target:close
-        NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:close + blocking:false + distance:%ball_far_approach_dist
+            FAR --> @ChangeAction + action:going_to_ball, @TrackBall, @GoToBall + target:close
+        NO --> @ChangeAction + action:going_to_ball + r:false, @TrackBall, @AvoidBallActive + r:false, @GoToBall + target:close + 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:close + distance:%ball_far_approach_dist
+        NO --> @ChangeAction + action:going_to_ball, @TrackBall, @GoToBall + target:close + distance:%ball_far_approach_dist
 
 -->BodyBehavior
 $DoOnce

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -27,7 +27,7 @@ $DoOnce
 @ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:right + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
 
 #Dribble
-@ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtBall, @LookAtFront, @DribbleForward
+@ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtFront, @DribbleForward
 
 #RolePositionWithPause
 $DoOnce
@@ -78,8 +78,8 @@ $GoalieActive
 #SupporterRole
 $PassStarted
     YES --> $BallSeen
-        YES --> @LookAtBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition
-        NO --> @SearchBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition
+        YES --> @TrackBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition
+        NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassAcceptPosition
     NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToPassPreparePosition
 
 #GoalieRole
@@ -97,8 +97,8 @@ $SecondaryStateTeamDecider
         RIGHT --> @PlayAnimationGoalieFallRight, @Stand
         CENTER --> @PlayAnimationGoalieFallCenter, @Stand
         ELSE --> $BallSeen
-            YES --> @LookAtBall, @Stand
-            NO --> @SearchBall, @Stand
+            YES --> @TrackBall, @Stand
+            NO --> @LookAtFieldFeatures, @Stand
 
 
 #Placing
@@ -179,8 +179,8 @@ $IsPenalized
             PENALTYSHOOT --> $SecondaryStateTeamDecider
                 OUR --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @Stand // we need to also see the goalie
                 OTHER --> $BallSeen
-                    YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @LookAtBall + 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, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @SearchBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand
+                    YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + 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, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
             ELSE --> #StandAndLook
         FINISHED --> $CurrentScore
             AHEAD --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @GetWalkready + r:false, @LookForward, @Stand

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/minimal.dsd

@@ -51,7 +51,7 @@
 R: reset ball in simulation
 
 Head Modes:
-0: Search for Ball and track it if found
+0: Track the last known ball position
 1: Look generally for all features on the field (ball, goals, corners, center point)
 2: Simply look directly forward
 3: Don't move the head
@@ -147,7 +147,7 @@ def __init__(self):
         self.head_msg.joint_names = ["HeadPan", "HeadTilt"]
         self.head_msg.positions = [0.0] * 2
 
-        self.head_mode_msg = HeadMode()
+        self.head_mode_msg = HeadMode(head_mode=HeadMode.DONT_MOVE)
 
         self.head_pan_step = 0.05
         self.head_tilt_step = 0.05
@@ -221,9 +221,9 @@ def loop(self):
                     self.head_msg.positions[1] = 0
                     self.head_pub.publish(self.head_msg)
                 elif key == "0":
-                    # Search for Ball and track it if found
-                    self.head_mode_msg.head_mode = HeadMode.SEARCH_BALL
-                    assert int(key) == HeadMode.SEARCH_BALL
+                    # Track the last known ball position
+                    self.head_mode_msg.head_mode = HeadMode.TRACK_BALL
+                    assert int(key) == HeadMode.TRACK_BALL
                 elif key == "1":
                     # Look generally for all features on the field (ball, goals, corners, center point)
                     self.head_mode_msg.head_mode = HeadMode.SEARCH_FIELD_FEATURES

bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py

@@ -45,49 +45,6 @@ move_head:
 
   # Search pattern for ball
   search_patterns:
-    search_ball:
-      # search pattern speed
-      tilt_speed:
-        type: double
-        description: "Tilt speed for the search pattern"
-        default_value: 3.0
-
-      pan_speed:
-        type: double
-        description: "Pan speed for the search pattern"
-        default_value: 3.0
-
-      # Max values for the search pattern
-      pan_max:
-        type: double_array
-        default_value: [90.0, -90.0]
-        description: "Maximum pan values for the search pattern (in degrees)"
-        validation:
-          fixed_size<>: 2
-
-      tilt_max:
-        type: double_array
-        default_value: [-10.0, -60.0]
-        description: "Maximum tilt values for the search pattern (in degrees)"
-        validation:
-          fixed_size<>: 2
-
-      # Number of scan lines for the search pattern
-      scan_lines:
-        type: int
-        default_value: 2
-        description: "Number of scan lines for the search pattern"
-        validation:
-          gt_eq<>: [1]
-
-      # Reduces last scanline by that factor so that robot does not collide
-      reduce_last_scanline:
-        type: double
-        default_value: 0.2
-        description: "Reduces last scanline by that factor so that robot does not collide"
-        validation:
-          bounds<>: [0.0, 1.0]
-
     search_ball_penalty:
       tilt_speed:
         type: double