Merge branch 'main' into feature/demo_behavior

.devcontainer/Dockerfile

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

.gitignore

@@ -221,7 +221,9 @@ doku/*
 /lib/
 
 # Path to the protocol buffer definitions, which are a diffrerent repository and managed by vcstool
-/bitbots_team_communication/bitbots_team_communication/RobocupProtocol
+/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/RobocupProtocol
 # Protobuf generated file
-/bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py
+/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py
 
+# Workspace git status file from the deploy tool
+**/workspace_status.json

.vscode/settings.json

@@ -5,6 +5,7 @@
     "cSpell.words": [
         "animatable",
         "ansible",
+        "antiwindup",
         "autoconnect",
         "basler",
         "bitbot",
@@ -14,6 +15,7 @@
         "chrt",
         "coef",
         "colcon",
+        "conv",
         "cornerkick",
         "costmap",
         "costmaps",
@@ -52,9 +54,11 @@
         "joern",
         "jupyter",
         "kalman",
+        "Leph",
         "linalg",
         "matplotlib",
         "mmse",
+        "moveit",
         "msgify",
         "mutex",
         "nanosec",
@@ -71,12 +75,16 @@
         "penaltyshoot",
         "pointcloud",
         "pointclouds",
+        "popen",
         "pretrained",
         "proto",
         "protos",
         "pyplot",
+        "pywrapper",
+        "Quaterniond",
         "rclcpp",
         "rclpy",
+        "reapproach",
         "rhoban",
         "robocup",
         "rory",
@@ -87,28 +95,35 @@
         "rosgraph",
         "rosout",
         "roundrobin",
+        "Rouxel",
         "rtype",
         "rviz",
         "scipy",
+        "sdev",
         "seaborn",
         "segmentations",
+        "srdf",
         "ssid",
         "std_srvs",
         "taskset",
+        "teamcomm",
         "teamplayer",
         "teleop",
         "throwin",
         "timespec",
         "tldr",
+        "torqueless",
         "tqdm",
         "unpenalize",
         "unpenalized",
         "urdf",
         "walkready",
         "wandb",
         "webots",
+        "Werror",
         "wifi",
         "wolfgang",
+        "xacro",
         "yoeo",
         "yolo",
         "yolov"
@@ -197,7 +212,9 @@
         "valarray": "cpp",
         "variant": "cpp",
         "regex": "cpp",
-        "future": "cpp"
+        "future": "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
@@ -216,4 +233,7 @@
     "[xml]": {
         "editor.defaultFormatter": "DotJoshJohnson.xml"
     },
+    "[json]": {
+        "editor.defaultFormatter": "vscode.json-language-features"
+    },
 }

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 \
@@ -70,6 +74,8 @@ fresh-libs: remove-libs setup-libs
 remove-libs:
 	# Removes the lib directory and all its contents
 	rm -rf lib/*
+	# Also remove the generated protobuf files, as they are not needed anymore
+	rm bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py 2> /dev/null || true
 
 setup-libs:
 	# Clone lib repositories in workspace.repos into the lib directory

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/blackboard.py → bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py

@@ -19,18 +19,18 @@ def __init__(self, node: Node, tf_buffer: tf2.Buffer):
         self.tf_buffer = tf_buffer
 
         # Config
-        self.config = get_parameter_dict(node, "body")
+        self.config = get_parameter_dict(self.node, "")
         self.base_footprint_frame: str = self.node.get_parameter("base_footprint_frame").value
         self.map_frame: str = self.node.get_parameter("map_frame").value
         self.odom_frame: str = self.node.get_parameter("odom_frame").value
         self.in_sim: bool = self.node.get_parameter("use_sim_time").value
 
         # Capsules
-        self.misc = MiscCapsule(node)
-        self.gamestate = GameStatusCapsule(node)
-        self.animation = AnimationCapsule(node)
-        self.kick = KickCapsule(self)
-        self.world_model = WorldModelCapsule(self)
-        self.costmap = CostmapCapsule(self)
-        self.pathfinding = PathfindingCapsule(self, node)
-        self.team_data = TeamDataCapsule(node)
+        self.misc = MiscCapsule(self.node, self)
+        self.gamestate = GameStatusCapsule(self.node, self)
+        self.animation = AnimationCapsule(self.node, self)
+        self.kick = KickCapsule(self.node, self)
+        self.world_model = WorldModelCapsule(self.node, self)
+        self.costmap = CostmapCapsule(self.node, self)
+        self.pathfinding = PathfindingCapsule(self.node, self)
+        self.team_data = TeamDataCapsule(self.node, self)

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py

@@ -0,0 +1,14 @@
+from typing import TYPE_CHECKING, Optional
+
+from rclpy.node import Node
+
+if TYPE_CHECKING:
+    from bitbots_blackboard.body_blackboard import BodyBlackboard
+
+
+class AbstractBlackboardCapsule:
+    """Abstract class for blackboard capsules."""
+
+    def __init__(self, node: Node, blackboard: Optional["BodyBlackboard"] = None):
+        self._node = node
+        self._blackboard = blackboard

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py

@@ -1,31 +1,33 @@
 from rclpy.action import ActionClient
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.duration import Duration
-from rclpy.node import Node
 
+from bitbots_blackboard.capsules import AbstractBlackboardCapsule
 from bitbots_msgs.action import Dynup, LookAt, PlayAnimation
 
 
-class AnimationCapsule:
+class AnimationCapsule(AbstractBlackboardCapsule):
     """Communicates with the animation action server to play animations."""
 
-    def __init__(self, node: Node):
-        self.node = node
+    def __init__(self, node, blackboard):
+        super().__init__(node, blackboard)
         self.active = False
 
         # Config
-        self.goalie_arms_animation: str = self.node.get_parameter("Animations.Goalie.goalieArms").value
-        self.goalie_falling_right_animation: str = self.node.get_parameter("Animations.Goalie.fallRight").value
-        self.goalie_falling_left_animation: str = self.node.get_parameter("Animations.Goalie.fallLeft").value
-        self.goalie_falling_center_animation: str = self.node.get_parameter("Animations.Goalie.fallCenter").value
-        self.cheering_animation: str = self.node.get_parameter("Animations.Misc.cheering").value
-        self.init_animation: str = self.node.get_parameter("Animations.Misc.init").value
-
-        self.animation_client = ActionClient(node, PlayAnimation, "animation", callback_group=ReentrantCallbackGroup())
+        self.goalie_arms_animation: str = self._node.get_parameter("Animations.Goalie.goalieArms").value
+        self.goalie_falling_right_animation: str = self._node.get_parameter("Animations.Goalie.fallRight").value
+        self.goalie_falling_left_animation: str = self._node.get_parameter("Animations.Goalie.fallLeft").value
+        self.goalie_falling_center_animation: str = self._node.get_parameter("Animations.Goalie.fallCenter").value
+        self.cheering_animation: str = self._node.get_parameter("Animations.Misc.cheering").value
+        self.init_animation: str = self._node.get_parameter("Animations.Misc.init").value
+
+        self.animation_client = ActionClient(
+            self._node, PlayAnimation, "animation", callback_group=ReentrantCallbackGroup()
+        )
 
-        self.dynup_action_client = ActionClient(node, Dynup, "dynup", callback_group=ReentrantCallbackGroup())
+        self.dynup_action_client = ActionClient(self._node, Dynup, "dynup", callback_group=ReentrantCallbackGroup())
 
-        self.lookat_action_client = ActionClient(node, LookAt, "look_at_goal")
+        self.lookat_action_client = ActionClient(self._node, LookAt, "look_at_goal")
 
     def play_animation(self, animation: str, from_hcm: bool) -> bool:
         """
@@ -39,11 +41,11 @@ def play_animation(self, animation: str, from_hcm: bool) -> bool:
             return False
 
         if animation is None or animation == "":
-            self.node.get_logger().warn("Tried to play an animation with an empty name!")
+            self._node.get_logger().warn("Tried to play an animation with an empty name!")
             return False
 
         if not self.animation_client.wait_for_server(Duration(seconds=10)):
-            self.node.get_logger().error(
+            self._node.get_logger().error(
                 "Animation Action Server not running! Motion can not work without animation action server."
             )
             return False

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -1,8 +1,5 @@
 import math
-from typing import TYPE_CHECKING, List, Optional, Tuple
-
-if TYPE_CHECKING:
-    from bitbots_blackboard.blackboard import BodyBlackboard
+from typing import List, Optional, Tuple
 
 import numpy as np
 import tf2_ros as tf2
@@ -20,30 +17,33 @@
 from tf2_geometry_msgs import PointStamped
 from tf_transformations import euler_from_quaternion, quaternion_from_euler
 
+from bitbots_blackboard.capsules import AbstractBlackboardCapsule
+
 
-class CostmapCapsule:
+class CostmapCapsule(AbstractBlackboardCapsule):
     """Provides information about the cost of different positions and moves."""
 
-    def __init__(self, blackboard: "BodyBlackboard"):
-        self._blackboard = blackboard
-        self._node = blackboard.node
-        self.body_config = get_parameter_dict(self._blackboard.node, "body")
+    def __init__(self, node, blackboard):
+        super().__init__(node, blackboard)
+
+        self.body_config = get_parameter_dict(self._node, "")
 
-        self.map_frame: str = self._blackboard.node.get_parameter("map_frame").value
-        self.base_footprint_frame: str = self._blackboard.node.get_parameter("base_footprint_frame").value
+        self.map_frame: str = self._node.get_parameter("map_frame").value
+        self.base_footprint_frame: str = self._node.get_parameter("base_footprint_frame").value
 
         parameters = get_parameters_from_other_node(
-            self._blackboard.node, "/parameter_blackboard", ["field_length", "field_width", "goal_width"]
+            self._node, "/parameter_blackboard", ["field.size.x", "field.size.y", "field.goal.width"]
         )
-        self.field_length: float = parameters["field_length"]
-        self.field_width: float = parameters["field_width"]
-        self.goal_width: float = parameters["goal_width"]
+        self.field_length: float = parameters["field.size.x"]
+        self.field_width: float = parameters["field.size.y"]
+        self.goal_width: float = parameters["field.goal.width"]
+
         self.map_margin: float = self.body_config["map_margin"]
         self.obstacle_costmap_smoothing_sigma: float = self.body_config["obstacle_costmap_smoothing_sigma"]
         self.obstacle_cost: float = self.body_config["obstacle_cost"]
 
         # Publisher for visualization in RViZ
-        self.costmap_publisher = self._blackboard.node.create_publisher(OccupancyGrid, "debug/costmap", 1)
+        self.costmap_publisher = self._node.create_publisher(OccupancyGrid, "debug/costmap", 1)
 
         self.base_costmap: Optional[np.ndarray] = None  # generated once in constructor field features
         self.costmap: Optional[np.ndarray] = None  # updated on the fly based on the base_costmap
@@ -181,7 +181,7 @@ def cost_at_relative_xy(self, x: float, y: float) -> float:
             # Transform point of interest to the map
             point = self._blackboard.tf_buffer.transform(point, self.map_frame, timeout=Duration(seconds=0.3))
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
-            self._blackboard.node.get_logger().warn(str(e))
+            self._node.get_logger().warn(str(e))
             return 0.0
 
         return self.get_cost_at_field_position(point.point.x, point.point.y)
@@ -236,6 +236,14 @@ def calc_base_costmap(self):
             [
                 ((self.field_length, self.field_width / 2 - self.goal_width / 2), goalpost_value),
                 ((self.field_length, self.field_width / 2 + self.goal_width / 2), goalpost_value),
+                (
+                    (self.field_length + self.map_margin, self.field_width / 2 - self.goal_width / 2),
+                    corner_value + in_field_value_our_side,
+                ),
+                (
+                    (self.field_length + self.map_margin, self.field_width / 2 + self.goal_width / 2),
+                    corner_value + in_field_value_our_side,
+                ),
                 (
                     (self.field_length, self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance),
                     goal_value,
@@ -247,14 +255,14 @@ def calc_base_costmap(self):
                 (
                     (
                         self.field_length + self.map_margin,
-                        self.field_width / 2 - self.goal_width / 2 - goalpost_safety_distance,
+                        self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance,
                     ),
                     -0.2,
                 ),
                 (
                     (
                         self.field_length + self.map_margin,
-                        self.field_width / 2 + self.goal_width / 2 + goalpost_safety_distance,
+                        self.field_width / 2 + self.goal_width / 2 - goalpost_safety_distance,
                     ),
                     -0.2,
                 ),
@@ -334,7 +342,7 @@ def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_l
             pose = self._blackboard.tf_buffer.transform(pose, self.map_frame, timeout=Duration(seconds=0.3))
 
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
-            self._blackboard.node.get_logger().warn(str(e))
+            self._node.get_logger().warn(str(e))
             return 0.0
         d = euler_from_quaternion(numpify(pose.pose.orientation))[2]
         return self.get_cost_of_kick(pose.pose.position.x, pose.pose.position.y, d, kick_length, angular_range)