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

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -11,8 +11,6 @@
     PoseStamped,
     PoseWithCovarianceStamped,
     TransformStamped,
-    TwistStamped,
-    TwistWithCovarianceStamped,
 )
 from rclpy.clock import ClockType
 from rclpy.duration import Duration
@@ -49,14 +47,7 @@ def __init__(self, blackboard: "BodyBlackboard"):
         self.ball_teammate.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg()
         self.ball_teammate.header.frame_id = self.map_frame
         self.ball_lost_time = Duration(seconds=self._blackboard.node.get_parameter("body.ball_lost_time").value)
-        self.ball_twist_map: Optional[TwistStamped] = None
         self.ball_filtered: Optional[PoseWithCovarianceStamped] = None
-        self.ball_twist_lost_time = Duration(
-            seconds=self._blackboard.node.get_parameter("body.ball_twist_lost_time").value
-        )
-        self.ball_twist_precision_threshold = get_parameter_dict(
-            self._blackboard.node, "body.ball_twist_precision_threshold"
-        )
         self.reset_ball_filter = self._blackboard.node.create_client(Trigger, "ball_filter_reset")
 
         self.counter: int = 0
@@ -90,7 +81,6 @@ def __init__(self, blackboard: "BodyBlackboard"):
 
         # Publisher for visualization in RViZ
         self.ball_publisher = self._blackboard.node.create_publisher(PointStamped, "debug/viz_ball", 1)
-        self.ball_twist_publisher = self._blackboard.node.create_publisher(TwistStamped, "debug/ball_twist", 1)
         self.used_ball_pub = self._blackboard.node.create_publisher(PointStamped, "debug/used_ball", 1)
         self.which_ball_pub = self._blackboard.node.create_publisher(Header, "debug/which_ball_is_used", 1)
 
@@ -238,46 +228,6 @@ def ball_filtered_callback(self, msg: PoseWithCovarianceStamped):
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
             self._blackboard.node.get_logger().warn(str(e))
 
-    def recent_ball_twist_available(self) -> bool:
-        if self.ball_twist_map is None:
-            return False
-        return self._blackboard.node.get_clock().now() - self.ball_twist_map.header.stamp < self.ball_twist_lost_time
-
-    def ball_twist_callback(self, msg: TwistWithCovarianceStamped):
-        x_sdev = msg.twist.covariance[0]  # position 0,0 in a 6x6-matrix
-        y_sdev = msg.twist.covariance[7]  # position 1,1 in a 6x6-matrix
-        if (
-            x_sdev > self.ball_twist_precision_threshold["x_sdev"]
-            or y_sdev > self.ball_twist_precision_threshold["y_sdev"]
-        ):
-            return
-        if msg.header.frame_id != self.map_frame:
-            try:
-                # point (0,0,0)
-                point_a = PointStamped()
-                point_a.header = msg.header
-                # linear velocity vector
-                point_b = PointStamped()
-                point_b.header = msg.header
-                point_b.point.x = msg.twist.twist.linear.x
-                point_b.point.y = msg.twist.twist.linear.y
-                point_b.point.z = msg.twist.twist.linear.z
-                # transform start and endpoint of velocity vector
-                point_a = self._blackboard.tf_buffer.transform(point_a, self.map_frame, timeout=Duration(seconds=1.0))
-                point_b = self._blackboard.tf_buffer.transform(point_b, self.map_frame, timeout=Duration(seconds=1.0))
-                # build new twist using transform vector
-                self.ball_twist_map = TwistStamped(header=msg.header)
-                self.ball_twist_map.header.frame_id = self.map_frame
-                self.ball_twist_map.twist.linear.x = point_b.point.x - point_a.point.x
-                self.ball_twist_map.twist.linear.y = point_b.point.y - point_a.point.y
-                self.ball_twist_map.twist.linear.z = point_b.point.z - point_a.point.z
-            except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
-                self._blackboard.node.get_logger().warn(str(e))
-        else:
-            self.ball_twist_map = TwistStamped(header=msg.header, twist=msg.twist.twist)
-        if self.ball_twist_map is not None:
-            self.ball_twist_publisher.publish(self.ball_twist_map)
-
     def forget_ball(self, own: bool = True, team: bool = True, reset_ball_filter: bool = True) -> None:
         """
         Forget that we and the best teammate saw a ball, optionally reset the ball filter

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -5,7 +5,7 @@
 from bitbots_tf_buffer import Buffer
 from dynamic_stack_decider.dsd import DSD
 from game_controller_hl_interfaces.msg import GameState
-from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
@@ -75,13 +75,6 @@ def __init__(self, node: Node):
             qos_profile=1,
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
-        node.create_subscription(
-            TwistWithCovarianceStamped,
-            node.get_parameter("body.ball_movement_subscribe_topic").get_parameter_value().string_value,
-            blackboard.world_model.ball_twist_callback,
-            qos_profile=1,
-            callback_group=MutuallyExclusiveCallbackGroup(),
-        )
         node.create_subscription(
             Twist,
             "cmd_vel",

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -3,7 +3,7 @@
     # Data older than this is seen as non existent
     team_data_timeout: 2
     # minimal confidence to regard a ball of a team mate as valid
-    ball_max_covariance: 0.5
+    ball_max_covariance: 2.0
 
     body:
       roles:
@@ -71,18 +71,10 @@
       # it is in a reachable area of the robot
       ball_close_distance: 1.5
 
-      # the maximal allowed standard deviation of the ball twist.
-      ball_twist_precision_threshold:
-        x_sdev: 0.3
-        y_sdev: 0.3
-
-      # the duration after which a ball_twist is considered irrelevant.
-      ball_twist_lost_time: 2
-
       # the maximal allowed standard deviation of the ball position.
       ball_position_precision_threshold:
-        x_sdev: 0.5
-        y_sdev: 0.5
+        x_sdev: 2.5
+        y_sdev: 2.5
 
       # An area in which the ball can be kicked
       # defined by min/max x/y values in meters which represent ball positions relative to base_footprint
@@ -157,9 +149,6 @@
       # Angle at which the ball is normally approached again
       ball_reapproach_angle: 1.2
 
-      # topics the behavior subscribes to
-      ball_movement_subscribe_topic: 'ball_relative_movement'
-
       # The position where the supporter robot should place itself in order to accept a pass
       pass_position_x: 0.1
       pass_position_y: 1.0

bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py

@@ -12,8 +12,6 @@
     "/audio/audio",
     "/ball_obstacle_active",
     "/ball_position_relative_filtered",
-    "/ball_relative_filtered",
-    "/ball_relative_movement",
     "/balls_relative",
     "/camera/camera_info",
     "/camera/image_to_record",
@@ -23,7 +21,6 @@
     "/cop_r",
     "/core/power_switch_status",
     "/debug/approach_point",
-    "/debug/ball_twist",
     "/debug/dsd/body_behavior/dsd_current_action",
     "/debug/dsd/body_behavior/dsd_stack",
     "/debug/dsd/body_behavior/dsd_tree",

bitbots_misc/bitbots_containers/hlvs/Dockerfile

@@ -42,7 +42,7 @@ RUN sudo rosdep init
 
 # Add some requirements already here so that they are cached
 #RUN python3 -m pip install -U pip && \
-#    pip3 install -U PyYAML construct defusedxml filterpy matplotlib numpy opencv-python \
+#    pip3 install -U PyYAML construct defusedxml matplotlib numpy opencv-python \
 #    protobuf psutil pytorchyolo setuptools sklearn transforms3d
 
 ADD --chown=robot:robot https://raw.githubusercontent.com/bit-bots/bitbots_main/master/requirements/common.txt src/requirements_common.txt

bitbots_msgs/CMakeLists.txt

@@ -33,7 +33,6 @@ rosidl_generate_interfaces(
   "msg/NetworkInterface.msg"
   "msg/PoseWithCertainty.msg"
   "msg/PoseWithCertaintyArray.msg"
-  "msg/PoseWithCertaintyStamped.msg"
   "msg/RobotControlState.msg"
   "msg/RobotRelative.msg"
   "msg/RobotRelativeArray.msg"

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py

@@ -6,9 +6,7 @@
 from nav_msgs.msg import OccupancyGrid
 from rclpy.node import Node
 from ros2_numpy import msgify, numpify
-from tf2_geometry_msgs import PointStamped
-
-from bitbots_msgs.msg import PoseWithCertaintyStamped
+from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
 
 
 class Map:
@@ -33,13 +31,13 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
         self.config_inflation_dialation: int = self.node.declare_parameter("map.inflation.dialte", 3).value
         self.config_obstacle_value: int = self.node.declare_parameter("map.obstacle_value", 50).value
 
-    def set_ball(self, ball: PoseWithCertaintyStamped) -> None:
+    def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         """
         Adds a given ball to the ball buffer
         """
         point = PointStamped()
         point.header.frame_id = ball.header.frame_id
-        point.point = ball.pose.pose.pose.position
+        point.point = ball.pose.pose.position
         try:
             self.ball_buffer = [self.buffer.transform(point, self.frame).point]
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -1,15 +1,14 @@
 import rclpy
 import soccer_vision_3d_msgs.msg as sv3dm
 from bitbots_tf_buffer import Buffer
-from geometry_msgs.msg import PointStamped, PoseStamped, Twist
+from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist
 from nav_msgs.msg import OccupancyGrid, Path
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from std_msgs.msg import Empty
 
-from bitbots_msgs.msg import PoseWithCertaintyStamped
 from bitbots_path_planning.controller import Controller
 from bitbots_path_planning.map import Map
 from bitbots_path_planning.planner import Planner
@@ -37,8 +36,8 @@ def __init__(self) -> None:
 
         # Subscriber
         self.create_subscription(
-            PoseWithCertaintyStamped,
-            self.declare_parameter("map.ball_update_topic", "ball_relative_filtered").value,
+            PoseWithCovarianceStamped,
+            self.declare_parameter("map.ball_update_topic", "ball_position_relative_filtered").value,
             self.map.set_ball,
             5,
             callback_group=MutuallyExclusiveCallbackGroup(),

bitbots_navigation/bitbots_path_planning/config/path_planning.yaml

@@ -9,7 +9,7 @@ bitbots_path_planning:
       size:
         x: 11.0
         y: 8.0
-      ball_update_topic: ball_relative_filtered
+      ball_update_topic: ball_position_relative_filtered
       robot_update_topic: robots_relative_filtered
       ball_diameter: 0.13
       obstacle_value: 50

bitbots_navigation/bitbots_path_planning/package.xml

@@ -7,7 +7,6 @@
   <maintainer email="[email protected]">Florian Vahl</maintainer>
   <license>MIT</license>
 
-  <depend>bitbots_msgs</depend>
   <depend>bitbots_tf_buffer</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>