Merge branch 'main' into feature/imu_yaw_in_odometry

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_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

@@ -71,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

bitbots_misc/bitbots_bringup/launch/demo.launch

@@ -0,0 +1,22 @@
+<?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" />
+        <arg name="path_planning" value="false" />
+    </include>
+
+    <!-- load the path planning node in dummy mode, because we are limited by the map size otherwise and together with no localization 
+    this could lead to the robot not working after a while, because due to odometry errors the robot could be outside of the map -->
+    <include file="$(find-pkg-share bitbots_path_planning)/launch/path_planning.launch">
+        <arg name="sim" value="$(var sim)" />
+        <arg name="dummy" value="true" />
+    </include>
+</launch>

bitbots_misc/system_monitor/config/config.yaml

@@ -5,11 +5,13 @@ system_monitor:
 
     # These settings are quick_switches to completely disable certain parts of statistic collection
     do_cpu: true
+    do_gpu: true
     do_memory: true
     do_network: false
 
     # these are the threshold values at which we start going into a warn state
     cpu_load_percentage: 80.0
+    gpu_load_percentage: 95.0
     memory_load_percentage: 80.0
     network_rate_received_errors: 10.0
     network_rate_send_errors: 10.0

bitbots_misc/system_monitor/system_monitor/cpus.py

@@ -36,7 +36,8 @@ def collect_all():
 def _get_cpu_stats():
     """
     read and parse /proc/stat
-    :returns timings which contains accumulative busy and total cpu time
+
+    :returns: timings which contains accumulative busy and total cpu time
     """
     timings = {}
     with open("/proc/stat") as file_obj:

bitbots_misc/system_monitor/system_monitor/gpu.py

@@ -0,0 +1,19 @@
+import pyamdgpuinfo
+
+
+def collect_all():
+    """
+    use pyamdgpuinfo to get gpu metrics
+
+    :return: (load, vram_used, vram_total, temperature)
+    """
+    if pyamdgpuinfo.detect_gpus() == 0:
+        return (0, 0, 0, 0)
+
+    gpu = pyamdgpuinfo.get_gpu(0)
+    load = gpu.query_load()
+    vram_total = gpu.memory_info["vram_size"]
+    vram_used = gpu.query_vram_usage()
+    temperature = gpu.query_temperature()
+
+    return (load, vram_used, vram_total, temperature)

bitbots_misc/system_monitor/system_monitor/monitor.py

@@ -7,7 +7,7 @@
 from rclpy.node import Node
 
 from bitbots_msgs.msg import Workload as WorkloadMsg
-from system_monitor import cpus, memory, network_interfaces
+from system_monitor import cpus, gpu, memory, network_interfaces
 
 
 def main():
@@ -23,31 +23,39 @@ def main():
     # start all names with "SYSTEM" for diagnostic analyzer
     diag_cpu.name = "SYSTEMCPU"
     diag_cpu.hardware_id = "CPU"
+    diag_gpu = DiagnosticStatus()
+    diag_gpu.name = "SYSTEMGPU"
+    diag_gpu.hardware_id = "GPU"
     diag_mem = DiagnosticStatus()
     diag_mem.name = "SYSTEMMemory"
     diag_mem.hardware_id = "Memory"
 
     node.declare_parameter("update_frequency", 10.0)
-    node.declare_parameter("do_memory", True)
     node.declare_parameter("do_cpu", True)
+    node.declare_parameter("do_gpu", True)
+    node.declare_parameter("do_memory", True)
     node.declare_parameter("do_network", True)
     node.declare_parameter("cpu_load_percentage", 80.0)
+    node.declare_parameter("gpu_load_percentage", 80.0)
     node.declare_parameter("memory_load_percentage", 80.0)
     node.declare_parameter("network_rate_received_errors", 10.0)
     node.declare_parameter("network_rate_send_errors", 10.0)
 
     rate = node.get_parameter("update_frequency").get_parameter_value().double_value
-    do_memory = node.get_parameter("do_memory").get_parameter_value().bool_value
     do_cpu = node.get_parameter("do_cpu").get_parameter_value().bool_value
+    do_gpu = node.get_parameter("do_gpu").get_parameter_value().bool_value
+    do_memory = node.get_parameter("do_memory").get_parameter_value().bool_value
     do_network = node.get_parameter("do_network").get_parameter_value().bool_value
     cpu_load_percentage = node.get_parameter("cpu_load_percentage").get_parameter_value().double_value
+    gpu_load_percentage = node.get_parameter("gpu_load_percentage").get_parameter_value().double_value
     memory_load_percentage = node.get_parameter("memory_load_percentage").get_parameter_value().double_value
     network_rate_received_errors = node.get_parameter("network_rate_received_errors").get_parameter_value().double_value
     network_rate_send_errors = node.get_parameter("network_rate_send_errors").get_parameter_value().double_value
 
     while rclpy.ok():
         last_send_time = time.time()
         running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (-1, [], 0)
+        gpu_load, gpu_vram_used, gpu_vram_total, gpu_temperature = gpu.collect_all() if do_gpu else (0, 0, 0, 0)
         memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
         interfaces = network_interfaces.collect_all(node.get_clock()) if do_network else []
 
@@ -56,6 +64,10 @@ def main():
             cpus=cpu_usages,
             running_processes=running_processes,
             cpu_usage_overall=overall_usage_percentage,
+            gpu_load=gpu_load,
+            gpu_vram_used=gpu_vram_used,
+            gpu_vram_total=gpu_vram_total,
+            gpu_temperature=gpu_temperature,
             memory_available=memory_available,
             memory_used=memory_used,
             memory_total=memory_total,
@@ -73,6 +85,14 @@ def main():
             diag_cpu.level = DiagnosticStatus.OK
         diag_array.status.append(diag_cpu)
 
+        gpu_usage = gpu_load * 100
+        diag_gpu.message = str(gpu_usage) + "%"
+        if gpu_usage >= gpu_load_percentage:
+            diag_gpu.level = DiagnosticStatus.WARN
+        else:
+            diag_gpu.level = DiagnosticStatus.OK
+        diag_array.status.append(diag_gpu)
+
         memory_usage = round((memory_used / memory_total) * 100, 2)
         diag_mem.message = str(memory_usage) + "%"
         if memory_usage >= memory_load_percentage: