def transform(self, value):
point_new = geometry_msgs.msg.PointStamped()
point_new.header = value.req.header
point_new.point = value.req.point
transformed = self.tfBuffer.transform(point_new, self.camera_model.tfFrame())
transformed_new = PointStamped()
transformed_new.header = transformed.header
transformed_new.point = transformed.point
return transformed_new
def detect_fun(self):
found = False
sherd_poses = [] # initialize empty
# confirm that color mask exists
if self.color_mask is None:
rospy.logwarn('AutoCore: No color mask received.')
return found, sherd_poses
# run segment_sherds.py on what robot sees in this position
req = SherdDetectionsRequest()
req.color_mask = self.color_mask
try:
res = self.detection_srv(req)
except rospy.ServiceException as e:
rospy.logerr(
'AutoCore: Bounding box service call failed: {}'.format(e))
raise
else:
detections = res.detections.detections
rospy.logdebug('Bounding boxes message: {}'.format(detections))
if detections:
found = True
# TODO return (first or random or smallest etc.) sherd from list
for item in detections:
point_cam = PointStamped() # build ROS message for conversion
point_cam.header = item.header
point_cam.point.x, point_cam.point.y, point_cam.point.z = item.bbox.center.position.x, item.bbox.center.position.y, item.bbox.center.position.z
try:
point_base = self.tfBuffer.transform(
point_cam, 'base_link')
except tf2_ros.buffer_interface.TypeException as e:
rospy.logerr('AutoCore: tf failure: {}'.format(e))
raise
rospy.logdebug(
'Obtained transform between camera_link and base_link.')
rospy.logdebug(
'Sherd center point (x,y,z) [m] in base_link frame: {}'.
format(point_base))
sherd_poses.append({
'position':
np.array([
point_base.point.x, point_base.point.y,
point_base.point.z
]),
'roll':
item.bbox.center.roll
})
rospy.logdebug('sherds list = {}'.format(sherd_poses))
return found, sherd_poses
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.tf_buffer.transform(
point_a, self.map_frame, timeout=Duration(seconds=0.3))
point_b = self.tf_buffer.transform(
point_b, self.map_frame, timeout=Duration(seconds=0.3))
# 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.get_logger().warn(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 msg_to_pos(tf_buffer, msg, to_frame_id, scale=1.0):
if msg is None:
return None
ps = PointStamped()
ps.header = msg.header
ps.point.x = msg.pose.pose.position.x * scale
ps.point.y = msg.pose.pose.position.y * scale
ps.point.z = msg.pose.pose.position.z * scale
try:
ps2 = tf_buffer.transform(ps, to_frame_id)
except Exception as e:
rospy.logwarn('Exception during TF from {:s} to {:s}: {:s}'.format(
msg.header.frame_id, to_frame_id, e))
return None
return np.array([ps2.point.x, ps2.point.y, ps2.point.z])
def balls_relative_cb(self, msg):
if msg.poses:
balls = sorted(msg.poses, reverse=True, key=lambda ball: ball.confidence) # Sort all balls by confidence
ball = balls[0] # Ball with highest confidence
# Get ball relative to base footprint
ball_obj = PointStamped()
ball_obj.header = msg.header
ball_obj.point = ball
x, y = self.get_ball_position_uv(ball_obj)
self.ball_position = (float(x), float(y))
# Set ball distance
self.ball_distance = math.sqrt(float(self.ball_position[0])**2 + float(self.ball_position[1])**2)
# Calculate the angle in which we are facing the ball
self.ball_angle = math.atan2(float(self.ball_position[1]),float(self.ball_position[0]))
# Set the refresh time
self.ball_age = time.time()
else:
rospy.logwarn("Received empty balls message")
