55import tf2_ros as tf2
66from bitbots_utils .utils import get_parameters_from_other_node
77from geometry_msgs .msg import (
8+ Pose ,
89 PoseStamped ,
910 PoseWithCovarianceStamped ,
1011 TransformStamped ,
@@ -49,15 +50,14 @@ def __init__(self, node, blackboard):
4950 self .map_frame : str = self ._node .get_parameter ("map_frame" ).value
5051
5152 # Get body parameters
52- self .ball_lost_time = Duration (seconds = self ._node .get_parameter ("ball_lost_time" ).value )
5353 self .ball_max_covariance = self ._node .get_parameter ("ball_max_covariance" ).value
5454 self .map_margin : float = self ._node .get_parameter ("map_margin" ).value
5555
5656 # Ball state
57- self ._ball_seen_time : Time = Time (clock_type = ClockType .ROS_TIME ) # Time when the ball was last seen
5857 self ._ball : PointStamped = PointStamped (
5958 header = Header (stamp = Time (clock_type = ClockType .ROS_TIME ), frame_id = self .map_frame )
6059 ) # The ball in the map frame (default to the center of the field if ball is not seen yet)
60+ self ._ball_covariance : np .ndarray = np .zeros ((2 , 2 )) # Covariance of the ball
6161
6262 # Publisher for visualization in RViZ
6363 self .debug_publisher_used_ball = self ._node .create_publisher (PointStamped , "debug/behavior/used_ball" , 1 )
@@ -71,22 +71,12 @@ def __init__(self, node, blackboard):
7171 ############
7272
7373 def ball_seen_self (self ) -> bool :
74- """Returns true if we have seen the ball recently (less than ball_lost_time ago)"""
75- return self ._node .get_clock ().now () - self ._ball_seen_time < self .ball_lost_time
76-
77- def ball_last_seen (self ) -> Time :
78- """
79- Returns the time at which the ball was last seen if it is in the threshold or
80- the more recent ball from either the teammate or itself if teamcomm is available
81- """
82- if self .ball_seen_self ():
83- return self ._ball_seen_time
84- else :
85- return max (self ._ball_seen_time , self ._blackboard .team_data .get_teammate_ball_seen_time ())
74+ """Returns true we are reasonably sure that we have seen the ball"""
75+ return all (np .diag (self ._ball_covariance ) < self .ball_max_covariance )
8676
8777 def ball_has_been_seen (self ) -> bool :
88- """Returns true if we or a teammate have seen the ball recently (less than ball_lost_time ago) """
89- return self ._node . get_clock (). now () - self .ball_last_seen () < self . ball_lost_time
78+ """Returns true if we or a teammate are reasonably sure that we have seen the ball """
79+ return self .ball_seen_self () or self ._blackboard . team_data . teammate_ball_is_valid ()
9080
9181 def get_ball_position_xy (self ) -> Tuple [float , float ]:
9282 """Return the ball saved in the map frame, meaning the absolute position of the ball on the field"""
@@ -108,16 +98,17 @@ def get_best_ball_point_stamped(self) -> PointStamped:
10898 self .debug_publisher_which_ball .publish (Header (stamp = teammate_ball .header .stamp , frame_id = "teammate_ball" ))
10999 return teammate_ball
110100
111- # Otherwise, use the own ball even if it is too old
101+ # Otherwise, use the own ball even if it is bad
112102 if not self .ball_seen_self ():
113- self ._node .get_logger ().warn ("Using own ball even though it is too old , as no teammate ball is available" )
103+ self ._node .get_logger ().warn ("Using own ball even though it is bad , as no teammate ball is available" )
114104 self .debug_publisher_used_ball .publish (own_ball )
115105 self .debug_publisher_which_ball .publish (Header (stamp = own_ball .header .stamp , frame_id = "own_ball_map" ))
116106 return own_ball
117107
118108 def get_ball_position_uv (self ) -> Tuple [float , float ]:
119109 """
120- Returns the ball position relative to the robot in the base_footprint frame
110+ Returns the ball position relative to the robot in the base_footprint frame.
111+ U and V are returned, where positive U is forward, positive V is to the left.
121112 """
122113 ball = self .get_best_ball_point_stamped ()
123114 try :
@@ -131,58 +122,50 @@ def get_ball_position_uv(self) -> Tuple[float, float]:
131122 return ball_bfp .x , ball_bfp .y
132123
133124 def get_ball_distance (self ) -> float :
134- ball_pos = self .get_ball_position_uv ()
135- if ball_pos is None :
125+ """
126+ Returns the distance to the ball in meters.
127+ """
128+ if not (ball_pos := self .get_ball_position_uv ()):
136129 return np .inf # worst case (very far away)
137- else :
138- u , v = ball_pos
139-
130+ u , v = ball_pos
140131 return math .hypot (u , v )
141132
142133 def get_ball_angle (self ) -> float :
143- ball_pos = self .get_ball_position_uv ()
144- if ball_pos is None :
134+ """
135+ Returns the angle to the ball in radians.
136+ 0 is straight ahead, positive is to the left, negative is to the right.
137+ """
138+ if not (ball_pos := self .get_ball_position_uv ()):
145139 return - math .pi # worst case (behind robot)
146- else :
147- u , v = ball_pos
140+ u , v = ball_pos
148141 return math .atan2 (v , u )
149142
150143 def ball_filtered_callback (self , msg : PoseWithCovarianceStamped ):
151- # When the precision is not sufficient, the ball ages.
152- x_sdev = msg .pose .covariance [0 ] # position 0,0 in a 6x6-matrix
153- y_sdev = msg .pose .covariance [7 ] # position 1,1 in a 6x6-matrix
154- if x_sdev > self .ball_max_covariance or y_sdev > self .ball_max_covariance :
155- self .forget_ball (reset_ball_filter = False )
156- return
157-
158- try :
159- self ._ball = self ._blackboard .tf_buffer .transform (
160- PointStamped (header = msg .header , point = msg .pose .pose .position ),
161- self .map_frame ,
162- timeout = Duration (seconds = 1.0 ),
163- )
164- # Set timestamps to zero to get the newest transform when this is transformed later
165- self ._ball_seen_time = Time .from_msg (msg .header .stamp )
166- self ._ball .header .stamp = Time (clock_type = ClockType .ROS_TIME ).to_msg ()
144+ """
145+ Handles incoming ball messages
146+ """
147+ assert msg .header .frame_id == self .map_frame , "Ball needs to be in the map frame"
148+
149+ # Save ball
150+ self ._ball = PointStamped (
151+ header = Header (
152+ # Set timestamps to zero to get the newest transform when this is transformed later
153+ stamp = Time (clock_type = ClockType .ROS_TIME ).to_msg (),
154+ frame_id = self .map_frame ,
155+ ),
156+ point = msg .pose .pose .position ,
157+ )
167158
168- except ( tf2 . ConnectivityException , tf2 . LookupException , tf2 . ExtrapolationException ) as e :
169- self . _node . get_logger (). warn ( str ( e ))
159+ # Save covariance (only x and y parts)
160+ self . _ball_covariance = msg . pose . covariance . reshape (( 6 , 6 ))[: 2 , : 2 ]
170161
171- def forget_ball (self , reset_ball_filter : bool = True ) -> None :
162+ def forget_ball (self ) -> None :
172163 """
173- Forget that we saw a ball, optionally reset the ball filter
174- :param reset_ball_filter: Reset the ball filter, defaults to True
175- :type reset_ball_filter: bool, optional
164+ Forget that we saw a ball
176165 """
177- # Forget own ball
178- self ._ball_seen_time = Time (clock_type = ClockType .ROS_TIME )
179-
180- if reset_ball_filter : # Reset the ball filter
181- result : Trigger .Response = self .reset_ball_filter .call (Trigger .Request ())
182- if result .success :
183- self ._node .get_logger ().debug (f"Received message from ball filter: '{ result .message } )
184- else :
185- self ._node .get_logger ().warn (f"Ball filter reset failed with: '{ result .message } )
166+ result : Trigger .Response = self .reset_ball_filter .call (Trigger .Request ())
167+ if not result .success :
168+ self ._node .get_logger ().error (f"Ball filter reset failed with: '{ result .message } )
186169
187170 ########
188171 # Goal #
@@ -233,8 +216,7 @@ def get_current_position(self) -> Optional[Tuple[float, float, float]]:
233216 0,0,0 is the center of the field looking in the direction of the opponent goal.
234217 :returns x,y,theta:
235218 """
236- transform = self .get_current_position_transform ()
237- if transform is None :
219+ if not (transform := self .get_current_position_transform ()):
238220 return None
239221 theta = euler_from_quaternion (numpify (transform .transform .rotation ))[2 ]
240222 return transform .transform .translation .x , transform .transform .translation .y , theta
@@ -243,17 +225,15 @@ def get_current_position_pose_stamped(self) -> Optional[PoseStamped]:
243225 """
244226 Returns the current position as determined by the localization as a PoseStamped
245227 """
246- transform = self .get_current_position_transform ()
247- if transform is None :
228+ if not (transform := self .get_current_position_transform ()):
248229 return None
249- ps = PoseStamped ()
250- ps .header = transform .header
251- ps .pose .position = msgify (Point , numpify (transform .transform .translation ))
252- ps .pose .orientation .x = transform .transform .rotation .x
253- ps .pose .orientation .y = transform .transform .rotation .y
254- ps .pose .orientation .z = transform .transform .rotation .z
255- ps .pose .orientation .w = transform .transform .rotation .w
256- return ps
230+ return PoseStamped (
231+ header = transform .header ,
232+ pose = Pose (
233+ position = msgify (Point , numpify (transform .transform .translation )),
234+ orientation = transform .transform .rotation ,
235+ ),
236+ )
257237
258238 def get_current_position_transform (self ) -> TransformStamped :
259239 """
0 commit comments