Merge pull request #312 from bit-bots/feature/use_look_at_ball_before_dribble

Use look at ball before dribble

Author: jaagut

bitbots_body_behavior/bitbots_body_behavior/actions/head_modes.py

@@ -18,7 +18,7 @@ def __init__(self, blackboard, dsd, parameters=None):
         super().__init__(blackboard, dsd, parameters)
 
     def perform(self):
-        ball_position = self.blackboard.world_model.get_best_ball_position()
+        ball_position = self.blackboard.world_model.get_best_ball_point_stamped()
         server_running = self.blackboard.lookat_action_client.wait_for_server(timeout_sec=1.0)
         if not server_running:
             while not server_running and rclpy.ok():
@@ -34,7 +34,7 @@ def perform(self):
                 return self.pop()
 
         goal = LookAt.Goal()
-        goal.target_point = ball_position
+        goal.look_at_position = ball_position
         self.blackboard.lookat_action_client.send_goal_async(goal)
         return self.pop()
 

bitbots_body_behavior/bitbots_body_behavior/main.dsd

@@ -14,14 +14,14 @@
 @ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0, @LookForward + r:false, @KickBallDynamic, @LookAtFieldFeatures + r:false, @WalkInPlace + duration:1 + r:false
 
 #Dribble
-@ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtFront, @DribbleForward
+@ChangeAction + action:going_to_ball, @CancelPathplanning, @LookAtBall, @LookAtFront, @DribbleForward
 
 #DribbleWithAvoidance
 $AvoidBall
     NO --> $BallClose + distance:%body.ball_reapproach_dist + angle:%body.ball_reapproach_angle
         YES --> $BallKickArea
             NEAR --> #Dribble
-            FAR --> @ChangeAction + action:going_to_ball, @LookAtFront, @GoToBall + target:map_goal
+            FAR --> @ChangeAction + action:going_to_ball, @LookAtFront, @GoToBall + target:map_goal 
         NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map_goal + blocking:false + distance:%body.ball_far_approach_dist
     YES --> $ReachedPathPlanningGoalPosition + thres:%body.ball_far_approach_position_thresh
         YES --> @AvoidBallInactive