From 180b8df5a6d8068cd3d7bc8bf302e2e548faf15c Mon Sep 17 00:00:00 2001 From: Valerie Date: Thu, 13 Jun 2024 13:45:31 +0200 Subject: [PATCH 01/15] add is ball in FOV method --- .../bitbots_ball_filter/ball_filter.py | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index ce8f2e43a..f53ff4ba4 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -207,6 +207,45 @@ def filter_step(self) -> None: huge_cov_mat = np.eye(cov_mat.shape[0]) * 10 self.publish_data(state_vec, huge_cov_mat) + def is_ball_in_fov(self): + """ + Calculates if a ball should be currently visible + """ + # Check if we got a camera info to do this stuff + if self.camera_info is None: + self.logger.info("No camera info received. Not checking if the ball is currently visible.") + return False + + # Get ball filter state + state, cov_mat = self.kf.get_update() + + # Build a pose + ball_point = PointStamped() + ball_point.header.frame_id = self.filter_frame + ball_point.header.stamp = self.ball.get_header().stamp + ball_point.point.x = state[0] + ball_point.point.y = state[1] + + # Transform to camera frame + ball_in_camera_optical_frame = self.tf_buffer.transform( + ball_point, self.camera_info.header.frame_id, timeout=Duration(nanoseconds=0.5 * (10**9)) + ) + # Check if the ball is in front of the camera + if ball_in_camera_optical_frame.pose.position.z >= 0: + # Quick math to get the pixels + p = [ + ball_in_camera_optical_frame.pose.position.x, + ball_in_camera_optical_frame.pose.position.y, + ball_in_camera_optical_frame.pose.position.z, + ] + k = np.reshape(self.camera_info.K, (3, 3)) + p_pixel = np.matmul(k, p) + p_pixel = p_pixel * (1 / p_pixel[2]) + # Make sure that the transformed pixel is inside the resolution and positive. + if 0 < p_pixel[0] <= self.camera_info.width and 0 < p_pixel[1] <= self.camera_info.height: + return True + return False + def get_ball_measurement(self) -> Tuple[float, float]: """extracts filter measurement from ball message""" return self.ball.get_position().point.x, self.ball.get_position().point.y From 2b6b755a23e28c5c7426f392d8a9d76c22d10b1b Mon Sep 17 00:00:00 2001 From: Valerie Date: Thu, 13 Jun 2024 13:52:27 +0200 Subject: [PATCH 02/15] add camera info callback --- .../bitbots_ball_filter/ball_filter.py | 12 +++++++++++- .../config/ball_filter_parameters.yaml | 7 +++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index f53ff4ba4..4222f5f69 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -13,6 +13,7 @@ from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node +from sensor_msgs.msg import CameraInfo from soccer_vision_3d_msgs.msg import Ball, BallArray from std_msgs.msg import Header from std_srvs.srv import Trigger @@ -54,6 +55,7 @@ def __init__(self) -> None: self.filter_initialized = False self.ball = None # type: BallWrapper self.last_ball_stamp = None + self.camera_info = None self.update_params() @@ -73,7 +75,7 @@ def __init__(self) -> None: self.ball_publisher = self.create_publisher(PoseWithCertaintyStamped, self.config.ball_publish_topic, 1) # setup subscriber - self.subscriber = self.create_subscription( + self.ball_subscriber = self.create_subscription( BallArray, self.config.ball_subscribe_topic, self.ball_callback, @@ -81,6 +83,10 @@ def __init__(self) -> None: callback_group=MutuallyExclusiveCallbackGroup(), ) + self.camera_info_subscriber = self.create_subscription( + self.config.camera_info_subscribe_topic, CameraInfo, self.camera_info_callback, queue_size=1 + ) + self.reset_service = self.create_service( Trigger, self.config.ball_filter_reset_service_name, @@ -97,6 +103,10 @@ def reset_filter_cb(self, req, response) -> Tuple[bool, str]: response.success = True return response + def camera_info_callback(self, msg: CameraInfo): + """handles incoming ball messages""" + self.camera_info = msg + def ball_callback(self, msg: BallArray) -> None: if msg.balls: # Balls exist # Either select ball closest to previous prediction or with highest confidence diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index c93aba24e..532451508 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -98,3 +98,10 @@ bitbots_ball_filter: default_value: True, description: 'True if ball should be selected based on distance to filtered ball instead of highest rating' } + + camera_info_subscribe_topic: { + type: str, + default_value: 'camera/camera_info' , + description: 'Topic of the camera info', + read_only: True + } From 0a7ab5ccc2dfef2c0e5fba1e1ddab2e324f3dd3b Mon Sep 17 00:00:00 2001 From: Valerie Date: Thu, 13 Jun 2024 14:11:57 +0200 Subject: [PATCH 03/15] make process noise high, when ball is not seen --- .../bitbots_ball_filter/ball_filter.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 4222f5f69..caa830548 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -119,6 +119,20 @@ def ball_callback(self, msg: BallArray) -> None: position = self._get_transform(msg.header, ball_msg.center) if position is not None: self.ball = BallWrapper(position, msg.header, ball_msg.confidence.confidence) + # If we do not see a ball, but the KF predicts the ball to be there: + else: + # Check if we should be able to see the ball + if self.is_ball_in_fov(msg.header): + self.logger.info( + "The KF prediction expects a ball, but we see none... Process noise will be increased." + ) + self.kf.Q = Q_discrete_white_noise( + dim=2, + dt=self.filter_time_step, + var=self.config.process_noise_variance * 10, + block_size=2, + order_by_dim=False, + ) def _get_closest_ball_to_previous_prediction(self, ball_array: BallArray) -> Union[Ball, None]: closest_distance = math.inf @@ -217,7 +231,7 @@ def filter_step(self) -> None: huge_cov_mat = np.eye(cov_mat.shape[0]) * 10 self.publish_data(state_vec, huge_cov_mat) - def is_ball_in_fov(self): + def is_ball_in_fov(self, header: Header) -> bool: """ Calculates if a ball should be currently visible """ @@ -232,7 +246,7 @@ def is_ball_in_fov(self): # Build a pose ball_point = PointStamped() ball_point.header.frame_id = self.filter_frame - ball_point.header.stamp = self.ball.get_header().stamp + ball_point.header.stamp = header.stamp ball_point.point.x = state[0] ball_point.point.y = state[1] From 3f28902859e3324a1fcf02c3464862f4068d0273 Mon Sep 17 00:00:00 2001 From: Valerie Date: Thu, 13 Jun 2024 14:12:43 +0200 Subject: [PATCH 04/15] fix type in yaml --- .../bitbots_ball_filter/config/ball_filter_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index 532451508..b15c4ccce 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -100,7 +100,7 @@ bitbots_ball_filter: } camera_info_subscribe_topic: { - type: str, + type: string, default_value: 'camera/camera_info' , description: 'Topic of the camera info', read_only: True From e8cd742f561ea601db8911f6443efe6a15632099 Mon Sep 17 00:00:00 2001 From: Valerie Date: Thu, 13 Jun 2024 14:37:46 +0200 Subject: [PATCH 05/15] small fixes --- .../bitbots_ball_filter/ball_filter.py | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index caa830548..1c495668c 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -17,7 +17,7 @@ from soccer_vision_3d_msgs.msg import Ball, BallArray from std_msgs.msg import Header from std_srvs.srv import Trigger -from tf2_geometry_msgs import PointStamped +from tf2_geometry_msgs import PointStamped, PoseStamped from bitbots_ball_filter.ball_filter_parameters import bitbots_ball_filter as parameters from bitbots_msgs.msg import PoseWithCertaintyStamped @@ -84,7 +84,7 @@ def __init__(self) -> None: ) self.camera_info_subscriber = self.create_subscription( - self.config.camera_info_subscribe_topic, CameraInfo, self.camera_info_callback, queue_size=1 + CameraInfo, self.config.camera_info_subscribe_topic, self.camera_info_callback, 1 ) self.reset_service = self.create_service( @@ -244,16 +244,21 @@ def is_ball_in_fov(self, header: Header) -> bool: state, cov_mat = self.kf.get_update() # Build a pose - ball_point = PointStamped() - ball_point.header.frame_id = self.filter_frame - ball_point.header.stamp = header.stamp - ball_point.point.x = state[0] - ball_point.point.y = state[1] + ball_pose = PoseStamped() + ball_pose.header.frame_id = self.config.filter_frame + ball_pose.header.stamp = header.stamp + ball_pose.pose.position.x = state[0] + ball_pose.pose.position.y = state[1] # Transform to camera frame - ball_in_camera_optical_frame = self.tf_buffer.transform( - ball_point, self.camera_info.header.frame_id, timeout=Duration(nanoseconds=0.5 * (10**9)) - ) + try: + ball_in_camera_optical_frame = self.tf_buffer.transform( + ball_pose, self.camera_info.header.frame_id, timeout=Duration(nanoseconds=0.5 * (10**9)) + ) + except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: + self.logger.warning(str(e)) + return False + # Check if the ball is in front of the camera if ball_in_camera_optical_frame.pose.position.z >= 0: # Quick math to get the pixels @@ -262,7 +267,7 @@ def is_ball_in_fov(self, header: Header) -> bool: ball_in_camera_optical_frame.pose.position.y, ball_in_camera_optical_frame.pose.position.z, ] - k = np.reshape(self.camera_info.K, (3, 3)) + k = np.reshape(self.camera_info.k, (3, 3)) p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) # Make sure that the transformed pixel is inside the resolution and positive. From ad69a72363effed699b942e3f71335da8f2847b2 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 16 Jun 2024 23:05:25 +0200 Subject: [PATCH 06/15] Fix wrong return --- .../bitbots_ball_filter/bitbots_ball_filter/ball_filter.py | 1 - 1 file changed, 1 deletion(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index a468ae6e3..4b4b63bb3 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -297,7 +297,6 @@ def filter_step(self) -> None: self.kf = None self.reset_requested = False self.logger.info("Filter reset") - return # Early exit if filter is not initialized if self.kf is None: From cf63c2a4f34e9e4a714a4648996519692b7c16ac Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 16 Jun 2024 23:39:03 +0200 Subject: [PATCH 07/15] Handle images with a ball that is visible but filtered out --- .../bitbots_ball_filter/ball_filter.py | 51 ++++++++++--------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 4b4b63bb3..33cecaf98 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -122,6 +122,12 @@ def camera_info_callback(self, msg: CameraInfo): self.camera_info = msg def ball_callback(self, msg: BallArray) -> None: + # Keep track if we have updated the measurement + # We might not have a measurement if the ball is not visible + # We also filter out balls that are too far away from the filter's estimate + ball_measurement_updated: bool = False + + # Do filtering, transform, ... if we have a ball if msg.balls: # Balls exist # If we have a kalman filter, we use it's estimate to ignore balls that are too far away (false positives) # The ignore distance is calculated using the filter's covariance and a factor @@ -130,10 +136,10 @@ def ball_callback(self, msg: BallArray) -> None: ignore_threshold_x = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold ignore_threshold_y = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold - # Filter out balls that are too far away from the filter's estimate + # Array without out balls that are too far away from the filter's estimate filtered_balls: list[ tuple[Ball, PointStamped, float] - ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate + ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame, distance to filter estimate ball: Ball for ball in msg.balls: ball_transform = self._get_transform(msg.header, ball.center) @@ -155,6 +161,7 @@ def ball_callback(self, msg: BallArray) -> None: self.last_ball_measurement = BallWrapper( ball_measurement_map.point, ball_measurement_map.header, ball_msg.confidence.confidence ) + ball_measurement_updated = True # Initialize filter if not already done # We do this here, because we need the ball measurement to initialize the filter @@ -165,27 +172,21 @@ def ball_callback(self, msg: BallArray) -> None: assert msg.header.frame_id == "base_footprint", "Ball frame_id is not 'base_footprint'!" robot_ball_delta = math.hypot(ball_msg.center.x, ball_msg.center.y) self.update_measurement_noise(robot_ball_delta) - # Reset process noise if ball is not visible - self.kf.Q = Q_discrete_white_noise( - dim=2, dt=self.filter_time_step, var=self.config.process_noise_variance, block_size=2, order_by_dim=False - ) - else: # TODO handle if ball is visible but not matched - if self.is_estimate_in_fov(msg.header): - self.logger.info( - "The KF prediction expects a ball, but we see none... Process noise will be increased." - ) - self.kf.Q = Q_discrete_white_noise( - dim=2, - dt=self.filter_time_step, - var=self.config.process_noise_variance * 10, - block_size=2, - order_by_dim=False, - ) - else: - # Reset process noise - self.kf.Q = Q_discrete_white_noise( - dim=2, dt=self.filter_time_step, var=self.config.process_noise_variance, block_size=2, order_by_dim=False - ) + + # If we made no measurement, we can check if we expected one and increase process noise accordingly + if not ball_measurement_updated and self.is_estimate_in_fov(msg.header): + self.logger.info("The KF prediction expects a ball, but we see none... Process noise will be increased.") + self.set_process_noise_level(high=True) + else: + self.set_process_noise_level(high=False) + + def set_process_noise_level(self, high: bool) -> None: + process_noise_variance = self.config.process_noise_variance + if high: + process_noise_variance *= 10 + self.kf.Q = Q_discrete_white_noise( + dim=2, dt=self.filter_time_step, var=process_noise_variance, block_size=2, order_by_dim=False + ) def _get_closest_ball_to_previous_prediction(self, ball_array: BallArray) -> Optional[Ball]: closest_distance = math.inf @@ -233,7 +234,6 @@ def update_params(self) -> None: if self.kf: self.setup_filter() - def is_estimate_in_fov(self, header: Header) -> bool: """ Calculates if a ball should be currently visible @@ -244,7 +244,7 @@ def is_estimate_in_fov(self, header: Header) -> bool: return False # Get ball filter state - state, cov_mat = self.kf.get_update() + state, _ = self.kf.get_update() # Build a pose ball_pose = PoseStamped() @@ -274,6 +274,7 @@ def is_estimate_in_fov(self, header: Header) -> bool: p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) # Make sure that the transformed pixel is inside the resolution and positive. + # TODO border if 0 < p_pixel[0] <= self.camera_info.width and 0 < p_pixel[1] <= self.camera_info.height: return True return False From fc4f7d4a545b1c2d58e36e42b0adea810f1b62ca Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 16 Jun 2024 23:43:35 +0200 Subject: [PATCH 08/15] Remove duplicate key --- .../bitbots_ball_filter/config/ball_filter_parameters.yaml | 7 ------- 1 file changed, 7 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index 1d0a4a809..4fb6eebb1 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -79,13 +79,6 @@ bitbots_ball_filter: validation: bounds<>: [0.0, 1.0] - filter_reset_time: - type: int - default_value: 20 - description: 'Max ball not seen time in Seconds' - validation: - bounds<>: [0, 100] - camera_info_subscribe_topic: type: string default_value: 'camera/camera_info' From e6f89d9dcb9f414f9e0a7217b2228444951b75f9 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Mon, 17 Jun 2024 02:19:15 +0200 Subject: [PATCH 09/15] Directly modify covariance instead of process noise --- .../config/body_behavior.yaml | 4 ++-- .../bitbots_ball_filter/ball_filter.py | 19 +++++++------------ .../config/ball_filter_parameters.yaml | 2 +- 3 files changed, 10 insertions(+), 15 deletions(-) diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 4a186362e..c0b1a55ba 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -81,8 +81,8 @@ # the maximal allowed standard deviation of the ball position. ball_position_precision_threshold: - x_sdev: 0.5 - y_sdev: 0.5 + x_sdev: 1.0 + y_sdev: 1.0 # 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 diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 33cecaf98..70c25ffc5 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -176,17 +176,8 @@ def ball_callback(self, msg: BallArray) -> None: # If we made no measurement, we can check if we expected one and increase process noise accordingly if not ball_measurement_updated and self.is_estimate_in_fov(msg.header): self.logger.info("The KF prediction expects a ball, but we see none... Process noise will be increased.") - self.set_process_noise_level(high=True) - else: - self.set_process_noise_level(high=False) - - def set_process_noise_level(self, high: bool) -> None: - process_noise_variance = self.config.process_noise_variance - if high: - process_noise_variance *= 10 - self.kf.Q = Q_discrete_white_noise( - dim=2, dt=self.filter_time_step, var=process_noise_variance, block_size=2, order_by_dim=False - ) + if self.kf is not None: + self.kf.P += np.eye(4) * 0.0001 def _get_closest_ball_to_previous_prediction(self, ball_array: BallArray) -> Optional[Ball]: closest_distance = math.inf @@ -243,6 +234,10 @@ def is_estimate_in_fov(self, header: Header) -> bool: self.logger.info("No camera info received. Not checking if the ball is currently visible.") return False + # Check if we have an estimate + if self.kf is None: + return False + # Get ball filter state state, _ = self.kf.get_update() @@ -340,7 +335,7 @@ def init_filter(self, x: float, y: float) -> None: self.kf.x = np.array([x, y, 0, 0]) # multiplying by the initial uncertainty - self.kf.P = np.eye(4) * 1000 + self.kf.P = np.eye(4) * 0.1 # setup the other matrices, that can also be updated without reinitializing the filter self.setup_filter() diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index 4fb6eebb1..e8e641c38 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -53,7 +53,7 @@ bitbots_ball_filter: process_noise_variance: type: double - default_value: 0.001 + default_value: 0.0002 description: 'Noise which is added to the estimated position without new measurements' validation: bounds<>: [0.0, 1.0] From 926f8eb7d48bf7b4912455ab3869cbf1a83e8144 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 18 Jun 2024 17:48:23 +0200 Subject: [PATCH 10/15] Make ball filter no kalman filter --- .../capsules/world_model_capsule.py | 50 --- .../bitbots_body_behavior/body_behavior.py | 9 +- .../config/body_behavior.yaml | 11 - .../launch/rosbag_record.launch.py | 3 - bitbots_msgs/CMakeLists.txt | 1 - bitbots_msgs/msg/PoseWithCertaintyStamped.msg | 3 - .../bitbots_path_planning/map.py | 8 +- .../bitbots_path_planning/path_planning.py | 7 +- .../config/path_planning.yaml | 2 +- .../bitbots_path_planning/package.xml | 1 - .../bitbots_ball_filter/README.md | 6 - .../bitbots_ball_filter/ball_filter.py | 284 ++++-------------- .../config/ball_filter_parameters.yaml | 33 -- .../bitbots_ball_filter/package.xml | 1 - 14 files changed, 64 insertions(+), 355 deletions(-) delete mode 100644 bitbots_msgs/msg/PoseWithCertaintyStamped.msg delete mode 100644 bitbots_world_model/bitbots_ball_filter/README.md diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py index 42b07cda8..482d58426 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py +++ b/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 diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py index 85e332ee9..e8c77d3b8 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py +++ b/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", diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 4a186362e..e8e6b8c1f 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -71,14 +71,6 @@ # 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 @@ -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 diff --git a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py b/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py index 034b6276f..9b4cd2445 100644 --- a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py +++ b/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", diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt index 2488d45d1..61884419f 100644 --- a/bitbots_msgs/CMakeLists.txt +++ b/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" diff --git a/bitbots_msgs/msg/PoseWithCertaintyStamped.msg b/bitbots_msgs/msg/PoseWithCertaintyStamped.msg deleted file mode 100644 index 2b4eea88f..000000000 --- a/bitbots_msgs/msg/PoseWithCertaintyStamped.msg +++ /dev/null @@ -1,3 +0,0 @@ -# This expresses an estimated pose detection with a reference coordinate frame and timestamp -std_msgs/Header header -bitbots_msgs/PoseWithCertainty pose diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py index ab151d8b1..6a2b225d1 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py +++ b/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: diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py index 1d56d091b..214a48f17 100644 --- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py +++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py @@ -1,7 +1,7 @@ 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 @@ -9,7 +9,6 @@ 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(), diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml index 980c68dc3..16234b436 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml +++ b/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 diff --git a/bitbots_navigation/bitbots_path_planning/package.xml b/bitbots_navigation/bitbots_path_planning/package.xml index 6c06f70c1..88c5f04e2 100644 --- a/bitbots_navigation/bitbots_path_planning/package.xml +++ b/bitbots_navigation/bitbots_path_planning/package.xml @@ -7,7 +7,6 @@ Florian Vahl MIT - bitbots_msgs bitbots_tf_buffer geometry_msgs nav_msgs diff --git a/bitbots_world_model/bitbots_ball_filter/README.md b/bitbots_world_model/bitbots_ball_filter/README.md deleted file mode 100644 index 32d43d848..000000000 --- a/bitbots_world_model/bitbots_ball_filter/README.md +++ /dev/null @@ -1,6 +0,0 @@ -# bitbots_ball_filter - -ball_filter uses `filterpy` to create a kalmanfilter -to reduce noise when getting the -ball position from a ros node. It publishes the -estimated position and velocity. \ No newline at end of file diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 95806e683..bed463275 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -1,57 +1,33 @@ #! /usr/bin/env python3 -import math from typing import Optional, Tuple import numpy as np import rclpy import tf2_ros as tf2 from bitbots_tf_buffer import Buffer -from filterpy.common import Q_discrete_white_noise -from filterpy.kalman import KalmanFilter -from geometry_msgs.msg import Point, PoseWithCovarianceStamped, TwistWithCovarianceStamped +from geometry_msgs.msg import Point, PoseWithCovarianceStamped from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node +from rclpy.time import Time +from ros2_numpy import msgify, numpify from soccer_vision_3d_msgs.msg import Ball, BallArray from std_msgs.msg import Header from std_srvs.srv import Trigger from tf2_geometry_msgs import PointStamped from bitbots_ball_filter.ball_filter_parameters import bitbots_ball_filter as parameters -from bitbots_msgs.msg import PoseWithCertaintyStamped - - -class BallWrapper: - def __init__(self, position: Point, header: Header, confidence: float): - self.position = position - self.header = header - self.confidence = confidence - self.processed = False - - def flag_processed(self) -> None: - self.processed = True - - def get_processed(self) -> bool: - return self.processed - - def get_header(self) -> Header: - return self.header - - def get_position(self) -> Point: - return self.position - - def get_position_tuple(self) -> Tuple[float, float]: - return self.position.x, self.position.y - - def get_confidence(self) -> float: - return self.confidence class BallFilter(Node): + ball_state_position: np.ndarray + ball_state_covariance: np.ndarray + config: parameters.Params + def __init__(self) -> None: """ - creates Kalmanfilter and subscribes to messages which are needed + creates filter and subscribes to messages which are needed """ super().__init__("ball_filter") self.logger = self.get_logger() @@ -59,31 +35,22 @@ def __init__(self) -> None: # Setup dynamic reconfigure config self.param_listener = parameters.ParamListener(self) - self.kf: Optional[KalmanFilter] = None - self.last_ball_measurement: Optional[BallWrapper] = None - self.reset_requested = False - + # Initialize parameters self.update_params() - self.logger.info(f"Using frame '{self.config.filter_frame}' for ball filtering") + # Initialize state + self.reset_ball() + # publishes positions of ball self.ball_pose_publisher = self.create_publisher( PoseWithCovarianceStamped, self.config.ball_position_publish_topic, 1 ) - # publishes velocity of ball - self.ball_movement_publisher = self.create_publisher( - TwistWithCovarianceStamped, self.config.ball_movement_publish_topic, 1 - ) - - # publishes ball - self.ball_publisher = self.create_publisher(PoseWithCertaintyStamped, self.config.ball_publish_topic, 1) - # Create callback group self.callback_group = MutuallyExclusiveCallbackGroup() - # setup subscriber + # Setup subscriber self.subscriber = self.create_subscription( BallArray, self.config.ball_subscribe_topic, @@ -102,38 +69,40 @@ def __init__(self) -> None: self.filter_timer = self.create_timer( self.filter_time_step, self.filter_step, callback_group=self.callback_group ) - self.logger.info("Ball filter initialized") + + def reset_ball(self) -> None: + self.ball_state_position = np.zeros(3) + self.ball_state_covariance = np.eye(3) * 1000 def reset_filter_cb(self, req, response) -> Tuple[bool, str]: self.logger.info("Resetting bitbots ball filter...") - # Reset filter - self.reset_requested = True + self.reset_ball() response.success = True return response + def get_position_numpy(self) -> np.ndarray: + return np.array([*self.get_position_tuple()]) + def ball_callback(self, msg: BallArray) -> None: if msg.balls: # Balls exist - # If we have a kalman filter, we use it's estimate to ignore balls that are too far away (false positives) + # Ignore balls that are too far away (false positives) # The ignore distance is calculated using the filter's covariance and a factor # This way false positives are ignored if we already have a good estimate - if self.kf is not None: - ignore_threshold_x = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold - ignore_threshold_y = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold - - # Filter out balls that are too far away from the filter's estimate - filtered_balls: list[ - tuple[Ball, PointStamped, float] - ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate - ball: Ball - for ball in msg.balls: - ball_transform = self._get_transform(msg.header, ball.center) - if ball_transform: - distance_x = ball_transform.point.x - self.kf.x[0] - distance_y = ball_transform.point.y - self.kf.x[1] - if abs(distance_x) < ignore_threshold_x and abs(distance_y) < ignore_threshold_y: - filtered_balls.append((ball, ball_transform, math.hypot(distance_x, distance_y))) - else: - filtered_balls = [(ball, self._get_transform(msg.header, ball.center), 0) for ball in msg.balls] + ignore_threshold_x, ignore_threshold_y, _ = ( + np.sqrt(np.diag(self.ball_state_covariance)) * self.config.ignore_measurement_threshold + ) + + # Filter out balls that are too far away from the filter's estimate + filtered_balls: list[ + tuple[Ball, PointStamped, float] + ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate + ball: Ball + for ball in msg.balls: + ball_transform = self._get_transform(msg.header, ball.center) + if ball_transform: + diff = numpify(ball_transform.point) - self.get_position_numpy() + if abs(diff[0]) < ignore_threshold_x and abs(diff[0]) < ignore_threshold_y: + filtered_balls.append((ball, ball_transform, np.linalg.norm(diff))) # Select the ball with closest distance to the filter estimate # Return if no ball was found @@ -141,45 +110,17 @@ def ball_callback(self, msg: BallArray) -> None: if ball_measurement_map is None: return - # Store the ball measurement - self.last_ball_measurement = BallWrapper( - ball_measurement_map.point, ball_measurement_map.header, ball_msg.confidence.confidence + # Estimate the covariance of the measurement + # Calculate the distance from the robot to the ball + distance = np.linalg.norm(numpify(ball_msg.center)) + covariance = np.eye(3) * ( + self.config.measurement_certainty + (distance**2) * self.config.noise_increment_factor ) + covariance[2, 2] = 0.0 # Ignore z-axis - # Initialize filter if not already done - # We do this here, because we need the ball measurement to initialize the filter - if self.kf is None: - self.init_filter(*self.last_ball_measurement.get_position_tuple()) - - # Reset filter, if distance between the filter state and the measurement is too large - distance_measurement_prediction = math.dist( - (self.kf.get_update()[0][0], self.kf.get_update()[0][1]), - self.last_ball_measurement.get_position_tuple(), - ) - if distance_measurement_prediction > self.config.filter_reset_distance: - self.init_filter(*self.last_ball_measurement.get_position_tuple()) - self.logger.info( - f"Reset filter! Reason: Distance to ball {distance_measurement_prediction} > {self.config.filter_reset_distance} (filter_reset_distance)" - ) - - # Calculate distance from the robot to the ball and update measurement noise - assert msg.header.frame_id == "base_footprint", "Ball frame_id is not 'base_footprint'!" - robot_ball_delta = math.hypot(ball_msg.center.x, ball_msg.center.y) - self.update_measurement_noise(robot_ball_delta) - - def _get_closest_ball_to_previous_prediction(self, ball_array: BallArray) -> Optional[Ball]: - closest_distance = math.inf - closest_ball_msg = ball_array.balls[0] - for ball_msg in ball_array.balls: - ball_transform = self._get_transform(ball_array.header, ball_msg.center) - if ball_transform and self.last_ball_measurement: - distance = math.dist( - (ball_transform.point.x, ball_transform.point.y), - self.last_ball_measurement.get_position_tuple(), - ) - if distance < closest_distance: - closest_ball_msg = ball_msg - return closest_ball_msg + # Store the ball measurement + self.ball_state_position = numpify(ball_measurement_map.point) + self.ball_state_covariance = covariance def _get_transform( self, header: Header, point: Point, frame: Optional[str] = None, timeout: float = 0.3 @@ -200,9 +141,6 @@ def _get_transform( except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.logger.warning(str(e)) - def update_measurement_noise(self, distance: float) -> None: - self.kf.R = np.eye(2) * (self.config.measurement_certainty + self.config.noise_increment_factor * distance**2) - def update_params(self) -> None: """ Updates parameters from dynamic reconfigure @@ -210,8 +148,6 @@ def update_params(self) -> None: self.config = self.param_listener.get_params() self.filter_time_step = 1.0 / self.config.filter_rate self.filter_reset_duration = Duration(seconds=self.config.filter_reset_time) - if self.kf: - self.setup_filter() def filter_step(self) -> None: """ @@ -227,129 +163,21 @@ def filter_step(self) -> None: self.param_listener.refresh_dynamic_parameters() self.update_params() - # Reset filter if requested - if self.reset_requested: - self.kf = None - self.reset_requested = False - self.logger.info("Filter reset") - - # Early exit if filter is not initialized - if self.kf is None: - huge_cov_mat = np.eye(4) * 100 - self.publish_data(np.zeros((4,)), huge_cov_mat) - return - - # Reset filer,if last measurement is too old - age = self.get_clock().now() - rclpy.time.Time.from_msg(self.last_ball_measurement.get_header().stamp) - if age > self.filter_reset_duration: - self.kf = None - self.logger.info( - f"Reset filter! Reason: Latest ball is too old {age} > {self.filter_reset_duration} (filter_reset_duration)" - ) - return - - # Predict next state - self.kf.predict() - # Update filter with new measurement if available - if not self.last_ball_measurement.get_processed(): - self.last_ball_measurement.flag_processed() - self.kf.update(self.last_ball_measurement.get_position_tuple()) - else: - self.kf.update(None) - self.publish_data(*self.kf.get_update()) - - def init_filter(self, x: float, y: float) -> None: - """ - Initializes kalman filter at given position - - :param x: start x position of the ball - :param y: start y position of the ball - """ - self.logger.info(f"Initializing filter at position ({x}, {y})") - - # Create Kalman filter - self.kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=0) - - # initial value of position(x,y) of the ball and velocity - self.kf.x = np.array([x, y, 0, 0]) - - # multiplying by the initial uncertainty - self.kf.P = np.eye(4) * 1000 - - # setup the other matrices, that can also be updated without reinitializing the filter - self.setup_filter() - - def setup_filter(self) -> None: - """ - Sets up the kalman filter with - the different matrices - """ - # transition matrix - # The velocity factor is used to reduce the velocity of the ball over time - velocity_factor = (1 - self.config.velocity_reduction) ** (self.filter_time_step) - self.kf.F = np.array( - [ - [1.0, 0.0, 1.0, 0.0], - [0.0, 1.0, 0.0, 1.0], - [0.0, 0.0, velocity_factor, 0.0], - [0.0, 0.0, 0.0, velocity_factor], - ] - ) - # measurement function - self.kf.H = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]]) - - # assigning measurement noise - self.kf.R = np.eye(2) * self.config.measurement_certainty - - # assigning process noise - self.kf.Q = Q_discrete_white_noise( - dim=2, dt=self.filter_time_step, var=self.config.process_noise_variance, block_size=2, order_by_dim=False - ) - - def publish_data(self, state_vec: np.array, cov_mat: np.array) -> None: - """ - Publishes ball position and velocity to ros nodes - :param state_vec: current state of kalmanfilter - :param cov_mat: current covariance matrix - """ - header = Header() - header.frame_id = self.config.filter_frame - header.stamp = rclpy.time.Time.to_msg(self.get_clock().now()) - - # position - point_msg = Point(x=float(state_vec[0]), y=float(state_vec[1])) - - # covariance - pos_covariance = np.zeros((6, 6)) - pos_covariance[:2, :2] = cov_mat[:2, :2] + # Increase covariance + self.ball_state_covariance += np.eye(2) * self.config.process_noise_variance + # Pose pose_msg = PoseWithCovarianceStamped() - pose_msg.header = header - pose_msg.pose.pose.position = point_msg - pose_msg.pose.covariance = pos_covariance.reshape(-1) + pose_msg.header = Header( + stamp=Time.to_msg(self.get_clock().now()), + frame_id=self.config.filter_frame, + ) + pose_msg.pose.pose.position = msgify(Point, self.ball_state_position) + pose_msg.pose.covariance = np.zeros((6, 6)) + pose_msg.pose.covariance[:3, :3] = self.ball_state_covariance pose_msg.pose.pose.orientation.w = 1.0 self.ball_pose_publisher.publish(pose_msg) - # velocity - movement_msg = TwistWithCovarianceStamped() - movement_msg.header = header - movement_msg.twist.twist.linear.x = float(state_vec[2] * self.config.filter_rate) - movement_msg.twist.twist.linear.y = float(state_vec[3] * self.config.filter_rate) - vel_covariance = np.eye(6) - vel_covariance[:2, :2] = cov_mat[2:, 2:] - movement_msg.twist.covariance = vel_covariance.reshape(-1) - self.ball_movement_publisher.publish(movement_msg) - - # ball - ball_msg = PoseWithCertaintyStamped() - ball_msg.header = header - ball_msg.pose.pose.pose.position = point_msg - ball_msg.pose.pose.covariance = pos_covariance.reshape(-1) - ball_msg.pose.confidence = float( - self.last_ball_measurement.get_confidence() if self.last_ball_measurement else 0.0 - ) - self.ball_publisher.publish(ball_msg) - def main(args=None) -> None: rclpy.init(args=args) diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index d93c3d07a..d8ba038f3 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -11,18 +11,6 @@ bitbots_ball_filter: read_only: true description: 'Topic to publish the filtered ball position' - ball_movement_publish_topic: - type: string - default_value: 'ball_relative_movement' - read_only: true - description: 'Topic to publish the filtered ball movement' - - ball_publish_topic: - type: string - default_value: 'ball_relative_filtered' - read_only: true - description: 'Topic to publish the filtered ball' - ball_filter_reset_service_name: type: string default_value: 'ball_filter_reset' @@ -43,13 +31,6 @@ bitbots_ball_filter: read_only: true validation: bounds<>: [0, 100] - - velocity_reduction: - type: double - default_value: 0.4 - description: 'Velocity reduction (per axis) factor of the ball per second' - validation: - bounds<>: [0.0, 1.0] process_noise_variance: type: double @@ -64,20 +45,6 @@ bitbots_ball_filter: description: 'Ball measurement certainty in filter' validation: bounds<>: [0.0, 1.0] - - filter_reset_time: - type: int - default_value: 20 - description: 'Max ball not seen time in Seconds' - validation: - bounds<>: [0, 100] - - filter_reset_distance: - type: int - default_value: 99 - description: 'Distance to the current estimation causing a filter reset' - validation: - bounds<>: [0, 100] noise_increment_factor: type: double diff --git a/bitbots_world_model/bitbots_ball_filter/package.xml b/bitbots_world_model/bitbots_ball_filter/package.xml index f864ed94c..d67b20f9c 100644 --- a/bitbots_world_model/bitbots_ball_filter/package.xml +++ b/bitbots_world_model/bitbots_ball_filter/package.xml @@ -16,7 +16,6 @@ rosidl_default_generators rosidl_default_runtime - bitbots_msgs bitbots_tf_buffer geometry_msgs rclpy From 79000c384f710204828525e8c471a61551f69255 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 18 Jun 2024 18:44:28 +0200 Subject: [PATCH 11/15] Fix bugs --- .../bitbots_ball_filter/ball_filter.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 63f82972f..8f5a8a28b 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -91,9 +91,6 @@ def camera_info_callback(self, msg: CameraInfo): """handles incoming ball messages""" self.camera_info = msg - def get_position_numpy(self) -> np.ndarray: - return np.array([*self.get_position_tuple()]) - def ball_callback(self, msg: BallArray) -> None: # Keep track if we have updated the measurement # We might not have a measurement if the ball is not visible @@ -117,7 +114,7 @@ def ball_callback(self, msg: BallArray) -> None: for ball in msg.balls: ball_transform = self._get_transform(msg.header, ball.center) if ball_transform: - diff = numpify(ball_transform.point) - self.get_position_numpy() + diff = numpify(ball_transform.point) - self.ball_state_position if abs(diff[0]) < ignore_threshold_x and abs(diff[0]) < ignore_threshold_y: filtered_balls.append((ball, ball_transform, np.linalg.norm(diff))) @@ -170,7 +167,6 @@ def update_params(self) -> None: """ self.config = self.param_listener.get_params() self.filter_time_step = 1.0 / self.config.filter_rate - self.filter_reset_duration = Duration(seconds=self.config.filter_reset_time) def is_estimate_in_fov(self, header: Header) -> bool: """ @@ -224,7 +220,7 @@ def filter_step(self) -> None: self.update_params() # Increase covariance - self.ball_state_covariance += np.eye(2) * self.config.process_noise_variance + self.ball_state_covariance += np.eye(3) * self.config.process_noise_variance # Pose pose_msg = PoseWithCovarianceStamped() @@ -233,8 +229,9 @@ def filter_step(self) -> None: frame_id=self.config.filter_frame, ) pose_msg.pose.pose.position = msgify(Point, self.ball_state_position) - pose_msg.pose.covariance = np.zeros((6, 6)) - pose_msg.pose.covariance[:3, :3] = self.ball_state_covariance + covariance = np.zeros((6, 6)) + covariance[:3, :3] = self.ball_state_covariance + pose_msg.pose.covariance = covariance.flatten() pose_msg.pose.pose.orientation.w = 1.0 self.ball_pose_publisher.publish(pose_msg) From edaffce01bfb5548656a230dab1de399f79e392d Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 18 Jun 2024 18:45:49 +0200 Subject: [PATCH 12/15] Scale correct direction --- .../bitbots_ball_filter/bitbots_ball_filter/ball_filter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 8f5a8a28b..aa7e08a0f 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -140,7 +140,7 @@ def ball_callback(self, msg: BallArray) -> None: # If we did not get a ball measurement, we can check if we should have seen the ball # And increase the covariance if we did not see the ball if not ball_measurement_updated and self.is_estimate_in_fov(msg.header): - self.ball_state_covariance += self.config.negative_observation_value + self.ball_state_covariance += np.eye(3) * self.config.negative_observation_value def _get_transform( self, header: Header, point: Point, frame: Optional[str] = None, timeout: float = 0.3 From b179618618c7d63f05748daeadf7099163636742 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 19 Jun 2024 15:29:28 +0200 Subject: [PATCH 13/15] Fix reworked ball filter --- .../config/body_behavior.yaml | 2 +- .../bitbots_containers/hlvs/Dockerfile | 2 +- .../bitbots_ball_filter/ball_filter.py | 85 +++++----- .../config/ball_filter_parameters.yaml | 145 ++++++++++-------- .../bitbots_ball_filter/package.xml | 6 +- requirements/common.txt | 1 - 6 files changed, 127 insertions(+), 114 deletions(-) diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 060866dbe..8dbd5351a 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/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: diff --git a/bitbots_misc/bitbots_containers/hlvs/Dockerfile b/bitbots_misc/bitbots_containers/hlvs/Dockerfile index 71ef58d4c..c51e74415 100644 --- a/bitbots_misc/bitbots_containers/hlvs/Dockerfile +++ b/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 diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index aa7e08a0f..9b7faf7e1 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -10,9 +10,9 @@ from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -from sensor_msgs.msg import CameraInfo from rclpy.time import Time from ros2_numpy import msgify, numpify +from sensor_msgs.msg import CameraInfo from soccer_vision_3d_msgs.msg import Ball, BallArray from std_msgs.msg import Header from std_srvs.srv import Trigger @@ -38,7 +38,7 @@ def __init__(self) -> None: # Initialize parameters self.update_params() - self.logger.info(f"Using frame '{self.config.filter_frame}' for ball filtering") + self.logger.info(f"Using frame '{self.config.filter.frame}' for ball filtering") self.camera_info: Optional[CameraInfo] = None @@ -47,7 +47,7 @@ def __init__(self) -> None: # publishes positions of ball self.ball_pose_publisher = self.create_publisher( - PoseWithCovarianceStamped, self.config.ball_position_publish_topic, 1 + PoseWithCovarianceStamped, self.config.ros.ball_position_publish_topic, 1 ) # Create callback group @@ -56,19 +56,19 @@ def __init__(self) -> None: # setup subscriber self.ball_subscriber = self.create_subscription( BallArray, - self.config.ball_subscribe_topic, + self.config.ros.ball_subscribe_topic, self.ball_callback, 2, callback_group=self.callback_group, ) self.camera_info_subscriber = self.create_subscription( - CameraInfo, self.config.camera_info_subscribe_topic, self.camera_info_callback, 1 + CameraInfo, self.config.ros.camera_info_subscribe_topic, self.camera_info_callback, 1 ) self.reset_service = self.create_service( Trigger, - self.config.ball_filter_reset_service_name, + self.config.ros.ball_filter_reset_service_name, self.reset_filter_cb, callback_group=self.callback_group, ) @@ -103,7 +103,7 @@ def ball_callback(self, msg: BallArray) -> None: # The ignore distance is calculated using the filter's covariance and a factor # This way false positives are ignored if we already have a good estimate ignore_threshold_x, ignore_threshold_y, _ = ( - np.sqrt(np.diag(self.ball_state_covariance)) * self.config.ignore_measurement_threshold + np.diag(self.ball_state_covariance) * self.config.filter.tracking.ignore_measurement_threshold ) # Filter out balls that are too far away from the filter's estimate @@ -115,32 +115,31 @@ def ball_callback(self, msg: BallArray) -> None: ball_transform = self._get_transform(msg.header, ball.center) if ball_transform: diff = numpify(ball_transform.point) - self.ball_state_position - if abs(diff[0]) < ignore_threshold_x and abs(diff[0]) < ignore_threshold_y: + if abs(diff[0]) < ignore_threshold_x and abs(diff[1]) < ignore_threshold_y: filtered_balls.append((ball, ball_transform, np.linalg.norm(diff))) # Select the ball with closest distance to the filter estimate # Return if no ball was found ball_msg, ball_measurement_map, _ = min(filtered_balls, key=lambda x: x[2], default=(None, None, 0)) - if ball_measurement_map is None: - return - - # Estimate the covariance of the measurement - # Calculate the distance from the robot to the ball - distance = np.linalg.norm(numpify(ball_msg.center)) - covariance = np.eye(3) * ( - self.config.measurement_certainty + (distance**2) * self.config.noise_increment_factor - ) - covariance[2, 2] = 0.0 # Ignore z-axis - - # Store the ball measurement - self.ball_state_position = numpify(ball_measurement_map.point) - self.ball_state_covariance = covariance - ball_measurement_updated = True + if ball_measurement_map is not None: + # Estimate the covariance of the measurement + # Calculate the distance from the robot to the ball + distance = np.linalg.norm(numpify(ball_msg.center)) + covariance = np.eye(3) * ( + self.config.filter.covariance.measurement_uncertainty + + (distance**2) * self.config.filter.covariance.distance_factor + ) + covariance[2, 2] = 0.0 # Ignore z-axis + + # Store the ball measurement + self.ball_state_position = numpify(ball_measurement_map.point) + self.ball_state_covariance = covariance + ball_measurement_updated = True # If we did not get a ball measurement, we can check if we should have seen the ball # And increase the covariance if we did not see the ball if not ball_measurement_updated and self.is_estimate_in_fov(msg.header): - self.ball_state_covariance += np.eye(3) * self.config.negative_observation_value + self.ball_state_covariance *= np.eye(3) * self.config.filter.covariance.negative_observation.value def _get_transform( self, header: Header, point: Point, frame: Optional[str] = None, timeout: float = 0.3 @@ -150,7 +149,7 @@ def _get_transform( """ if frame is None: - frame = self.config.filter_frame + frame = self.config.filter.frame point_stamped = PointStamped() point_stamped.header = header @@ -166,7 +165,7 @@ def update_params(self) -> None: Updates parameters from dynamic reconfigure """ self.config = self.param_listener.get_params() - self.filter_time_step = 1.0 / self.config.filter_rate + self.filter_time_step = 1.0 / self.config.filter.rate def is_estimate_in_fov(self, header: Header) -> bool: """ @@ -176,10 +175,10 @@ def is_estimate_in_fov(self, header: Header) -> bool: if self.camera_info is None: self.logger.info("No camera info received. Not checking if the ball is currently visible.") return False - + # Build a pose ball_pose = PoseStamped() - ball_pose.header.frame_id = self.config.filter_frame + ball_pose.header.frame_id = self.config.filter.frame ball_pose.header.stamp = header.stamp ball_pose.pose.position = msgify(Point, self.ball_state_position) @@ -193,17 +192,21 @@ def is_estimate_in_fov(self, header: Header) -> bool: return False # Check if the ball is in front of the camera - if ball_in_camera_optical_frame.pose.position.z >= 0: - # Quick math to get the pixels - p = numpify(ball_in_camera_optical_frame.pose.position) - k = np.reshape(self.camera_info.k, (3, 3)) - p_pixel = np.matmul(k, p) - p_pixel = p_pixel * (1 / p_pixel[2]) - # Make sure that the transformed pixel is inside the resolution and positive. - # TODO border - if 0 < p_pixel[0] <= self.camera_info.width and 0 < p_pixel[1] <= self.camera_info.height: - return True - return False + if ball_in_camera_optical_frame.pose.position.z < 0: + return False + + # Quick math to get the pixel + p = numpify(ball_in_camera_optical_frame.pose.position) + k = np.reshape(self.camera_info.k, (3, 3)) + pixel = np.matmul(k, p) + pixel = pixel * (1 / pixel[2]) + + # Make sure that the transformed pixel is on the sensor and not too close to the border + border_fraction = self.config.filter.covariance.negative_observation.ignore_border + border_px = np.array([self.camera_info.width, self.camera_info.height]) / 2 * border_fraction + in_fov_horizontal = border_px[0] < pixel[0] <= self.camera_info.width - border_px[0] + in_fov_vertical = border_px[1] < pixel[1] <= self.camera_info.height - border_px[1] + return in_fov_horizontal and in_fov_vertical def filter_step(self) -> None: """ @@ -220,13 +223,13 @@ def filter_step(self) -> None: self.update_params() # Increase covariance - self.ball_state_covariance += np.eye(3) * self.config.process_noise_variance + self.ball_state_covariance[:2, :2] += np.eye(2) * self.config.filter.covariance.process_noise # Pose pose_msg = PoseWithCovarianceStamped() pose_msg.header = Header( stamp=Time.to_msg(self.get_clock().now()), - frame_id=self.config.filter_frame, + frame_id=self.config.filter.frame, ) pose_msg.pose.pose.position = msgify(Point, self.ball_state_position) covariance = np.zeros((6, 6)) diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index 9a68506ec..e29757d35 100644 --- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -1,74 +1,87 @@ bitbots_ball_filter: - ball_subscribe_topic: - type: string - default_value: 'balls_relative' - read_only: true - description: 'Topic to subscribe to for ball detections' + ros: + camera_info_subscribe_topic: + type: string + default_value: 'camera/camera_info' + description: 'Topic of the camera info' + read_only: True - ball_position_publish_topic: - type: string - default_value: 'ball_position_relative_filtered' - read_only: true - description: 'Topic to publish the filtered ball position' - - ball_filter_reset_service_name: - type: string - default_value: 'ball_filter_reset' - read_only: true - description: 'Service to reset the ball filter' - - filter_frame: - type: string - default_value: 'map' - description: 'Frame to filter the ball in' - validation: - one_of<>: [['map', 'odom']] + ball_subscribe_topic: + type: string + default_value: 'balls_relative' + read_only: true + description: 'Topic to subscribe to for ball detections' - filter_rate: - type: int - default_value: 62 - description: 'Filtering rate in Hz' - read_only: true - validation: - bounds<>: [0, 100] + ball_position_publish_topic: + type: string + default_value: 'ball_position_relative_filtered' + read_only: true + description: 'Topic to publish the filtered ball position' - process_noise_variance: - type: double - default_value: 0.0002 - description: 'Noise which is added to the estimated position without new measurements' - validation: - bounds<>: [0.0, 1.0] + ball_filter_reset_service_name: + type: string + default_value: 'ball_filter_reset' + read_only: true + description: 'Service to reset the ball filter' - measurement_certainty: - type: double - default_value: 0.02 - description: 'Ball measurement certainty in filter' - validation: - bounds<>: [0.0, 1.0] - - noise_increment_factor: - type: double - default_value: 0.2 - description: 'Factor to increase the noise if the ball is further away. This also depends on the reference distance.' - validation: - bounds<>: [0.0, 1.0] + filter: + frame: + type: string + default_value: 'map' + description: 'Frame to filter the ball in' + read_only: true + validation: + one_of<>: [['map', 'odom']] - camera_info_subscribe_topic: - type: string - default_value: 'camera/camera_info' - description: 'Topic of the camera info' - read_only: True + rate: + type: int + default_value: 62 + description: 'Filtering rate in Hz' + read_only: true + validation: + bounds<>: [0, 100] + + covariance: + process_noise: + type: double + default_value: 0.005 + description: 'Noise which is added to the estimated position without new measurements' + validation: + bounds<>: [0.0, 1.0] + + measurement_uncertainty: + type: double + default_value: 0.5 + description: 'Ball measurement certainty in filter' + validation: + bounds<>: [0.0, 1.0] + + distance_factor: + type: double + default_value: 0.02 + description: 'Factor to increase the noise if the ball measurement is further away. This also depends on the reference distance.' + validation: + bounds<>: [0.0, 1.0] + + negative_observation: + value: + type: double + default_value: 1.5 + description: 'Value by which the covariance is increased if we have a negative observation, meaning we did not see the ball if we should have seen it.' + validation: + bounds<>: [0.0, 10.0] - ignore_measurement_threshold: - type: double - default_value: 3.0 - description: 'We calculate a distance around our filter state over which we ignore measurements. This threshold is based on the covariance of the filter scaled by this factor. This is used to ignore false positives if we already have a good estimation.' - validation: - bounds<>: [0.0, 10.0] + ignore_border: + type: double + default_value: 0.1 + description: 'Border as a fraction of the image height and width. We do not consider the negative observation if they are close to the border of the image, as we might not see the ball there, when it is partially occluded.' + validation: + bounds<>: [0.0, 1.0] - negative_observation_value: - type: double - default_value: 3.0 - description: 'Value by which the covariance is increased if we have a negative observation, meaning we did not see the ball if we should have seen it.' - validation: - bounds<>: [0.0, 10.0] + tracking: + ignore_measurement_threshold: + type: double + default_value: 6.0 + description: 'We calculate a distance around our filter state over which we ignore measurements. This threshold is based on the covariance of the filter scaled by this factor. This is used to ignore false positives if we already have a good estimation.' + validation: + bounds<>: [0.0, 10.0] diff --git a/bitbots_world_model/bitbots_ball_filter/package.xml b/bitbots_world_model/bitbots_ball_filter/package.xml index d67b20f9c..7c3cf3f1b 100644 --- a/bitbots_world_model/bitbots_ball_filter/package.xml +++ b/bitbots_world_model/bitbots_ball_filter/package.xml @@ -12,18 +12,16 @@ Hamburg Bit-Bots - - rosidl_default_generators rosidl_default_runtime bitbots_tf_buffer + generate_parameter_library geometry_msgs rclpy + ros2_numpy std_msgs std_srvs tf2_geometry_msgs - generate_parameter_library - diff --git a/requirements/common.txt b/requirements/common.txt index 336e60371..2fbee8e57 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -1,5 +1,4 @@ # This file includes common dependencies for all environments -filterpy pip transforms3d git+https://github.com/Flova/pyastar2d From 97e24fe42b12ba79d68602fb679e2e1a6159ec45 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 19 Jun 2024 15:41:10 +0200 Subject: [PATCH 14/15] Fix comments --- .../bitbots_ball_filter/ball_filter.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 9b7faf7e1..e90ffff6d 100755 --- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -88,17 +88,19 @@ def reset_filter_cb(self, req, response) -> Tuple[bool, str]: return response def camera_info_callback(self, msg: CameraInfo): - """handles incoming ball messages""" + """Updates the camera intrinsics""" self.camera_info = msg def ball_callback(self, msg: BallArray) -> None: + """handles incoming ball detections form a frame captured by the camera""" + # Keep track if we have updated the measurement # We might not have a measurement if the ball is not visible # We also filter out balls that are too far away from the filter's estimate ball_measurement_updated: bool = False # Do filtering, transform, ... if we have a ball - if msg.balls: # Balls exist + if msg.balls: # Balls exist in the frame # Ignore balls that are too far away (false positives) # The ignore distance is calculated using the filter's covariance and a factor # This way false positives are ignored if we already have a good estimate @@ -112,19 +114,24 @@ def ball_callback(self, msg: BallArray) -> None: ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate ball: Ball for ball in msg.balls: + # Bring ball detection into the frame of the filter and decouple the ball from the robots movement ball_transform = self._get_transform(msg.header, ball.center) + # This checks if the ball can be transformed to the filter frame if ball_transform: + # Check if the ball is close enough to the filter's estimate diff = numpify(ball_transform.point) - self.ball_state_position if abs(diff[0]) < ignore_threshold_x and abs(diff[1]) < ignore_threshold_y: + # Store the ball relative to the robot, the ball in the filter frame and the distance to the filter estimate filtered_balls.append((ball, ball_transform, np.linalg.norm(diff))) # Select the ball with closest distance to the filter estimate - # Return if no ball was found ball_msg, ball_measurement_map, _ = min(filtered_balls, key=lambda x: x[2], default=(None, None, 0)) + # Only update the measurement if we have a ball that is close enough to the filter's estimate if ball_measurement_map is not None: # Estimate the covariance of the measurement # Calculate the distance from the robot to the ball distance = np.linalg.norm(numpify(ball_msg.center)) + # Calculate the covariance of the measurement based on the distance and the filter's configuration covariance = np.eye(3) * ( self.config.filter.covariance.measurement_uncertainty + (distance**2) * self.config.filter.covariance.distance_factor @@ -225,7 +232,7 @@ def filter_step(self) -> None: # Increase covariance self.ball_state_covariance[:2, :2] += np.eye(2) * self.config.filter.covariance.process_noise - # Pose + # Build message pose_msg = PoseWithCovarianceStamped() pose_msg.header = Header( stamp=Time.to_msg(self.get_clock().now()), From 59d2be718a14074a9f9b231c9efea47a0b8d2e8b Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 19 Jun 2024 16:21:52 +0200 Subject: [PATCH 15/15] Increase prescision thres --- .../bitbots_body_behavior/config/body_behavior.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 8dbd5351a..bb4b03e70 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -73,8 +73,8 @@ # the maximal allowed standard deviation of the ball position. ball_position_precision_threshold: - x_sdev: 1.0 - y_sdev: 1.0 + 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