Add sim demo behavior

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

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()

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd

@@ -8,38 +8,33 @@
 @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
 
 #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

bitbots_misc/bitbots_bringup/launch/demo.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sim" default="false" description="Whether the robot is running in simulation or on real hardware" />
+    <arg name="behavior_dsd_file" default="demo.dsd" description="The behavior dsd file that should be used" />
+
+    <!-- load teamplayer software stack without some unnecessary stuff, that is not needed in the demo -->
+    <include file="$(find-pkg-share bitbots_bringup)/launch/teamplayer.launch">
+        <arg name="behavior_dsd_file" value="$(var behavior_dsd_file)" />
+        <arg name="game_controller" value="false"/>
+        <arg name="localization" value="false" />
+        <arg name="sim" value="$(var sim)" />
+        <arg name="teamcom" value="false" />
+    </include>
+</launch>