diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py index 9c8490430..a621bf0a3 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py @@ -26,7 +26,6 @@ class PathfindingCapsule(AbstractBlackboardCapsule): def __init__(self, node, blackboard): super().__init__(node, blackboard) - self.map_frame: str = self._node.get_parameter("map_frame").value self.position_threshold: str = self._node.get_parameter("pathfinding_position_threshold").value self.orientation_threshold: str = self._node.get_parameter("pathfinding_orientation_threshold").value @@ -176,17 +175,20 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa ball_u, ball_v = self._blackboard.world_model.get_ball_position_uv() angle = math.atan2(ball_v, ball_u) goal_u = ball_u - math.cos(angle) * distance - goal_v = ball_v - math.sin(angle) * distance + goal_v = ball_v - math.sin(angle) * distance + side_offset ball_point = (goal_u, goal_v, angle, self._blackboard.world_model.base_footprint_frame) else: self._node.get_logger().error(f"Target {target} for go_to_ball action not implemented.") return + # Create the goal pose message pose_msg = PoseStamped() - pose_msg.header.stamp = self._node.get_clock().now().to_msg() pose_msg.header.frame_id = ball_point[3] pose_msg.pose.position = Point(x=ball_point[0], y=ball_point[1], z=0.0) pose_msg.pose.orientation = quat_from_yaw(ball_point[2]) + # Convert the goal to the map frame + pose_msg = self._blackboard.tf_buffer.transform(pose_msg, self._blackboard.map_frame) + return pose_msg diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py index 86a04b5e1..ebb24ec83 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py @@ -22,9 +22,11 @@ def __init__(self, blackboard, dsd, parameters): self.blocking = parameters.get("blocking", True) self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"]) + # Offset so we kick the ball with one foot instead of the center between the feet + self.side_offset = parameters.get("side_offset", 0.08) def perform(self, reevaluate=False): - pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, 0.08) + pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset) self.blackboard.pathfinding.publish(pose_msg) approach_marker = Marker() diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd index e35d7df90..e8b4b4e67 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd @@ -8,23 +8,23 @@ @ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand #PerformKickLeft -@ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:left + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false +@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:left + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false #PerformKickRight -@ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:right + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false +@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 #KickWithAvoidance $AvoidBall - NO --> $BallClose + distance:%body.ball_reapproach_dist + angle:%body.ball_reapproach_angle + NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle YES --> $BallKickArea 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:%body.ball_far_approach_dist - YES --> $ReachedPathPlanningGoalPosition + threshold:%body.ball_far_approach_position_thresh + NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @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:%body.ball_far_approach_dist + NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%ball_far_approach_dist #Init @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand @@ -32,14 +32,9 @@ $AvoidBall #NormalBehavior $BallSeen NO --> #SearchBall - YES --> #StrikerRole + YES --> #KickWithAvoidance -->BodyBehavior $IsPenalized YES --> #DoNothing ELSE --> #NormalBehavior - INITIAL --> #Init - READY --> #StandAndLook - SET --> #StandAndLook - FINISHED --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand - PLAYING --> #NormalBehavior diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch new file mode 100644 index 000000000..e3c1ef3cd --- /dev/null +++ b/bitbots_misc/bitbots_bringup/launch/demo.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + +