def get_detection_based_goal_position_uv(self):
"""
returns the position of the goal relative to the robot.
if only a single post is detected, the position of the post is returned.
else, it is the point between the posts
:return:
"""
left = PointStamped(self.goal_odom.header, self.goal_odom.left_post)
right = PointStamped(self.goal_odom.header, self.goal_odom.right_post)
left.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
right.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
try:
left_bfp = self.tf_buffer.transform(
left, self.base_footprint_frame,
timeout=Duration(seconds=0.2)).point
right_bfp = self.tf_buffer.transform(
right,
self.base_footprint_frame,
timeout=Duration(seconds=0.2)).point
except tf2.ExtrapolationException as e:
self.get_logger().warn(e)
self.get_logger().error(
'Severe transformation problem concerning the goal!')
return None
return (left_bfp.x + right_bfp.x / 2.0), \
(left_bfp.y + right_bfp.y / 2.0)
def forget_ball(self, own=True, team=True, reset_ball_filter=True):
"""
Forget that we and the best teammate saw a ball, optionally reset the ball filter
:param own: Forget the ball recognized by the own robot, defaults to True
:type own: bool, optional
:param team: Forget the ball received from the team, defaults to True
:type team: bool, optional
:param reset_ball_filter: Reset the ball filter, defaults to True
:type reset_ball_filter: bool, optional
"""
if own: # Forget own ball
self.ball_seen_time = rclpy.Time(seconds=0, nanoseconds=0)
self.ball = PointStamped()
if team: # Forget team ball
self.ball_seen_time_teammate = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_teammate = PointStamped()
if reset_ball_filter: # Reset the ball filter
result = self.reset_ball_filter()
if result.success:
self.get_logger().info(
f"Received message from ball filter: '{result.message}'",
logger_name='bitbots_blackboard')
else:
self.get_logger().warn(
f"Ball filter reset failed with: '{result.message}'",
logger_name='bitbots_blackboard')
def ball_filtered_callback(self, msg: PoseWithCovarianceStamped):
self.ball_filtered = msg
# When the precision is not sufficient, the ball ages.
x_sdev = msg.pose.covariance[0] # position 0,0 in a 6x6-matrix
y_sdev = msg.pose.covariance[7] # position 1,1 in a 6x6-matrix
if x_sdev > self.body_config['ball_position_precision_threshold']['x_sdev'] or \
y_sdev > self.body_config['ball_position_precision_threshold']['y_sdev']:
self.forget_ball(own=True, team=False, reset_ball_filter=False)
return
ball_buffer = PointStamped(msg.header, msg.pose.pose.position)
try:
self.ball = self.tf_buffer.transform(ball_buffer,
self.base_footprint_frame,
timeout=Duration(seconds=0.3))
self.ball_odom = self.tf_buffer.transform(
ball_buffer, self.odom_frame, timeout=Duration(seconds=0.3))
self.ball_map = self.tf_buffer.transform(
ball_buffer, self.map_frame, timeout=Duration(seconds=0.3))
# Set timestamps to zero to get the newest transform when this is transformed later
self.ball_odom.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_map.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_seen_time = self.get_clock().now()
self.ball_publisher.publish(self.ball)
self.ball_seen = True
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
self.get_logger().warn(e)
def get_cost_of_kick_relative(self, x, y, direction, kick_length,
angular_range):
if self.costmap is None:
return 0.0
pose = PoseStamped()
pose.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
pose.header.frame_id = self.base_footprint_frame
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, direction))
try:
# Transform point of interest to the map
pose = self.tf_buffer.transform(pose,
self.map_frame,
timeout=Duration(seconds=0.3))
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
self.get_logger().warn(e)
return 0.0
d = euler_from_quaternion([
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
])[2]
return self.get_cost_of_kick(pose.pose.position.x,
pose.pose.position.y, d, kick_length,
angular_range)
def perform(self, reevaluate=False):
"""
Check if the time we last saw the ball is now.
"Now"-Tolerance can be configured through ball_lost_time.
:param reevaluate: Has no effect
"""
ball_last_seen = self.blackboard.world_model.ball_last_seen()
if ball_last_seen != rclpy.Time(0) and self.get_clock().now() - ball_last_seen < self.ball_lost_time:
return 'YES'
return 'NO'
def localization_pose_current(self) -> bool:
"""
Returns whether we can transform into and from the map frame.
"""
# if we can do this, we should be able to transform the ball
# (unless the localization dies in the next 0.2 seconds)
try:
t = self.get_clock().now() - Duration(seconds=0.3)
except TypeError as e:
self.get_logger().error(e)
t = rclpy.Time(seconds=0, nanoseconds=0)
return self.tf_buffer.can_transform(self.base_footprint_frame,
self.map_frame, t)
def get_current_position_transform(self) -> TransformStamped:
"""
Returns the current position as determined by the localization as a TransformStamped
"""
try:
# get the most recent transform
transform = self.tf_buffer.lookup_transform(
self.map_frame, self.base_footprint_frame,
rclpy.Time(seconds=0, nanoseconds=0))
except (tf2.LookupException, tf2.ConnectivityException,
tf2.ExtrapolationException) as e:
self.get_logger().warn(e)
return None
return transform
def transform_goal_to_map(self, msg):
# type: (PoseStamped) -> PoseStamped
# transform local goal to goal in map frame
if msg.header.frame_id == self.map_frame:
return msg
else:
try:
msg.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
map_goal = self.tf_buffer.transform(
msg, self.map_frame, timeout=Duration(seconds=0.5))
e = euler_from_quaternion(
(map_goal.pose.orientation.x, map_goal.pose.orientation.y,
map_goal.pose.orientation.z, map_goal.pose.orientation.w))
q = quaternion_from_euler(0, 0, e[2])
map_goal.pose.orientation.x = q[0]
map_goal.pose.orientation.y = q[1]
map_goal.pose.orientation.z = q[2]
map_goal.pose.orientation.w = q[3]
map_goal.pose.position.z = 0
return map_goal
except Exception as e:
self.get_logger().warn(e)
return
def cost_at_relative_xy(self, x, y):
"""
Returns cost at relative position to the base footprint.
"""
if self.costmap is None:
return 0.0
point = PointStamped()
point.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
point.header.frame_id = self.base_footprint_frame
point.point.x = x
point.point.y = y
try:
# Transform point of interest to the map
point = self.tf_buffer.transform(point,
self.map_frame,
timeout=Duration(seconds=0.3))
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
self.get_logger().warn(e)
return 0.0
return self.get_cost_at_field_position(point.point.x, point.point.y)
def __init__(self, blackboard, node: Node):
self.node = node
self._blackboard = blackboard
self.body_config = self.node.get_parameter(
"behavior/body").get_parameter_value().string_value
# This pose is not supposed to be used as robot pose. Just as precision measurement for the TF position.
self.pose = PoseWithCovarianceStamped()
self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30))
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.odom_frame = self.node.get_parameter(
'~odom_frame').get_parameter_value().string_value
self.map_frame = self.node.get_parameter(
'~map_frame').get_parameter_value().string_value
self.ball_frame = self.node.get_parameter(
'~ball_frame').get_parameter_value().string_value
self.base_footprint_frame = self.node.get_parameter(
'~base_footprint_frame').get_parameter_value().string_value
self.ball = PointStamped() # The ball in the base footprint frame
self.ball_odom = PointStamped(
) # The ball in the odom frame (when localization is not usable)
self.ball_odom.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_odom.header.frame_id = self.odom_frame
self.ball_map = PointStamped(
) # The ball in the map frame (when localization is usable)
self.ball_map.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_map.header.frame_id = self.map_frame
self.ball_teammate = PointStamped()
self.ball_teammate.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_teammate.header.frame_id = self.map_frame
self.ball_lost_time = Duration(seconds=self.node.get_parameter(
'behavior/body/ball_lost_time').get_parameter_value().double_value)
self.ball_twist_map = None
self.ball_filtered = None
self.ball_twist_lost_time = Duration(seconds=self.node.get_parameter(
'behavior/body/ball_twist_lost_time').get_parameter_value().
double_value)
self.ball_twist_precision_threshold = self.node.get_parameter(
'behavior/body/ball_twist_precision_threshold'
).get_parameter_value().double_value
self.reset_ball_filter = self.node.create_client(
Trigger, 'ball_filter_reset')
self.goal = GoalRelative() # The goal in the base footprint frame
self.goal_odom = GoalRelative()
self.goal_odom.header.stamp = self.get_clock().now()
self.goal_odom.header.frame_id = self.odom_frame
self.my_data = dict()
self.counter = 0
self.ball_seen_time = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_seen_time_teammate = rclpy.Time(seconds=0, nanoseconds=0)
self.goal_seen_time = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_seen = False
self.ball_seen_teammate = False
self.field_length = self.node.get_parameter(
'field_length').get_parameter_value().double_value
self.field_width = self.node.get_parameter(
'field_width').get_parameter_value().double_value
self.goal_width = self.node.get_parameter(
'goal_width').get_parameter_value().double_value
self.map_margin = self.node.get_parameter(
'behavior/body/map_margin').get_parameter_value().double_value
self.obstacle_costmap_smoothing_sigma = self.node.get_parameter(
'"behavior/body/obstacle_costmap_smoothing_sigma"'
).get_parameter_value().double_value
self.obstacle_cost = self.node.get_parameter(
'behavior/body/obstacle_cost').get_parameter_value().double_value
self.use_localization = self.node.get_parameter(
'behavior/body/use_localization').get_parameter_value(
).double_value
self.pose_precision_threshold = self.node.get_parameter(
'behavior/body/pose_precision_threshold').get_parameter_value(
).double_value
# Publisher for visualization in RViZ
self.ball_publisher = self.create_publisher(PointStamped,
'debug/viz_ball', 1)
self.goal_publisher = self.create_publisher(PoseWithCertaintyArray,
'debug/viz_goal', 1)
self.ball_twist_publisher = self.create_publisher(
TwistStamped, 'debug/ball_twist', 1)
self.used_ball_pub = self.create_publisher(PointStamped,
'debug/used_ball', 1)
self.which_ball_pub = self.create_publisher(
Header, 'debug/which_ball_is_used', 1)
self.costmap_publisher = self.create_publisher(OccupancyGrid,
'debug/costmap', 1)
self.base_costmap = None # generated once in constructor field features
self.costmap = None # updated on the fly based on the base_costmap
self.gradient_map = None # global heading map (static) only dependent on field structure
# Calculates the base costmap and gradient map based on it
self.calc_base_costmap()
self.calc_gradients()