Merge branch 'main' into feature/non-root-dev-container

.gitignore

@@ -225,3 +225,5 @@ doku/*
 # Protobuf generated file
 /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,6 +54,7 @@
         "joern",
         "jupyter",
         "kalman",
+        "Leph",
         "linalg",
         "matplotlib",
         "mmse",
@@ -77,6 +80,8 @@
         "proto",
         "protos",
         "pyplot",
+        "pywrapper",
+        "Quaterniond",
         "rclcpp",
         "rclpy",
         "reapproach",
@@ -90,6 +95,7 @@
         "rosgraph",
         "rosout",
         "roundrobin",
+        "Rouxel",
         "rtype",
         "rviz",
         "scipy",
@@ -113,12 +119,13 @@
         "walkready",
         "wandb",
         "webots",
+        "Werror",
         "wifi",
         "wolfgang",
         "xacro",
         "yoeo",
         "yolo",
-        "yolov",
+        "yolov"
     ],
     "files.autoSave": "afterDelay",
     "files.autoSaveDelay": 500,
@@ -204,7 +211,8 @@
         "valarray": "cpp",
         "variant": "cpp",
         "regex": "cpp",
-        "future": "cpp"
+        "future": "cpp",
+        "*.ipp": "cpp"
     },
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py

@@ -30,6 +30,8 @@ def __init__(self, node, blackboard):
         self.connect()
 
     def connect(self):
+        # Do not connect if dynamic_kick is disabled
+        return
         topic = self._blackboard.config["dynamic_kick"]["topic"]
         self.__action_client = ActionClient(self._node, Kick, topic, callback_group=ReentrantCallbackGroup())
         self.__connected = self.__action_client.wait_for_server(self._blackboard.config["dynamic_kick"]["wait_time"])
@@ -43,6 +45,7 @@ def kick(self, goal: Kick.Goal):
         :type goal: KickGoal
         :raises RuntimeError: when not connected to dynamic_kick server
         """
+        raise NotImplementedError("The dynamic_kick is disabled currently")
         if not self.__connected:
             # try to connect again
             self.__connected = self.__action_client.wait_for_server(

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -3,7 +3,6 @@
 import numpy as np
 from bitbots_utils.utils import get_parameters_from_other_node
 from geometry_msgs.msg import PointStamped, Pose
-from rclpy.clock import ClockType
 from rclpy.duration import Duration
 from rclpy.time import Time
 from ros2_numpy import numpify
@@ -68,7 +67,6 @@ def __init__(self, node, blackboard):
         # Config
         self.data_timeout: float = self._node.get_parameter("team_data_timeout").value
         self.ball_max_covariance: float = self._node.get_parameter("ball_max_covariance").value
-        self.ball_lost_time: float = Duration(seconds=self._node.get_parameter("ball_lost_time").value)
 
     def is_valid(self, data: TeamData) -> bool:
         """
@@ -205,14 +203,6 @@ def publish_strategy(self):
     def publish_time_to_ball(self):
         self.time_to_ball_publisher.publish(Float32(data=self.own_time_to_ball))
 
-    def get_teammate_ball_seen_time(self) -> Time:
-        """Returns the time at which a teammate has seen the ball accurately enough"""
-        teammate_ball = self.get_teammate_ball()
-        if teammate_ball is not None:
-            return Time.from_msg(teammate_ball.header.stamp)
-        else:
-            return Time(clock_type=ClockType.ROS_TIME)
-
     def teammate_ball_is_valid(self):
         """Returns true if a teammate has seen the ball accurately enough"""
         return self.get_teammate_ball() is not None

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -5,6 +5,7 @@
 import tf2_ros as tf2
 from bitbots_utils.utils import get_parameters_from_other_node
 from geometry_msgs.msg import (
+    Pose,
     PoseStamped,
     PoseWithCovarianceStamped,
     TransformStamped,
@@ -49,15 +50,14 @@ def __init__(self, node, blackboard):
         self.map_frame: str = self._node.get_parameter("map_frame").value
 
         # Get body parameters
-        self.ball_lost_time = Duration(seconds=self._node.get_parameter("ball_lost_time").value)
         self.ball_max_covariance = self._node.get_parameter("ball_max_covariance").value
         self.map_margin: float = self._node.get_parameter("map_margin").value
 
         # Ball state
-        self._ball_seen_time: Time = Time(clock_type=ClockType.ROS_TIME)  # Time when the ball was last seen
         self._ball: PointStamped = PointStamped(
             header=Header(stamp=Time(clock_type=ClockType.ROS_TIME), frame_id=self.map_frame)
         )  # The ball in the map frame (default to the center of the field if ball is not seen yet)
+        self._ball_covariance: np.ndarray = np.zeros((2, 2))  # Covariance of the ball
 
         # Publisher for visualization in RViZ
         self.debug_publisher_used_ball = self._node.create_publisher(PointStamped, "debug/behavior/used_ball", 1)
@@ -71,22 +71,12 @@ def __init__(self, node, blackboard):
     ############
 
     def ball_seen_self(self) -> bool:
-        """Returns true if we have seen the ball recently (less than ball_lost_time ago)"""
-        return self._node.get_clock().now() - self._ball_seen_time < self.ball_lost_time
-
-    def ball_last_seen(self) -> Time:
-        """
-        Returns the time at which the ball was last seen if it is in the threshold or
-        the more recent ball from either the teammate or itself if teamcomm is available
-        """
-        if self.ball_seen_self():
-            return self._ball_seen_time
-        else:
-            return max(self._ball_seen_time, self._blackboard.team_data.get_teammate_ball_seen_time())
+        """Returns true we are reasonably sure that we have seen the ball"""
+        return all(np.diag(self._ball_covariance) < self.ball_max_covariance)
 
     def ball_has_been_seen(self) -> bool:
-        """Returns true if we or a teammate have seen the ball recently (less than ball_lost_time ago)"""
-        return self._node.get_clock().now() - self.ball_last_seen() < self.ball_lost_time
+        """Returns true if we or a teammate are reasonably sure that we have seen the ball"""
+        return self.ball_seen_self() or self._blackboard.team_data.teammate_ball_is_valid()
 
     def get_ball_position_xy(self) -> Tuple[float, float]:
         """Return the ball saved in the map frame, meaning the absolute position of the ball on the field"""
@@ -108,16 +98,17 @@ def get_best_ball_point_stamped(self) -> PointStamped:
             self.debug_publisher_which_ball.publish(Header(stamp=teammate_ball.header.stamp, frame_id="teammate_ball"))
             return teammate_ball
 
-        # Otherwise, use the own ball even if it is too old
+        # Otherwise, use the own ball even if it is bad
         if not self.ball_seen_self():
-            self._node.get_logger().warn("Using own ball even though it is too old, as no teammate ball is available")
+            self._node.get_logger().warn("Using own ball even though it is bad, as no teammate ball is available")
         self.debug_publisher_used_ball.publish(own_ball)
         self.debug_publisher_which_ball.publish(Header(stamp=own_ball.header.stamp, frame_id="own_ball_map"))
         return own_ball
 
     def get_ball_position_uv(self) -> Tuple[float, float]:
         """
-        Returns the ball position relative to the robot in the base_footprint frame
+        Returns the ball position relative to the robot in the base_footprint frame.
+        U and V are returned, where positive U is forward, positive V is to the left.
         """
         ball = self.get_best_ball_point_stamped()
         try:
@@ -131,58 +122,50 @@ def get_ball_position_uv(self) -> Tuple[float, float]:
         return ball_bfp.x, ball_bfp.y
 
     def get_ball_distance(self) -> float:
-        ball_pos = self.get_ball_position_uv()
-        if ball_pos is None:
+        """
+        Returns the distance to the ball in meters.
+        """
+        if not (ball_pos := self.get_ball_position_uv()):
             return np.inf  # worst case (very far away)
-        else:
-            u, v = ball_pos
-
+        u, v = ball_pos
         return math.hypot(u, v)
 
     def get_ball_angle(self) -> float:
-        ball_pos = self.get_ball_position_uv()
-        if ball_pos is None:
+        """
+        Returns the angle to the ball in radians.
+        0 is straight ahead, positive is to the left, negative is to the right.
+        """
+        if not (ball_pos := self.get_ball_position_uv()):
             return -math.pi  # worst case (behind robot)
-        else:
-            u, v = ball_pos
+        u, v = ball_pos
         return math.atan2(v, u)
 
     def ball_filtered_callback(self, msg: PoseWithCovarianceStamped):
-        # When the precision is not sufficient, the ball ages.
-        x_sdev = msg.pose.covariance[0]  # position 0,0 in a 6x6-matrix
-        y_sdev = msg.pose.covariance[7]  # position 1,1 in a 6x6-matrix
-        if x_sdev > self.ball_max_covariance or y_sdev > self.ball_max_covariance:
-            self.forget_ball(reset_ball_filter=False)
-            return
-
-        try:
-            self._ball = self._blackboard.tf_buffer.transform(
-                PointStamped(header=msg.header, point=msg.pose.pose.position),
-                self.map_frame,
-                timeout=Duration(seconds=1.0),
-            )
-            # Set timestamps to zero to get the newest transform when this is transformed later
-            self._ball_seen_time = Time.from_msg(msg.header.stamp)
-            self._ball.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg()
+        """
+        Handles incoming ball messages
+        """
+        assert msg.header.frame_id == self.map_frame, "Ball needs to be in the map frame"
+
+        # Save ball
+        self._ball = PointStamped(
+            header=Header(
+                # Set timestamps to zero to get the newest transform when this is transformed later
+                stamp=Time(clock_type=ClockType.ROS_TIME).to_msg(),
+                frame_id=self.map_frame,
+            ),
+            point=msg.pose.pose.position,
+        )
 
-        except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
-            self._node.get_logger().warn(str(e))
+        # Save covariance (only x and y parts)
+        self._ball_covariance = msg.pose.covariance.reshape((6, 6))[:2, :2]
 
-    def forget_ball(self, reset_ball_filter: bool = True) -> None:
+    def forget_ball(self) -> None:
         """
-        Forget that we saw a ball, optionally reset the ball filter
-        :param reset_ball_filter: Reset the ball filter, defaults to True
-        :type reset_ball_filter: bool, optional
+        Forget that we saw a ball
         """
-        # Forget own ball
-        self._ball_seen_time = Time(clock_type=ClockType.ROS_TIME)
-
-        if reset_ball_filter:  # Reset the ball filter
-            result: Trigger.Response = self.reset_ball_filter.call(Trigger.Request())
-            if result.success:
-                self._node.get_logger().debug(f"Received message from ball filter: '{result.message}'")
-            else:
-                self._node.get_logger().warn(f"Ball filter reset failed with: '{result.message}'")
+        result: Trigger.Response = self.reset_ball_filter.call(Trigger.Request())
+        if not result.success:
+            self._node.get_logger().error(f"Ball filter reset failed with: '{result.message}'")
 
     ########
     # Goal #
@@ -233,8 +216,7 @@ def get_current_position(self) -> Optional[Tuple[float, float, float]]:
         0,0,0 is the center of the field looking in the direction of the opponent goal.
         :returns x,y,theta:
         """
-        transform = self.get_current_position_transform()
-        if transform is None:
+        if not (transform := self.get_current_position_transform()):
             return None
         theta = euler_from_quaternion(numpify(transform.transform.rotation))[2]
         return transform.transform.translation.x, transform.transform.translation.y, theta
@@ -243,17 +225,15 @@ def get_current_position_pose_stamped(self) -> Optional[PoseStamped]:
         """
         Returns the current position as determined by the localization as a PoseStamped
         """
-        transform = self.get_current_position_transform()
-        if transform is None:
+        if not (transform := self.get_current_position_transform()):
             return None
-        ps = PoseStamped()
-        ps.header = transform.header
-        ps.pose.position = msgify(Point, numpify(transform.transform.translation))
-        ps.pose.orientation.x = transform.transform.rotation.x
-        ps.pose.orientation.y = transform.transform.rotation.y
-        ps.pose.orientation.z = transform.transform.rotation.z
-        ps.pose.orientation.w = transform.transform.rotation.w
-        return ps
+        return PoseStamped(
+            header=transform.header,
+            pose=Pose(
+                position=msgify(Point, numpify(transform.transform.translation)),
+                orientation=transform.transform.rotation,
+            ),
+        )
 
     def get_current_position_transform(self) -> TransformStamped:
         """

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/forget_ball.py

@@ -6,5 +6,5 @@ class ForgetBall(AbstractActionElement):
     blackboard: BodyBlackboard
 
     def perform(self, reevaluate=False):
-        self.blackboard.world_model.forget_ball(reset_ball_filter=True)
+        self.blackboard.world_model.forget_ball()
         self.pop()

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py

@@ -10,7 +10,6 @@ class GetWalkready(AbstractActionElement):
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
-        self.direction = "walkready"
         self.first_perform = True
         self.active = False
 
@@ -59,7 +58,7 @@ def start_animation(self) -> bool:
 
         # Dynup action server is running, we can start the walkready action
         goal = Dynup.Goal()
-        goal.direction = self.direction
+        goal.direction = Dynup.Goal.DIRECTION_WALKREADY
         self.active = True
         self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal)
         self.dynup_action_current_goal.add_done_callback(

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py

@@ -9,7 +9,7 @@ class AbstractKickAction(AbstractActionElement):
     blackboard: BodyBlackboard
 
     def pop(self):
-        self.blackboard.world_model.forget_ball(reset_ball_filter=True)
+        self.blackboard.world_model.forget_ball()
         super().pop()
 
 

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_seen.py

@@ -10,13 +10,10 @@ def __init__(self, blackboard, dsd, parameters):
 
     def perform(self, reevaluate=False):
         """
-        Determines whether the ball was seen recently (as defined in config)
+        Determines whether we are confident regarding the ball's position.
         :param reevaluate:
         :return:
         """
-        self.publish_debug_data(
-            "Ball lost time", self.blackboard.node.get_clock().now() - self.blackboard.world_model.ball_last_seen()
-        )
         if self.blackboard.world_model.ball_has_been_seen():
             return "YES"
         return "NO"

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -48,10 +48,6 @@ body_behavior:
     # Time to wait in ready state before moving to role position to give the localization time to converge.
     ready_wait_time: 4.0
 
-    # When the ball has not been seen for `ball_lost_time` seconds,
-    # it is considered lost and will be searched
-    ball_lost_time: 3.0
-
     # The orientation threshold defining which range (in Degrees) is acceptable as aligned to the goal (in each direction)
     goal_alignment_orientation_threshold: 5.0
 

bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml

@@ -37,6 +37,7 @@ wolfgang_hardware_interface:
       auto_torque: true
 
       set_ROM_RAM: true # set the following values on startup to all motors
+      start_delay: 0.5 # delay after the motors are turned on until values are written, in seconds
 
       ROM_RAM_DEFAULT:
         # all names of parameters have to be the same as on the dynamixel table (see dynamixel_workbench_toolbox/src/dynamixel_item.cpp )

bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp

@@ -33,6 +33,8 @@ class WolfgangHardwareInterface {
   std::vector<std::vector<bitbots_ros_control::HardwareInterface *>> interfaces_;
   DynamixelServoHardwareInterface servo_interface_;
   rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
+  std::optional<rclcpp::Time> motor_start_time_;
+  bool motor_first_write_{false};
 
   // prevent unnecessary error when power is turned on
   bool first_ping_error_;