From ee8a1269179991ad345a124a1aaa27c1e4a4c10d Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sat, 26 Jun 2021 12:32:10 +0200 Subject: [PATCH 1/2] Check if we should be able to see the ball --- bitbots_ball_filter/cfg/BallFilter.cfg | 1 + bitbots_ball_filter/config/params.yaml | 1 + .../src/bitbots_ball_filter/ball_filter.py | 76 ++++++++++++++++++- 3 files changed, 75 insertions(+), 3 deletions(-) diff --git a/bitbots_ball_filter/cfg/BallFilter.cfg b/bitbots_ball_filter/cfg/BallFilter.cfg index 3d8ebe0..4f9a8c3 100755 --- a/bitbots_ball_filter/cfg/BallFilter.cfg +++ b/bitbots_ball_filter/cfg/BallFilter.cfg @@ -20,6 +20,7 @@ group_ROS= gen.add_group("ROS", type="tab") group_filter= gen.add_group("Filter", type="tab") group_ROS.add("ball_subscribe_topic", str_t, 0, "", None) +group_ROS.add("camera_info_subscribe_topic", str_t, 0, "", None) group_ROS.add("ball_position_publish_topic", str_t, 0, "", None) group_ROS.add("ball_movement_publish_topic", str_t, 0, "", None) group_ROS.add("ball_publish_topic", str_t, 0, "", None) diff --git a/bitbots_ball_filter/config/params.yaml b/bitbots_ball_filter/config/params.yaml index ae8c1bf..5cd8f59 100644 --- a/bitbots_ball_filter/config/params.yaml +++ b/bitbots_ball_filter/config/params.yaml @@ -1,5 +1,6 @@ # Topics ball_subscribe_topic: 'balls_relative' +camera_info_subscribe_topic: 'camera/camera_info' ball_position_publish_topic: 'ball_position_relative_filtered' ball_movement_publish_topic: 'ball_relative_movement' ball_publish_topic: 'ball_relative_filtered' diff --git a/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py b/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py index aed8a27..c6d19e1 100755 --- a/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py +++ b/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py @@ -8,12 +8,14 @@ from filterpy.common import Q_discrete_white_noise from filterpy.kalman import KalmanFilter from geometry_msgs.msg import (PoseWithCovarianceStamped, + PoseStamped, TwistWithCovarianceStamped) from humanoid_league_msgs.msg import (PoseWithCertainty, PoseWithCertaintyArray, PoseWithCertaintyStamped) from std_msgs.msg import Header from std_srvs.srv import Trigger +from sensor_msgs.msg import CameraInfo from tf2_geometry_msgs import PointStamped from bitbots_ball_filter.cfg import BallFilterConfig @@ -48,6 +50,8 @@ def _dynamic_reconfigure_callback(self, config, level): self.ball = None # type: PointStamped self.ball_header = None # type: Header self.last_ball_msg = PoseWithCertainty() # type: PoseWithCertainty + self.ball_should_be_visible_counter = 0 + self.camera_info = None self.filter_rate = config['filter_rate'] self.min_ball_confidence = config['min_ball_confidence'] @@ -90,13 +94,20 @@ def _dynamic_reconfigure_callback(self, config, level): ) # setup subscriber - self.subscriber = rospy.Subscriber( + self.ball_subscriber = rospy.Subscriber( config['ball_subscribe_topic'], PoseWithCertaintyArray, self.ball_callback, queue_size=1 ) - + + self.camera_info_subscriber = rospy.Subscriber( + config['camera_info_subscribe_topic'], + CameraInfo, + self.camera_info_callback, + queue_size=1 + ) + self.reset_service = rospy.Service( config['ball_filter_reset_service_name'], Trigger, @@ -107,14 +118,24 @@ def _dynamic_reconfigure_callback(self, config, level): self.filter_timer = rospy.Timer(rospy.Duration(self.filter_time_step), self.filter_step) return config + def camera_info_callback(self, msg: CameraInfo): + """handles incoming ball messages""" + self.camera_info = msg + def ball_callback(self, msg: PoseWithCertaintyArray): """handles incoming ball messages""" if msg.poses: + # Ball was visible so reset counter + self.ball_should_be_visible_counter = 0 + rospy.loginfo("I saw the ball :D") + + # Sort balls balls = sorted(msg.poses, reverse=True, key=lambda ball: ball.confidence) # Sort all balls by confidence ball = balls[0] # Ball with highest confidence if ball.confidence < self.min_ball_confidence: return + self.last_ball_msg = ball ball_buffer = PointStamped(msg.header, ball.pose.pose.position) try: @@ -122,6 +143,55 @@ def ball_callback(self, msg: PoseWithCertaintyArray): self.ball_header = msg.header except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: rospy.logwarn(e) + else: + # Check if we should be able to see the ball + if self.ball_should_be_visible(): + rospy.logerr("Should be able to see the ball but no...") + # Increase a counter for the grace period + self.ball_should_be_visible_counter += 1 + # If we should be able to see the ball for n steps but don't reset the filter + if self.ball_should_be_visible_counter > 5: + # Reset and counter + self.reset_filter_cb(None) + self.ball_should_be_visible_counter = 0 + rospy.logerr("I definetly lost the ball here...") + else: + #self.ball_should_be_visible_counter = 0 + rospy.loginfo("Its okay not seeing the ball here") + + + def ball_should_be_visible(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: + rospy.logwarn("No camera info recived. 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_pose = PoseStamped() + ball_pose.header.frame_id = self.filter_frame + ball_pose.header.stamp = rospy.Time.now() + 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_pose, self.camera_info.header.frame_id, timeout=rospy.Duration(0.5)) + # 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 reset_filter_cb(self, req): rospy.loginfo("Resetting bitbots ball filter...", logger_name="ball_filter") @@ -148,7 +218,7 @@ def filter_step(self, event): self.publish_data(*state) self.last_state = state self.ball = None - else: + else: if self.filter_initialized: if (rospy.Time.now() - self.ball_header.stamp) > self.filter_reset_duration: self.filter_initialized = False From 82a2d42461413faec1c669b0bb8fe7149d2d357e Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sat, 26 Jun 2021 12:42:42 +0200 Subject: [PATCH 2/2] Remove ball min confidence --- bitbots_ball_filter/cfg/BallFilter.cfg | 1 - bitbots_ball_filter/config/params.yaml | 3 --- bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py | 4 ---- 3 files changed, 8 deletions(-) diff --git a/bitbots_ball_filter/cfg/BallFilter.cfg b/bitbots_ball_filter/cfg/BallFilter.cfg index 4f9a8c3..57f4ea8 100755 --- a/bitbots_ball_filter/cfg/BallFilter.cfg +++ b/bitbots_ball_filter/cfg/BallFilter.cfg @@ -29,7 +29,6 @@ group_ROS.add("ball_filter_reset_service_name", str_t, 0, "Name of service for r group_filter.add("filter_rate", int_t, 0, "Filtering rate in Hz", min=0, max=255) -group_filter.add("min_ball_confidence", double_t, 0, "Minimum ball confidence", min=0.0, max=1.0) group_filter.add("velocity_reduction", double_t, 0, "Velocity reduction (per axis) factor of the ball per second", min=0, max=1.0) group_filter.add("process_noise_variance", double_t, 0, "Noise which is added to the estimated position without new measurements", min=0, max=1.0) group_filter.add("measurement_certainty", double_t, 0, "Ball measurement certainty in filter", min=0, max=1.0) diff --git a/bitbots_ball_filter/config/params.yaml b/bitbots_ball_filter/config/params.yaml index 5cd8f59..06f66d8 100644 --- a/bitbots_ball_filter/config/params.yaml +++ b/bitbots_ball_filter/config/params.yaml @@ -13,9 +13,6 @@ filter_frame: 'odom' # Filtering rate in Hz filter_rate: 62 # we get an image every 16 ms -> 62.5 hz -# Balls with lower confidence will be dropped -min_ball_confidence: 0.6 - # Velocity reduction (per axis) factor of the ball per second velocity_reduction: 0.6 diff --git a/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py b/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py index c6d19e1..d0c63f0 100755 --- a/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py +++ b/bitbots_ball_filter/src/bitbots_ball_filter/ball_filter.py @@ -54,7 +54,6 @@ def _dynamic_reconfigure_callback(self, config, level): self.camera_info = None self.filter_rate = config['filter_rate'] - self.min_ball_confidence = config['min_ball_confidence'] self.measurement_certainty = config['measurement_certainty'] self.filter_time_step = 1.0 / self.filter_rate self.filter_reset_duration = rospy.Duration(secs=config['filter_reset_time']) @@ -133,9 +132,6 @@ def ball_callback(self, msg: PoseWithCertaintyArray): balls = sorted(msg.poses, reverse=True, key=lambda ball: ball.confidence) # Sort all balls by confidence ball = balls[0] # Ball with highest confidence - if ball.confidence < self.min_ball_confidence: - return - self.last_ball_msg = ball ball_buffer = PointStamped(msg.header, ball.pose.pose.position) try: