Merge branch 'main' into fix/docs_extrinsic_calibration_command_2

Author: Flova

.devcontainer/Dockerfile

@@ -32,7 +32,6 @@ RUN apt-get update -y \
     tree \
     uvcdynctrl \
     vim \
-    vlc \
     wget \
     x11-apps \
     zsh

.vscode/settings.json

@@ -112,6 +112,7 @@
         "throwin",
         "timespec",
         "tldr",
+        "torqueless",
         "tqdm",
         "unpenalize",
         "unpenalized",
@@ -212,7 +213,8 @@
         "variant": "cpp",
         "regex": "cpp",
         "future": "cpp",
-        "*.ipp": "cpp"
+        "*.ipp": "cpp",
+        "span": "cpp"
     },
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set

Makefile

@@ -47,6 +47,8 @@ pull-files:
 	wget \
 		--no-verbose \
 		--show-progress \
+		--timeout=15 \
+		--tries=2 \
 		--recursive \
 		--timestamping \
 		--no-parent \
@@ -57,6 +59,8 @@ pull-files:
 	wget \
 		--no-verbose \
 		--show-progress \
+		--timeout=15 \
+		--tries=2 \
 		--recursive \
 		--timestamping \
 		--no-parent \
@@ -86,7 +90,7 @@ rosdep:
 	# Initialize rosdep if not already done
 	[ -f /etc/ros/rosdep/sources.list.d/20-default.list ] || sudo rosdep init
 	# Update rosdep and install dependencies from meta directory
-	rosdep update
+	rosdep update --include-eol-distros
 	rosdep install --from-paths . --ignore-src --rosdistro iron -y
 
 status:

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
 
@@ -175,16 +174,21 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa
         elif BallGoalType.CLOSE == target:
             ball_u, ball_v = self._blackboard.world_model.get_ball_position_uv()
             angle = math.atan2(ball_v, ball_u)
-            ball_point = (ball_u, ball_v, angle, self._blackboard.world_model.base_footprint_frame)
+            goal_u = ball_u - math.cos(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_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -189,6 +189,19 @@ def is_not_goalie(team_data: TeamData) -> bool:
         # Count valid team data infos (aka robots with valid team data)
         return sum(map(self.is_valid, team_data_infos))
 
+    def get_is_goalie_active(self) -> bool:
+        def is_a_goalie(team_data: TeamData) -> bool:
+            return team_data.strategy.role == Strategy.ROLE_GOALIE
+
+        # Get the team data infos for all robots (ignoring the robot id/name)
+        team_data_infos = self.team_data.values()
+
+        # Remove none goalie Data
+        team_data_infos = filter(is_a_goalie, team_data_infos)
+
+        # Count valid team data infos (aka robots with valid team data)
+        return sum(map(self.is_valid, team_data_infos)) == 1
+
     def get_own_time_to_ball(self) -> float:
         return self.own_time_to_ball
 

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py

@@ -56,6 +56,7 @@ def __init__(self, blackboard, dsd, parameters):
         """Go to an absolute position on the field"""
         super().__init__(blackboard, dsd, parameters)
         self.point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
+        self.blocking = parameters.get("blocking", True)
 
     def perform(self, reevaluate=False):
         pose_msg = PoseStamped()
@@ -64,11 +65,26 @@ def perform(self, reevaluate=False):
 
         pose_msg.pose.position.x = self.point[0]
         pose_msg.pose.position.y = self.point[1]
-        pose_msg.pose.position.z = 0
+        pose_msg.pose.position.z = 0.0
         pose_msg.pose.orientation = quat_from_yaw(math.radians(self.point[2]))
 
         self.blackboard.pathfinding.publish(pose_msg)
 
+        if not self.blocking:
+            self.pop()
+
+
+class GoToAbsolutePositionFieldFraction(GoToAbsolutePosition):
+    def __init__(self, blackboard, dsd, parameters):
+        """Go to an absolute position of the field, specified by the fraction of the field size"""
+        super().__init__(blackboard, dsd, parameters)
+        point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
+        self.point = (
+            point[0] * self.blackboard.world_model.field_length / 2,
+            point[1] * self.blackboard.world_model.field_width / 2,
+            self.point[2],
+        )
+
 
 class GoToOwnGoal(GoToAbsolutePosition):
     def __init__(self, blackboard, dsd, parameters):
@@ -77,7 +93,7 @@ def __init__(self, blackboard, dsd, parameters):
         self.point = (
             self.blackboard.world_model.get_map_based_own_goal_center_xy()[0],
             self.blackboard.world_model.get_map_based_own_goal_center_xy()[1],
-            parameters,
+            self.point[2],
         )
 
 
@@ -88,12 +104,12 @@ def __init__(self, blackboard, dsd, parameters):
         self.point = (
             self.blackboard.world_model.get_map_based_opp_goal_center_xy()[0],
             self.blackboard.world_model.get_map_based_opp_goal_center_xy()[1],
-            parameters,
+            self.point[2],
         )
 
 
 class GoToCenterpoint(GoToAbsolutePosition):
     def __init__(self, blackboard, dsd, parameters):
         """Go to the center of the field and look towards the enemy goal"""
         super().__init__(blackboard, dsd, parameters)
-        self.point = 0, 0, 0
+        self.point = 0.0, 0.0, 0.0

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/decisions/goalie_active.py

@@ -0,0 +1,22 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+
+
+class GoalieActive(AbstractDecisionElement):
+    """
+    Decides whether the goalie is on field or not
+    """
+
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+
+    def perform(self, reevaluate=False):
+        if self.blackboard.team_data.get_is_goalie_active():
+            return "YES"
+        else:
+            return "NO"
+
+    def get_reevaluate(self):
+        return True

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd

@@ -0,0 +1,28 @@
+#SearchBall
+@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn
+
+#PerformKickLeft
+@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, @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:%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:%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
+
+-->BodyBehavior
+$DoOnce
+    NOT_DONE --> @Stand + duration:15
+    DONE --> $BallSeen
+        YES --> #KickWithAvoidance
+        NO --> #SearchBall

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -1,5 +1,15 @@
 #SearchBall
-@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn
+$DoOnce
+    NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
+    DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true
+        NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5
+        YES --> $DoOnce
+            NOT_DONE --> @Turn + duration:15
+            DONE --> $DoOnce
+                NOT_DONE --> @GoToAbsolutePositionFieldFraction + y:1.0 + t:-90 + blocking:false
+                DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.2 + latch:true
+                    YES --> @Stand
+                    NO --> @GoToAbsolutePositionFieldFraction + y:1.0 + t:-90
 
 #DoNothing
 @ChangeAction + action:waiting, @LookForward, @Stand
@@ -61,7 +71,9 @@ $GoalieHandlingBall
     NO --> #KickWithAvoidance
 
 #DefensePositioning
-@LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
+$GoalieActive
+    YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToDefensePosition
+    NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToBlockPosition
 
 #SupporterRole
 $PassStarted
@@ -148,7 +160,7 @@ $IsPenalized
         READY --> $AnyGoalScoreRecently + time:50
             YES --> #PositioningReady
             NO --> $DoOnce
-                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:1, @GetWalkready + r:false
+                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
                 DONE --> #PositioningReady
         SET --> $SecondaryStateDecider
             PENALTYSHOOT --> $SecondaryStateTeamDecider