def to_pose_with_certainty_array(self):
p = PoseWithCertaintyArray()
p.header = self.header
l = PoseWithCertainty()
l.pose.pose.position = self.left_post
r = PoseWithCertainty()
r.pose.pose.position = self.right_post
p.poses = [l, r]
return p
def _callback(self, msg):
br = tf.TransformBroadcaster()
for i in range(len(msg.name)):
if msg.name[i] == "/":
transform = TransformStamped()
transform.header.frame_id = "world"
transform.header.stamp = rospy.Time.now()
transform.child_frame_id = "base_link"
transform.transform.translation = msg.pose[i].position
transform.transform.rotation = msg.pose[i].orientation
br.sendTransformMessage(transform)
self.robo_msg.pose.pose = msg.pose[i]
self.robo_msg.header.stamp = rospy.Time.now()
self.robo_msg.header.frame_id = "world"
self.robot_pub.publish(self.robo_msg)
elif msg.name[i] == "teensize_ball":
transform = TransformStamped()
transform.header.frame_id = "world"
transform.header.stamp = rospy.Time.now()
transform.child_frame_id = "ball"
transform.transform.translation = msg.pose[i].position
transform.transform.rotation = msg.pose[i].orientation
br.sendTransformMessage(transform)
ball_msg = PoseWithCertainty()
ball_msg.pose.pose.position.x = msg.pose[i].position.x
ball_msg.pose.pose.position.y = msg.pose[i].position.y
ball_msg.pose.pose.position.z = msg.pose[i].position.z
ball_msg.pose.pose.orientation.x = 0
ball_msg.pose.pose.orientation.y = 0
ball_msg.pose.pose.orientation.z = 0
ball_msg.pose.pose.orientation.w = 1
ball_msg.pose.covariance = [1] * 36
ball_msg.confidence = 1.0
self.balls.append(ball_msg)
balls_msg = PoseWithCertaintyArray()
balls_msg.header.frame_id = "world"
balls_msg.header.stamp = rospy.Time.now()
balls_msg.poses = self.balls
self.balls_pub.publish(balls_msg)
transform = TransformStamped()
transform.header.frame_id = "world"
transform.header.stamp = rospy.Time.now()
transform.child_frame_id = "map"
transform.transform.translation.x = -10.15 / 2
transform.transform.translation.y = -7.13 / 2
transform.transform.translation.z = 0
transform.transform.rotation.x = 0
transform.transform.rotation.y = 0
transform.transform.rotation.z = 0
transform.transform.rotation.w = 1
br.sendTransformMessage(transform)
def _callback_ball(self, msg):
field = self.get_plane(msg.header.stamp, self._ball_height)
if field is None:
return
balls = []
for ball in msg.candidates:
transformed_ball = self._transform_point(ball.center, field,
msg.header.stamp)
if transformed_ball is not None:
ball_relative = PoseWithCertainty()
ball_relative.pose.pose.position = transformed_ball
ball_relative.confidence = ball.confidence
balls.append(ball_relative)
balls_relative = PoseWithCertaintyArray()
balls_relative.header.stamp = msg.header.stamp
balls_relative.header.frame_id = self._publish_frame
balls_relative.poses = balls
self._balls_relative_pub.publish(balls_relative)
def _callback_goalposts(self, msg: GoalPostInImageArray):
field = self.get_plane(msg.header.stamp, 0.0)
if field is None:
return
bar_plane = self.get_plane(msg.header.stamp, self._bar_height)
if bar_plane is None:
return
# Create new message
goalposts_relative_msg = PoseWithCertaintyArray()
goalposts_relative_msg.header.stamp = msg.header.stamp
goalposts_relative_msg.header.frame_id = self._publish_frame
# Transform goal posts
for goal_post_in_image in msg.posts:
# Check if footpoint is not in the bottom area of the image, to filter out goal posts without visible footpoint
image_vertical_resolution = self._camera_info.height / max(
self._camera_info.binning_y, 1)
# Check if post is not going out of the image at the bottom
if not self._object_at_bottom_of_image(
goal_post_in_image.foot_point.y,
self._goalpost_footpoint_out_of_image_threshold):
# Transform footpoint
relative_foot_point = self._transform_point(
goal_post_in_image.foot_point, field, msg.header.stamp)
if relative_foot_point is None:
rospy.logwarn_throttle(
5.0,
rospy.get_name() +
": Got a post with foot point ({},{}) I could not transform."
.format(goal_post_in_image.foot_point.x,
goal_post_in_image.foot_point.y))
else:
post_relative = PoseWithCertainty()
post_relative.pose.pose.position = relative_foot_point
post_relative.confidence = goal_post_in_image.confidence
goalposts_relative_msg.poses.append(post_relative)
self._goalposts_relative.publish(goalposts_relative_msg)
def publish_marker(self, e):
# construct PoseWithCertaintyArray() message for map frame
ball_absolute = PoseWithCertainty()
ball_absolute.pose.pose = self.pose
ball_absolute.confidence = 1.0
balls_absolute = PoseWithCertaintyArray()
balls_absolute.header.stamp = rospy.get_rostime()
balls_absolute.header.frame_id = "map"
balls_absolute.poses = [ball_absolute]
# publish the new ball position
if self.publish:
self.absolute_publisher.publish(balls_absolute)
# check if ball is also visible for the robot and publish on relative topic if this is the case
# only works if camera info is provided
if self.cam_info:
try:
ball_point_stamped = PointStamped()
ball_point_stamped.header.stamp = rospy.Time.now()
ball_point_stamped.header.frame_id = "map"
ball_point_stamped.point = ball_absolute.pose.pose.position
ball_in_camera_optical_frame = tf_buffer.transform(
ball_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
if ball_in_camera_optical_frame.point.z >= 0:
p = [
ball_in_camera_optical_frame.point.x,
ball_in_camera_optical_frame.point.y,
ball_in_camera_optical_frame.point.z
]
k = np.reshape(self.cam_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.cam_info["width"] and 0 < p_pixel[
1] <= self.cam_info["height"]:
ball_in_footprint_frame = tf_buffer.transform(
ball_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
ball_relative = PoseWithCertainty()
ball_relative.pose.pose.position = ball_in_footprint_frame.point
ball_relative.confidence = 1.0
balls_relative = PoseWithCertaintyArray()
balls_relative.header = ball_in_footprint_frame.header
balls_relative.poses = [ball_relative]
self.relative_publisher.publish(balls_relative)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
def _dynamic_reconfigure_callback(self, config, level):
"""
Callback for the dynamic reconfigure configuration.
:param config: New _config
:param level: The level is a definable int in the Vision.cfg file. All changed params are or ed together by dynamic reconfigure.
"""
# creates kalmanfilter with 4 dimensions
self.kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=0)
self.filter_initialized = False
self.ball = None # type: PointStamped
self.ball_header = None # type: Header
self.last_ball_msg = PoseWithCertainty() # type: PoseWithCertainty
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'])
self.filter_reset_distance = config['filter_reset_distance']
filter_frame = config.get('filter_frame')
if filter_frame == "odom":
self.filter_frame = rospy.get_param('~odom_frame')
elif filter_frame == "map":
self.filter_frame = rospy.get_param('~map_frame')
rospy.loginfo(f"Using frame '{self.filter_frame}' for ball filtering",
logger_name="ball_filter")
# adapt velocity factor to frequency
self.velocity_factor = (1 - config['velocity_reduction'])**(
1 / self.filter_rate)
self.process_noise_variance = config['process_noise_variance']
# publishes positions of ball
self.ball_pose_publisher = rospy.Publisher(
config['ball_position_publish_topic'],
PoseWithCovarianceStamped,
queue_size=1)
# publishes velocity of ball
self.ball_movement_publisher = rospy.Publisher(
config['ball_movement_publish_topic'],
TwistWithCovarianceStamped,
queue_size=1)
# publishes ball
self.ball_publisher = rospy.Publisher(config['ball_publish_topic'],
PoseWithCertaintyStamped,
queue_size=1)
# setup subscriber
self.subscriber = rospy.Subscriber(config['ball_subscribe_topic'],
PoseWithCertaintyArray,
self.ball_callback,
queue_size=1)
self.reset_service = rospy.Service(
config['ball_filter_reset_service_name'], Trigger,
self.reset_filter_cb)
self.config = config
self.filter_timer = rospy.Timer(rospy.Duration(self.filter_time_step),
self.filter_step)
return config
min_x = 0.3
max_y = 10
min_y = -10
max_z = 0
min_z = 0
if __name__ == "__main__":
rclpy.init(args=None)
balls_relative_publisher = self.create_publisher(PoseWithCertaintyArray, "balls_relative", 10, tcp_nodelay=True)
x = random.uniform(min_x, max_x) / 4
y = random.uniform(min_y, max_y) / 4
z = random.uniform(min_z, max_z) / 4
while rclpy.ok():
ball_msg = PoseWithCertainty()
if len(sys.argv) is in [3, 4]:
ball_msg.pose.pose.position.x = float(sys.argv[1])
ball_msg.pose.pose.position.y = float(sys.argv[2])
if len(sys.argv) == 4:
ball_msg.pose.pose.position.z= float(sys.argv[3])
else:
ball_msg.ball_relative.x = x
ball_msg.ball_relative.y = y
ball_msg.ball_relative.z = z
# New position for next step
x = max(min_x, min(x + delta * random.uniform(-1, 1), max_x))
y = max(min_y, min(y + delta * random.uniform(-1, 1), max_y))
z = max(min_z, min(z + delta * random.uniform(-1, 1), max_z))
"""
This script publishes dummy values for ball, goalpost, position and obstacles for testing the team communication.
"""
import rospy
from geometry_msgs.msg import PoseWithCovariance
from humanoid_league_msgs.msg import ObstacleRelativeArray, ObstacleRelative, PoseWithCertaintyArray, PoseWithCertainty
if __name__ == '__main__':
rospy.init_node("TeamCommTest")
ball_pub = rospy.Publisher("balls_relative", PoseWithCertaintyArray, queue_size=1)
position_pub = rospy.Publisher("pose_with_certainty", PoseWithCertainty, queue_size=1)
obstacle_pub = rospy.Publisher("obstacles_relative", ObstacleRelativeArray, queue_size=1)
position_msg = PoseWithCertainty()
position_msg.pose.pose.position.x = 2
position_msg.confidence = 0.7
obstacle_msg = ObstacleRelativeArray()
obstacle = ObstacleRelative()
obstacle.pose.pose.pose.position.x = 4
obstacle.type = 2
obstacle_msg.obstacles.append(obstacle)
obstacle2 = ObstacleRelative()
obstacle2.pose.pose.pose.position.x = 2
obstacle2.type = 2
obstacle_msg.obstacles.append(obstacle2)
ball_msg = PoseWithCertaintyArray()
ball = PoseWithCertainty()
ball.confidence = 1.0
ball_position = PoseWithCovariance()
def state_update(state_msg):
global ball_pose
global goal_post_1_pose
global goal_post_2_pose
global tf_buffer
global cam_info
global ball_pub
global goal_posts_pub
if not cam_info:
return
ball_indices = []
for i, name in enumerate(state_msg.name):
if name == "teensize_ball":
ball_indices.append(i)
post_index1 = 0
post_index2 = 0
for name in state_msg.name:
if name == "teensize_goal":
if post_index1 == 0:
post_index1 = post_index2
else:
break
post_index2 += 1
ball_msgs = []
for ball_index in ball_indices:
ball_pose = state_msg.pose[ball_index]
ball_pose_stamped = PoseStamped()
ball_pose_stamped.header.stamp = rospy.Time.now()
ball_pose_stamped.header.frame_id = "map"
ball_pose_stamped.pose = ball_pose
ball_in_camera_optical_frame = tf_buffer.transform(
ball_pose_stamped,
cam_info.header.frame_id,
timeout=rospy.Duration(0.5))
# we only have to compute if the ball is inside the image, if the ball is in front of the camera
if ball_in_camera_optical_frame.pose.position.z >= 0:
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(cam_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.
# also z has to be positive to make sure that the ball is in front of the camera and not in the back
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
ball_in_footprint_frame = tf_buffer.transform(
ball_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
ball_relative = PoseWithCertainty()
ball_relative.pose.pose = ball_in_footprint_frame.pose.position
ball_relative.confidence = 1.0
ball_msgs.append(ball_relative)
balls_relative_msg = PoseWithCertaintyArray()
balls_relative_msg.header = ball_in_footprint_frame.header
balls_relative_msg.poses = ball_msgs
ball_pub.publish(balls_relative_msg)
goal = PoseWithCertaintyArray()
goal_post_1_pose = state_msg.pose[post_index1]
goal_post_2_pose = state_msg.pose[post_index2]
for gp in (goal_post_1_pose, goal_post_2_pose):
goal_post_stamped = PoseWithCertainty()
goal_post_stamped.pose.pose = gp
left_post = deepcopy(goal_post_stamped)
left_post.pose.pose.position.y += 1.35
left_post_in_camera_optical_frame = tf_buffer.transform(
left_post.pose,
cam_info.header.frame_id,
timeout=rospy.Duration(0.5))
if left_post_in_camera_optical_frame.pose.position.z >= 0:
p = [
left_post_in_camera_optical_frame.pose.position.x,
left_post_in_camera_optical_frame.pose.position.y,
left_post_in_camera_optical_frame.pose.position.z
]
k = np.reshape(cam_info.K, (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
left_post = PoseWithCertainty()
left_post.confidence = 1
left_post.pose = tf_buffer.transform(
left_post.pose,
"base_footprint",
timeout=rospy.Duration(0.5)).pose.position
goal.poses.append(left_post)
right_post = goal_post_stamped
right_post.pose.position.y -= 1.35
right_post_in_camera_optical_frame = tf_buffer.transform(
right_post, cam_info.header.frame_id, timeout=rospy.Duration(0.5))
if right_post_in_camera_optical_frame.pose.position.z >= 0:
p = [
right_post_in_camera_optical_frame.pose.position.x,
right_post_in_camera_optical_frame.pose.position.y,
right_post_in_camera_optical_frame.pose.position.z
]
k = np.reshape(cam_info.K, (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[
1] <= cam_info.height:
right_post = PoseWithCertainty()
right_post.confidence = 1
right_post.pose = tf_buffer.transform(
right_post_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5)).pose.position
goal.poses.append(right_post)
if len(goal.poses) > 0:
goal_posts_pub.publish(goal)
def publish_marker(self, e):
# construct GoalRelative message
goal_absolute = PoseWithCertaintyArray()
goal_absolute.header.stamp = rospy.get_rostime()
goal_absolute.header.frame_id = "map"
# calculate the positions of the right and the left post
orientation = euler_from_quaternion(
(self.pose.orientation.x, self.pose.orientation.y,
self.pose.orientation.z, self.pose.orientation.w))
angle = orientation[2]
left_post = PoseWithCertainty()
left_post.pose.pose.position.x = self.pose.position.x - math.sin(
angle) * GOAL_WIDTH / 2
left_post.pose.pose.position.y = self.pose.position.y + math.cos(
angle) * GOAL_WIDTH / 2
left_post.confidence = 1.0
right_post = PoseWithCertainty()
right_post.pose.pose.position.x = self.pose.position.x + math.sin(
angle) * GOAL_WIDTH / 2
right_post.pose.pose.position.y = self.pose.position.y - math.cos(
angle) * GOAL_WIDTH / 2
right_post.confidence = 1.0
goal_absolute.poses.append(left_post)
goal_absolute.poses.append(right_post)
# publish the new goal position
if self.publish:
self.absolute_publisher.publish(goal_absolute)
# check if goal is also visible for the robot and publish on relative topic if this is the case
# only works if camera info is provided
if self.cam_info:
try:
goal_relative = PoseWithCertaintyArray()
goal_relative.header.stamp = rospy.Time.now()
goal_relative.header.frame_id = "base_footprint"
goal_left_point_stamped = PointStamped()
goal_left_point_stamped.header.stamp = goal_relative.header.stamp
goal_left_point_stamped.header.frame_id = "map"
goal_left_point_stamped.point = left_post.pose.pose.position
left_post_in_camera_optical_frame = tf_buffer.transform(
goal_left_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
lpost_visible = False
rpost_visible = False
if left_post_in_camera_optical_frame.point.z >= 0:
p = [
left_post_in_camera_optical_frame.point.x,
left_post_in_camera_optical_frame.point.y,
left_post_in_camera_optical_frame.point.z
]
k = np.reshape(self.cam_info["K"], (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= self.cam_info["width"] and 0 < p_pixel[
1] <= self.cam_info["height"]:
left_post = PoseWithCertainty()
left_post.pose.pose.position = tf_buffer.transform(
left_post_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5)).point
goal_relative.poses.append(left_post)
lpost_visible = True
goal_right_point_stamped = PointStamped()
goal_right_point_stamped.header.stamp = goal_relative.header.stamp
goal_right_point_stamped.header.frame_id = "map"
goal_right_point_stamped.point = right_post.pose.pose.position
right_post_in_camera_optical_frame = tf_buffer.transform(
goal_right_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
if right_post_in_camera_optical_frame.point.z >= 0:
p = [
right_post_in_camera_optical_frame.point.x,
right_post_in_camera_optical_frame.point.y,
right_post_in_camera_optical_frame.point.z
]
k = np.reshape(self.cam_info["K"], (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
if 0 < p_pixel[0] <= self.cam_info["width"] and 0 < p_pixel[
1] <= self.cam_info["height"]:
right_post = PoseWithCertainty()
right_post.pose.pose.position = tf_buffer.transform(
right_post_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5)).point
goal_relative.poses.append(right_post)
rpost_visible = True
# publish goal relative msg
if rpost_visible or lpost_visible:
self.relative_publisher.publish(goal_relative)
self.relative_posts_publisher.publish(goal_relative)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None