def transform(self, point, field, stamp):
K = self.camera_info.K
x = (point.x - K[2]) / K[0]
y = (point.y - K[5]) / K[4]
z = 1.0
point_on_image = np.array([x, y, z])
intersection = line_plane_intersection(field[0], field[1],
point_on_image)
if intersection is None:
return None
intersection_stamped = PointStamped()
intersection_stamped.point = intersection
intersection_stamped.header.stamp = stamp
intersection_stamped.header.frame_id = self.camera_info.header.frame_id
try:
intersection_transformed = self.tf_buffer.transform(
intersection_stamped, self.publish_frame)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(
5.0, "Could not transform from " + self.publish_frame +
" to " + intersection_stamped.header.frame_id)
rospy.logwarn_throttle(5.0, ex)
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(
5.0, "Waiting for transforms to become available...")
rospy.logwarn_throttle(5.0, ex)
return None
return intersection_transformed.point
def _projectPointIntoFrame(self, point, source_frame_id, target_frame_id):
"""!
@brief Transform a point from one frame of reference to another.
@param point The geometry_msgs/Point to transform.
@param source_frame_id The fully resolved frame_id the point is in reference to.
@param target_frame_id The fully resolved frame_id to which the point should be
transformed.
@return The geometry_msgs/Point in the new frame of reference.
"""
# First, create a PointStamped, since that is what is needed to transform.
point_source_stamped = PointStamped()
point_source_stamped.header.stamp = rospy.Time.now()
point_source_stamped.header.frame_id = source_frame_id
point_source_stamped.point = point
# Do the transform. Give a timeout to let it wait for the latest TF if it would
# otherwise need to extrapolate.
timeout = rospy.Duration(self._tf_timeout)
try:
point_target_stamped = self._tf_buffer.transform(
object_stamped=point_source_stamped,
target_frame=target_frame_id,
timeout=timeout)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as error:
rospy.logerr(msg='TF transform error: {error}'.format(error=error))
raise
return point_target_stamped.point
def obstacle_cb(self, msg):
for item in self.obstacles:
self._scene.removeItem(item)
for obstacle_msg in msg.obstacles:
obstacle = QGraphicsEllipseItem(0, 0, self.obsacle_size,
self.obsacle_size, self.radar)
obstacle.setPen(self.obstacle_pen)
obstacle.setOpacity(self.opacity)
obstacle_point = PointStamped()
obstacle_point.header.frame_id = msg.header.frame_id
obstacle_point.header.stamp = msg.header.stamp
obstacle_point.point = obstacle_msg.pose.pose.pose.position
try:
obstacle_point = self.tf_buffer.transform(
obstacle_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " +
msg.header.frame_id + " to 'base_footprint'")
return None
self.set_scaled_position(obstacle, -1 * obstacle_point.point.x,
-1 * obstacle_point.point.y,
self.obsacle_size, self.obsacle_size)
color_tuple = self.confidence2color(obstacle_msg.pose.confidence)
obstacle.setBrush(QBrush(QColor(*color_tuple)))
obstacle.setVisible(True)
self.obstacles.append(obstacle)
def balls_cb(self, msg):
ball_msgs = msg.poses
for ball_msg in ball_msgs:
ball_point = PointStamped()
ball_point.header.frame_id = msg.header.frame_id
ball_point.header.stamp = msg.header.stamp
ball_point.point = ball_msg.pose.pose.position
try:
ball_point = self.tf_buffer.transform(ball_point,
"base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " +
msg.header.frame_id + " to 'base_footprint'")
return None
ball = QGraphicsEllipseItem(0, 0, self.ball_size, self.ball_size,
self.radar)
ball.setPen(self.ball_pen)
ball.setVisible(False)
ball.setOpacity(self.opacity)
self.set_scaled_position(ball, -1 * ball_point.point.x,
-1 * ball_point.point.y, self.ball_size,
self.ball_size)
color_tuple = self.confidence2color(ball_msg.confidence)
ball.setBrush(QBrush(QColor(*color_tuple)))
ball.setVisible(self.ball_active)
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 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 write_circles(self, data):
object_frame = rospy.get_param("~object_frame")
global_frame = rospy.get_param(
"~global_frame") #NE MOZE MAPA BITI OBJECT...
tf = self.tfBuffer.lookup_transform(global_frame, object_frame,
rospy.Time(0))
orientation = tf.transform.rotation
quat_list = [
orientation.x, orientation.y, orientation.z, orientation.w
]
(roll, pitch, yaw) = euler_from_quaternion(quat_list)
# print yaw
newData = Obstacles() #for publishing transformed circles
newData.circles = []
newData.header = data.header
newData.header.frame_id = global_frame
self.circles = []
temp_point_stamped = PointStamped()
for circle in data.circles:
# x = self.pioneerPose.position.x + math.cos(yaw) * circle.center.x + math.sin(yaw) * circle.center.y
# y = self.pioneerPose.position.y - math.sin(yaw) * circle.center.x + math.cos(yaw) * circle.center.y
# circle.center.x = x
# circle.center.y = y
temp_point_stamped.point = circle.center
new_center = tf2_geometry_msgs.do_transform_point(
temp_point_stamped, tf)
circle.center = new_center.point
self.circles.append(circle)
newData.circles.append(circle)
self.transformed_obstacles.publish(newData)
self.circles = data.circles
max_num_of_no_actor = rospy.get_param("~max_num_of_no_actor")
self.header = data.header
self.header.frame_id = global_frame
if self.actor != None:
self.update_actor()
if self.count >= max_num_of_no_actor:
self.stop_robot()
rospy.signal_shutdown("Actor has been lost!!!")
def draw_goal_part(self, obj, msg, point, size):
goal_point = PointStamped()
goal_point.header.frame_id = msg.header.frame_id
goal_point.header.stamp = msg.header.stamp
goal_point.point = point
try:
goal_point = self.tf_buffer.transform(goal_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " + msg.header.frame_id +
" to 'base_footprint'")
return None
self.set_scaled_position(obj, -1 * goal_point.point.x,
-1 * goal_point.point.y, size, size)
color_tuple = self.confidence2color(msg.confidence)
obj.setBrush(QBrush(QColor(*color_tuple)))
obj.setVisible(self.goal_active)
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")
def ball_cb(self, msg):
ball_point = PointStamped()
ball_point.header.frame_id = msg.header.frame_id
ball_point.header.stamp = msg.header.stamp
ball_point.point = msg.ball_relative
try:
ball_point = self.tf_buffer.transform(ball_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " + msg.header.frame_id +
" to 'base_footprint'")
return None
if msg.ball_relative is not None:
self.set_scaled_position(self.ball, -1 * ball_point.point.x,
-1 * ball_point.point.y, self.ball_size,
self.ball_size)
color_tuple = self.confidence2color(msg.confidence)
self.ball.setBrush(QBrush(QColor(*color_tuple)))
self.ball.setVisible(self.ball_active)
def get_relative_msg(self):
# check if obstacle is also visible for the robot and only then return
if self.cam_info:
try:
obstacle_point_stamped = PointStamped()
obstacle_point_stamped.header.stamp = rospy.Time.now()
obstacle_point_stamped.header.frame_id = "map"
obstacle_point_stamped.point = self.pose.position
obstacle_in_camera_optical_frame = tf_buffer.transform(
obstacle_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
if obstacle_in_camera_optical_frame.point.z >= 0:
p = [
obstacle_in_camera_optical_frame.point.x,
obstacle_in_camera_optical_frame.point.y,
obstacle_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"]:
obstacle_relative = ObstacleRelative()
ball_in_footprint_frame = tf_buffer.transform(
obstacle_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
obstacle_relative.pose.pose.pose.position = ball_in_footprint_frame.point
obstacle_relative.pose.confidence = self.confidence
obstacle_relative.playerNumber = self.player_number
obstacle_relative.type = self.type
return obstacle_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
return None
def perform(self, connector, reevaluate=False):
goal_u, goal_v = connector.world_model.get_opp_goal_center_uv()
goal = PointStamped()
goal.header.frame_id = 'base_footprint'
goal.point = Point(goal_u, goal_v, 0)
self.look_at(goal, connector)
print("Motor positions {}, {}".format(head_pan, head_tilt))
pos_msg.positions = [head_pan, head_tilt]
pos_msg.header.stamp = rospy.Time.now()
publish_motor_goals.publish(pos_msg)
# Sleep 1 to wait for head being at its position
sleep(1)
###### Validation ######
# Reverse read position where the robot is looking at
# Get line of camera
camera_origin = PointStamped()
camera_origin.header.stamp = rospy.Time.now()
camera_origin.header.frame_id = 'camera'
camera_origin.point = Point(0, 0, 0)
camera_one = PointStamped()
camera_one.header.stamp = rospy.Time.now()
camera_one.header.frame_id = 'camera'
camera_one.point = Point(1, 0, 0)
camera_origin_bf = tf_buffer.transform(
camera_origin, 'base_footprint', timeout=rospy.Duration(0.3)).point
camera_one_bf = tf_buffer.transform(camera_one,
'base_footprint',
timeout=rospy.Duration(0.3)).point
line_vector = Point(camera_one_bf.x - camera_origin_bf.x,
camera_one_bf.y - camera_origin_bf.y,
camera_one_bf.z - camera_origin_bf.z)
# Calculate where it touches the plane of base_footprint
# line will be described as camera_one_bf + u * line_vector, where u is a scalar
# now the line should equal (x y 0)
def removeOutsideRobot(mask, robot, tf_buffer, camera_frame_id, camera_info):
"""!
@brief Removes everything in the provided mask that is not within the bounds
of the robot.
Each mask is only on a single robot. If you project the vertices of the bounding
shape onto the image, there cannot possibly be any part of the mask outside of the
containing rectangle on the image. This provides a simple, but powerful filter for
the image.
@param mask The mask to filter.
@param robot The robot currently being segmented in the image.
@param tf_buffer A TF buffer to use to lookup point transforms. This is required
to project the robot's bounding shape points into the camera.
@param camera_frame_id The name of the frame in TF used by the camera.
@param camera_info The camera_info ROS message with camera calibration values.
@return An image with everything outside of the robot's area removed.
"""
# First, get the bounding shape of the robot, which is a list of point mesages.
bounding_shape = robot.getBoundingShape()
# Project each point onto the image.
points_image = numpy.zeros(shape=(len(bounding_shape), 2))
count = 0
for point in bounding_shape:
# TF needs a stamp to transform appropriately.
point_source_stamped = PointStamped()
point_source_stamped.header.stamp = rospy.Time.now()
point_source_stamped.header.frame_id = robot.getFullFrame()
point_source_stamped.point = point
# Transform into the camera
try:
point_target_stamped = tf_buffer.transform(
object_stamped=point_source_stamped, target_frame=camera_frame_id, timeout=rospy.Duration(2.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as error:
rospy.logerr(msg='TF transform error: {error}'.format(error=error))
raise
# Project onto the image.
point_target_numpy = numpy.zeros(shape=(1, 3))
point_target_numpy[0][0] = point_target_stamped.point.x
point_target_numpy[0][1] = point_target_stamped.point.y
point_target_numpy[0][2] = point_target_stamped.point.z
intrinsic = numpy.array(object=camera_info.K).reshape(3, 3)
zeros = numpy.zeros(shape=(3, 1))
(point_image, _) = cv2.projectPoints(objectPoints=point_target_numpy,
rvec=zeros, tvec=zeros, cameraMatrix=intrinsic, distCoeffs=None)
point_image = numpy.squeeze(point_image)
# Extract the point and save for comparison with the rest.
points_image[count, :] = point_image
count += 1
# Determine the outer bounds on the images to get the bounding rectangle.
# These are used as pixel indices, so need to be ints.
mins = points_image.min(axis=0)
mins = numpy.floor(mins)
mins = mins.astype(dtype=numpy.int16)
maxs = points_image.max(axis=0)
maxs = numpy.ceil(maxs)
maxs = maxs.astype(dtype=numpy.int16)
# Use that rectangle to create an image mask
new_image = numpy.zeros_like(mask)
new_image[mins[1]:maxs[1], mins[0]:maxs[0]
] = mask[mins[1]:maxs[1], mins[0]:maxs[0]]
return new_image
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
