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