From 11f8d9d59520d26fdfb71bd1d61e8f3ebdc1a098 Mon Sep 17 00:00:00 2001 From: Jonathan Spraggett Date: Wed, 31 Jul 2024 15:54:56 -0400 Subject: [PATCH] Js refactor perception (#874) * wip refactor * made a ros split for fieldline detection, objectdetection, camera. first refactor of detector_fieldline, object+detect_node. deleated goal post detection and will use yolov5 as that is much more accurate. split the tests and got most of them working. got ros unit tests to work. * moved some functions and removed ready from camera. put all testing data into data folder. fixed all the pathways for downloading datasets and models. getting rid of detectir objects since some of it should be in strategy and others in object detection node. Made a utils file for common functions. finished all documentation other then functionaly analysis since these modules are too small. Good enough for now until we rework certain things * fixed the ros versions so they can be used in sim --- .dockerignore | 1 + .gitignore | 5 +- soccer_common/src/soccer_common/__init__.py | 2 +- soccer_common/src/soccer_common/camera.py | 358 ------------ soccer_common/src/soccer_common/camera_old.py | 387 ------------- .../src/soccer_common/perception/__init__.py | 0 .../soccer_common/perception/camera_base.py | 104 ++++ .../perception/camera_calculations.py | 216 +++++++ .../perception/camera_calculations_ros.py | 87 +++ .../src/soccer_common/test_common.py | 42 -- soccer_common/test/test_camera.py | 77 +++ soccer_common/test/test_common.py | 14 + .../.ipynb_checkpoints/fhgj-checkpoint.ipynb | 160 ------ .../soccer_pycontrol/exp/double_pendulum.xml | 44 ++ .../src/soccer_pycontrol/exp/mujoco_view.py | 60 ++ soccer_perception/data/.gitkeep | 0 ...0.19657546078618005_-1.049088190788794.png | 3 - ...0.18866567631476938_1.5249962322470259.png | 3 - ...7_1.629702800190004_-2.915223713456875.png | 3 - ...2.330417857677584_-0.15655553984103276.png | 3 - ..._-0.168873476816235_1.9748205753546078.png | 3 - ...2.4511358573282696_-0.8450607115290163.png | 3 - ...-0.5816355306389931_-0.502654929457147.png | 3 - ...-1.5254301032646078_-2.984049153805578.png | 3 - ...-0.8891447062075253_-2.658362073693127.png | 3 - ...0.9547434357596831_0.22595902027652404.png | 3 - ...1.6704487675687467_-1.7207161315253598.png | 3 - ...0.7841084784936343_-0.3400951076777976.png | 3 - ..._1.495115016609354_-0.5027702268566427.png | 3 - ..._-2.003114586611703_2.5573300113397712.png | 3 - ....9692340250080882_-0.04040952933386244.png | 3 - ...-1.973872686616836_-2.1962339783923808.png | 3 - ..._0.5160025182546568_1.0373781171521408.png | 3 - ....4566688643207222_-0.38770547262037836.png | 3 - .../doc/ClassDiagrams.md | 40 ++ .../soccer_object_detection/doc/FlowCharts.md | 40 ++ .../soccer_object_detection/doc/README.md | 4 + .../src/soccer_object_detection/dfsv.py | 23 - .../object_detect_node.py | 145 +---- .../object_detect_node_ros.py | 109 ++++ .../test/test_object_detection.py | 356 ++++++------ .../test/test_object_detection_ros.py | 89 +++ .../soccer_object_detection/test/utils.py | 56 ++ .../doc/ClassDiagrams.md | 42 ++ .../doc/FlowCharts.md | 45 ++ .../soccer_object_localization/doc/README.md | 5 + .../soccer_object_localization/detector.py | 30 +- .../detector_fieldline.py | 151 +---- .../detector_fieldline_ros.py | 121 ++++ .../detector_goalpost.py | 167 ------ .../detector_objects.py | 314 +++++----- .../test/test_camera.py | 51 -- .../test/test_object_localization.py | 535 +++--------------- .../test/test_object_localization_ros.py | 63 +++ .../communication/game_controller_receiver.py | 2 +- .../old/robot_controlled_2d.py | 2 +- soccerbot/launch/modules/localization.launch | 56 +- soccerbot/launch/soccerbot.launch | 170 +++--- soccerbot/launch/soccerbot_multi.launch | 36 +- test_integration.py | 4 +- 60 files changed, 1852 insertions(+), 2415 deletions(-) delete mode 100644 soccer_common/src/soccer_common/camera.py delete mode 100644 soccer_common/src/soccer_common/camera_old.py rename soccer_perception/soccer_object_detection/videos/.gitkeep => soccer_common/src/soccer_common/perception/__init__.py (100%) create mode 100644 soccer_common/src/soccer_common/perception/camera_base.py create mode 100644 soccer_common/src/soccer_common/perception/camera_calculations.py create mode 100644 soccer_common/src/soccer_common/perception/camera_calculations_ros.py delete mode 100644 soccer_common/src/soccer_common/test_common.py create mode 100644 soccer_common/test/test_camera.py create mode 100644 soccer_common/test/test_common.py delete mode 100644 soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/.ipynb_checkpoints/fhgj-checkpoint.ipynb create mode 100644 soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/double_pendulum.xml create mode 100644 soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/mujoco_view.py create mode 100644 soccer_perception/data/.gitkeep delete mode 100644 soccer_perception/images/fieldlines/img0_-0.6805418953941712_-0.19657546078618005_-1.049088190788794.png delete mode 100644 soccer_perception/images/fieldlines/img10_0.8641847711947435_0.18866567631476938_1.5249962322470259.png delete mode 100644 soccer_perception/images/fieldlines/img11_0.0425320918554577_1.629702800190004_-2.915223713456875.png delete mode 100644 soccer_perception/images/fieldlines/img12_1.109509427721366_2.330417857677584_-0.15655553984103276.png delete mode 100644 soccer_perception/images/fieldlines/img13_-1.0020274648257552_-0.168873476816235_1.9748205753546078.png delete mode 100644 soccer_perception/images/fieldlines/img14_-0.4107905429521215_2.4511358573282696_-0.8450607115290163.png delete mode 100644 soccer_perception/images/fieldlines/img15_-1.4659361337858274_-0.5816355306389931_-0.502654929457147.png delete mode 100644 soccer_perception/images/fieldlines/img16_-0.7853100428576383_-1.5254301032646078_-2.984049153805578.png delete mode 100644 soccer_perception/images/fieldlines/img1_0.031962395374173_-0.8891447062075253_-2.658362073693127.png delete mode 100644 soccer_perception/images/fieldlines/img2_-0.19940776631633783_0.9547434357596831_0.22595902027652404.png delete mode 100644 soccer_perception/images/fieldlines/img2_-1.170645594381017_1.6704487675687467_-1.7207161315253598.png delete mode 100644 soccer_perception/images/fieldlines/img3_-0.22778119664751917_0.7841084784936343_-0.3400951076777976.png delete mode 100644 soccer_perception/images/fieldlines/img4_1.4594846269094557_1.495115016609354_-0.5027702268566427.png delete mode 100644 soccer_perception/images/fieldlines/img5_-0.9405160632232356_-2.003114586611703_2.5573300113397712.png delete mode 100644 soccer_perception/images/fieldlines/img6_-1.1265115856779753_-0.9692340250080882_-0.04040952933386244.png delete mode 100644 soccer_perception/images/fieldlines/img7_-0.5656089451711844_-1.973872686616836_-2.1962339783923808.png delete mode 100644 soccer_perception/images/fieldlines/img8_1.1242042670726646_0.5160025182546568_1.0373781171521408.png delete mode 100644 soccer_perception/images/fieldlines/img9_-0.09214002310303626_-0.4566688643207222_-0.38770547262037836.png create mode 100644 soccer_perception/soccer_object_detection/doc/ClassDiagrams.md create mode 100644 soccer_perception/soccer_object_detection/doc/FlowCharts.md create mode 100644 soccer_perception/soccer_object_detection/doc/README.md delete mode 100644 soccer_perception/soccer_object_detection/src/soccer_object_detection/dfsv.py create mode 100755 soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py create mode 100644 soccer_perception/soccer_object_detection/test/test_object_detection_ros.py create mode 100644 soccer_perception/soccer_object_detection/test/utils.py create mode 100644 soccer_perception/soccer_object_localization/doc/ClassDiagrams.md create mode 100644 soccer_perception/soccer_object_localization/doc/FlowCharts.md create mode 100644 soccer_perception/soccer_object_localization/doc/README.md create mode 100755 soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline_ros.py delete mode 100755 soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_goalpost.py delete mode 100644 soccer_perception/soccer_object_localization/test/test_camera.py create mode 100644 soccer_perception/soccer_object_localization/test/test_object_localization_ros.py diff --git a/.dockerignore b/.dockerignore index 33a14ed8a..ff76a6300 100644 --- a/.dockerignore +++ b/.dockerignore @@ -19,3 +19,4 @@ soccerbot/bags/ LICENSE venv/ *.webm +/soccer_perception/data/images/* diff --git a/.gitignore b/.gitignore index 8235f78aa..2e9384efd 100644 --- a/.gitignore +++ b/.gitignore @@ -27,8 +27,9 @@ venv/ /docs/_build/ /docs/generated/ *.bag -/soccer_perception/soccer_object_localization/images/ -/soccer_perception/soccer_object_detection/images/* + +/soccer_perception/data/images/* +/soccer_perception/data/videos/* /soccer_strategy/src/soccer_strategy/log.txt .mypy_cache/ cuda-ubuntu2004.pin.1 diff --git a/soccer_common/src/soccer_common/__init__.py b/soccer_common/src/soccer_common/__init__.py index 9d7044031..5c340e227 100644 --- a/soccer_common/src/soccer_common/__init__.py +++ b/soccer_common/src/soccer_common/__init__.py @@ -2,7 +2,7 @@ from soccer_common.transformation2d import Transformation2D try: - from soccer_common.camera import Camera + from soccer_common.perception.camera_calculations import CameraCalculations from soccer_common.pid import PID except: print("not using camera") diff --git a/soccer_common/src/soccer_common/camera.py b/soccer_common/src/soccer_common/camera.py deleted file mode 100644 index 84b4b8850..000000000 --- a/soccer_common/src/soccer_common/camera.py +++ /dev/null @@ -1,358 +0,0 @@ -import math -from functools import cached_property - -import numpy as np - -from soccer_common.transformation import Transformation - - -class Camera: - # TODO maybe put into a common percpetion - # TODO also could be split up into different files too many things - HORIZONTAL_FOV = 1.39626 - - def __init__(self): - # TODO why does this need pose shouldnt all the calcualtions be in the robots relative frame and - # then transformed into the world frame that would make it a lot less relient on - # knowledge of its global position - self.pose = Transformation() - self.camera_info = None - self.horizontalFOV = Camera.HORIZONTAL_FOV - self.focal_length = 3.67 #: Focal length of the camera (meters) distance to the camera plane as projected in 3D - - def ready(self) -> bool: - """ - Function to determine when the camera object has recieved the necessary information and is ready to be used - - :return: True if the camera is ready, else False - """ - return self.pose is not None and self.resolution_x is not None and self.resolution_y is not None and self.camera_info is not None - - def reset_position(self, from_world_frame=False, camera_frame="/camera", skip_if_not_found=False): - # same hardcoded values - if from_world_frame: - trans = [0, 0, 0] - rot = [0, 0, 0, 1] - self.pose = Transformation(trans, rot) - else: - trans = [0, 0, 0] - rot = [0, 0, 0, 1] - self.pose = Transformation(trans, rot) - # TODO put in its own ros file - """ - - Resets the position of the camera, it uses a series of methods that fall back on each other to get the location of the camera - - :param from_world_frame: If this is set to true, the camera position transformation will be from the world instead of the robot odom frame - :param timestamp: What time do we want the camera tf frame, rospy.Time(0) if get the latest transform - :param camera_frame: The name of the camera frame - :param skip_if_not_found: If set to true, then will not wait if it cannot find the camera transform after the specified duration (1 second), it will just return - """ - """ - if from_world_frame: - try: - self.tf_listener.waitForTransform("world", self.robot_name + camera_frame, timestamp, rospy.Duration(nsecs=1000000)) - (trans, rot) = self.tf_listener.lookupTransform("world", self.robot_name + camera_frame, timestamp) - self.pose = Transformation(trans, rot) - return - except ( - tf2_py.LookupException, - tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException, - tf2_py.TransformException, - ) as ex: - rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") - pass - else: - - try: - # Find the odom to base_footprint and publish straight base footprint - self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp, rospy.Duration(secs=1)) - (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp) - world_to_base_link = Transformation(trans, rot) - e = world_to_base_link.orientation_euler - e[1] = 0 - e[2] = 0 - world_to_base_link.orientation_euler = e - self.pose_base_link_straight = world_to_base_link - - # Calculate the camera transformation - self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp, rospy.Duration(secs=1)) - (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp) - world_to_camera = Transformation(trans, rot) - - camera_to_base_link = scipy.linalg.inv(world_to_base_link) @ world_to_camera - - self.pose = camera_to_base_link - return - except ( - tf2_py.LookupException, - tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException, - tf2_py.TransformException, - ) as ex: - rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") - """ - - def findFloorCoordinate(self, pos: [int]) -> [int]: - """ - From a camera pixel, get a coordinate on the floor - - :param pos: The position on the screen in pixels (x, y) - :return: The 3D coordinate of the pixel as projected to the floor - """ - tx, ty = self.imageToWorldFrame(pos[0], pos[1]) - pixel_pose = Transformation(position=(self.focal_length, tx, ty)) - camera_pose = self.pose - pixel_world_pose = camera_pose @ pixel_pose - ratio = (camera_pose.position[2] - pixel_world_pose.position[2]) / self.pose.position[2] # TODO Fix divide by 0 problem - x_delta = (pixel_world_pose.position[0] - camera_pose.position[0]) / ratio - y_delta = (pixel_world_pose.position[1] - camera_pose.position[1]) / ratio - - return [x_delta + camera_pose.position[0], y_delta + camera_pose.position[1], 0] - - def findCameraCoordinate(self, pos: [int]) -> [int]: - """ - From a 3d position on the field, get the camera coordinate, opposite of :func:`~soccer_common.Camera.findFloorCoordinate` - - :param pos: The 3D coordinate of the object - :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera - """ - pos3d = Transformation(pos) - camera_pose = self.pose - pos3d_tr = np.linalg.inv(camera_pose) @ pos3d - - return self.findCameraCoordinateFixedCamera(pos3d_tr.position) - - def findCameraCoordinateFixedCamera(self, pos: [int]) -> [int]: - """ - Helper function for :func:`~soccer_common.Camera.findCameraCoordinate`, finds the camera coordinate if the camera were fixed at the origin - - :param pos: The 3D coordinate of the object - :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera and the camera is placed at the origin - """ - - pos = Transformation(pos) - - ratio = self.focal_length / pos.position[0] - - tx = pos.position[1] * ratio - ty = pos.position[2] * ratio - x, y = self.worldToImageFrame(tx, ty) - return [x, y] - - def imageToWorldFrame(self, pixel_x: int, pixel_y: int) -> tuple: - """ - From image pixel coordinates, get the coordinates of the pixel as if they have been projected ot the camera plane, which is - positioned at (0,0) in 3D world coordinates - https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163680c589a_0_0 - - :param pixel_x: x pixel of the camera - :param pixel_y: y pixel of the camera - :return: 3D position (X, Y) of the pixel in meters - """ - return ( - (self.resolution_x / 2.0 - (pixel_x + 0.5)) * self.pixelWidth, - (self.resolution_y / 2.0 - (pixel_y + 0.5)) * self.pixelHeight, - ) - - def worldToImageFrame(self, pos_x: float, pos_y: float) -> tuple: - """ - Reverse function for :func:`~soccer_common.Camera.imageToWorldFrame`, takes the 3D world coordinates of the camera plane - and returns pixels - - :param pos_x: X position of the pixel on the world plane in meters - :param pos_y: Y position of the pixel on the world plane in meters - :return: Tuple (x, y) of the pixel coordinates of in the image - """ - return ( - (self.resolution_x / 2.0 + pos_x / self.pixelWidth) - 0.5, - (self.resolution_y / 2.0 + pos_y / self.pixelHeight) - 0.5, - ) - - def calculateBoundingBoxesFromBall(self, ball_position: Transformation, ball_radius: float = 0.07): - """ - Takes a 3D ball transformation and returns the bounding boxes of the ball if seen on camera - - :param ball_position: 3D coordinates of the ball stored in the :class:`Transformation` format - :param ball_radius: The radious of the ball in centimeters - :return: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left - and bottom right of the bounding box respectively - """ - - camera_pose = self.pose - pos3d_tr = np.linalg.inv(camera_pose) @ ball_position - - x = pos3d_tr.position[0] - y = -pos3d_tr.position[1] - z = -pos3d_tr.position[2] - r = ball_radius - - theta_y = math.atan2(y, x) - dy = math.sqrt(x**2 + y**2) - phi_y = math.asin(r / dy) - - xy_far = [x - math.sin(theta_y + phi_y) * r, y + math.cos(theta_y + phi_y) * r] - xy_near = [x + math.sin(theta_y - phi_y) * r, y - math.cos(theta_y - phi_y) * r] - - theta_z = math.atan2(z, x) - dz = math.sqrt(x**2 + z**2) - phi_z = math.asin(r / dz) - - xz_far = [x - math.sin(theta_z + phi_z) * r, z + math.cos(theta_z + phi_z) * r] - xz_near = [x + math.sin(theta_z - phi_z) * r, z - math.cos(theta_z - phi_z) * r] - - ball_right_point = [xy_far[0], xy_far[1], z] - ball_left_point = [xy_near[0], xy_near[1], z] - ball_bottom_point = [xz_far[0], y, xz_far[1]] - ball_top_point = [xz_near[0], y, xz_near[1]] - - ball_left_point_cam = self.findCameraCoordinateFixedCamera(ball_left_point) - ball_right_point_cam = self.findCameraCoordinateFixedCamera(ball_right_point) - ball_top_point_cam = self.findCameraCoordinateFixedCamera(ball_top_point) - ball_bottom_point_cam = self.findCameraCoordinateFixedCamera(ball_bottom_point) - - left_border_x = ball_left_point_cam[0] - right_border_x = ball_right_point_cam[0] - top_border_y = ball_top_point_cam[1] - bottom_border_y = ball_bottom_point_cam[1] - - bounding_box = [[left_border_x, top_border_y], [right_border_x, bottom_border_y]] - - return bounding_box - - def calculateBallFromBoundingBoxes(self, ball_radius: float = 0.07, bounding_boxes: [float] = []) -> Transformation: - """ - Reverse function for :func:`~soccer_common.Camera.calculateBoundingBoxesFromBall`, takes the bounding boxes - of the ball as seen on the camera and return the 3D position of the ball assuming that the ball is on the ground - - :param ball_radius: The radius of the ball in meters - :param bounding_boxes: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left and bottom right of the bounding box respectively - :return: 3D coordinates of the ball stored in the :class:`Transformation` format - """ - - # bounding boxes [(y1, z1), (y2, z2)] - r = ball_radius - - y1 = bounding_boxes[0][0] - z1 = bounding_boxes[0][1] - y2 = bounding_boxes[1][0] - z2 = bounding_boxes[1][1] - - # Assuming the ball is a sphere, the bounding box must be a square, averaging the borders - ym = (y1 + y2) / 2 - zm = (z1 + z2) / 2 - length = z2 - z1 - width = y2 - y1 - y1 = ym - (width / 2) - z1 = zm - (length / 2) - y2 = ym + (width / 2) - z2 = zm + (length / 2) - - y1w, z1w = self.imageToWorldFrame(y1, z1) - y2w, z2w = self.imageToWorldFrame(y2, z2) - y1w = -y1w - z1w = -z1w - y2w = -y2w - z2w = -z2w - - f = self.focal_length - - theta_y1 = math.atan2(y1w, f) - theta_y2 = math.atan2(y2w, f) - - theta_yy = (theta_y2 - theta_y1) / 2 - theta_y = theta_y1 + theta_yy - - dy = r / math.sin(theta_yy) - - xy = (math.cos(theta_y) * dy, math.sin(theta_y) * dy) - - theta_z1 = math.atan2(z1w, f) - theta_z2 = math.atan2(z2w, f) - - theta_zz = (theta_z2 - theta_z1) / 2 - theta_z = theta_z1 + theta_zz - - dz = r / math.sin(theta_zz) - - xz = (math.cos(theta_z) * dz, math.sin(theta_z) * dz) - - ball_x = xy[0] - ball_y = xy[1] - ball_z = xz[1] - - tr = Transformation([ball_x, -ball_y, -ball_z]) - tr_cam = self.pose @ tr - - return tr_cam - - def calculateHorizonCoverArea(self) -> int: - """ - Given the camera's position, return the area that is covered by the horizon (that is not the field area) in pixels from the top position - :return: Pixel length from the top of the image to the point where it meets the horizon - """ - - pitch = self.pose.orientation_euler[1] - d = math.sin(pitch) * self.focal_length - - (r, h) = self.worldToImageFrame(0, -d) - return int(min(max(0, h), self.resolution_y)) - - # CACHED PROPERTIES ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - @cached_property - def resolution_x(self) -> int: - """ - The X resolution of the camera or the width of the screen in pixels - - :return: width in pixels - """ - return self.camera_info.width - - @cached_property - def resolution_y(self): - """ - The Y resolution of the camera or the height of the screen in pixels - - :return: height in pixels - """ - return self.camera_info.height - - @cached_property - def verticalFOV(self): - """ - The vertical field of vision of the camera. - See `Field of View `_ - """ - return 2 * math.atan(math.tan(self.horizontalFOV * 0.5) * (self.resolution_y / self.resolution_x)) - - @cached_property - def imageSensorHeight(self): - """ - The height of the image sensor (m) - """ - return math.tan(self.verticalFOV / 2.0) * 2.0 * self.focal_length - - @cached_property - def imageSensorWidth(self): - """ - The width of the image sensor (m) - """ - return math.tan(self.horizontalFOV / 2.0) * 2.0 * self.focal_length - - @cached_property - def pixelHeight(self): - """ - The height of a pixel in real 3d measurements (m) - """ - return self.imageSensorHeight / self.resolution_y - - @cached_property - def pixelWidth(self): - """ - The wdith of a pixel in real 3d measurements (m) - """ - return self.imageSensorWidth / self.resolution_x - pass diff --git a/soccer_common/src/soccer_common/camera_old.py b/soccer_common/src/soccer_common/camera_old.py deleted file mode 100644 index 0ad3cc67b..000000000 --- a/soccer_common/src/soccer_common/camera_old.py +++ /dev/null @@ -1,387 +0,0 @@ -from functools import cached_property - -import numpy as np -import rospy -import scipy -import tf -import tf2_py -from rospy import Subscriber -from sensor_msgs.msg import CameraInfo -from tf import TransformListener -from tf.transformations import * - -from soccer_common.transformation import Transformation - - -class Camera: - """ - This is a reusable class that instantiates an instance of a Camera object that listens to the camera related topics - related to a robot and has useful functions that use geometry to determine the 3d/2d projection and location of things - - """ - - HORIZONTAL_FOV = 1.39626 - - def __init__(self, robot_name: str): - """ - Initializes the camera object - - :param robot_name: Name of the robot, to be used in subscribers - """ - - self.robot_name = robot_name #: Name of the robot - self.pose = Transformation() #: Pose of the camera - self.pose_base_link_straight = Transformation() #: Pose of the camera - self.camera_info = None #: Camera info object recieved from the subscriber - self.horizontalFOV = Camera.HORIZONTAL_FOV - self.focal_length = 3.67 #: Focal length of the camera (meters) distance to the camera plane as projected in 3D - - self.camera_info_subscriber = Subscriber("/" + robot_name + "/camera/camera_info", CameraInfo, self.cameraInfoCallback) - - self.tf_listener = TransformListener() - - self.init_time = rospy.Time.now() - - def ready(self) -> bool: - """ - Function to determine when the camera object has recieved the necessary information and is ready to be used - - :return: True if the camera is ready, else False - """ - return self.pose is not None and self.resolution_x is not None and self.resolution_y is not None and self.camera_info is not None - - def reset_position(self, from_world_frame=False, timestamp=rospy.Time(0), camera_frame="/camera", skip_if_not_found=False): - """ - Resets the position of the camera, it uses a series of methods that fall back on each other to get the location of the camera - - :param from_world_frame: If this is set to true, the camera position transformation will be from the world instead of the robot odom frame - :param timestamp: What time do we want the camera tf frame, rospy.Time(0) if get the latest transform - :param camera_frame: The name of the camera frame - :param skip_if_not_found: If set to true, then will not wait if it cannot find the camera transform after the specified duration (1 second), it will just return - """ - if from_world_frame: - try: - self.tf_listener.waitForTransform("world", self.robot_name + camera_frame, timestamp, rospy.Duration(nsecs=1000000)) - (trans, rot) = self.tf_listener.lookupTransform("world", self.robot_name + camera_frame, timestamp) - print(trans) - print(rot) - self.pose = Transformation(trans, rot) - return - except ( - tf2_py.LookupException, - tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException, - tf2_py.TransformException, - ) as ex: - rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") - pass - else: - - try: - # Find the odom to base_footprint and publish straight base footprint - self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp, rospy.Duration(secs=1)) - (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp) - world_to_base_link = Transformation(trans, rot) - e = world_to_base_link.orientation_euler - e[1] = 0 - e[2] = 0 - world_to_base_link.orientation_euler = e - self.pose_base_link_straight = world_to_base_link - - # Calculate the camera transformation - self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp, rospy.Duration(secs=1)) - (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp) - world_to_camera = Transformation(trans, rot) - - camera_to_base_link = scipy.linalg.inv(world_to_base_link) @ world_to_camera - - self.pose = camera_to_base_link - return - except ( - tf2_py.LookupException, - tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException, - tf2_py.TransformException, - ) as ex: - rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") - pass - - def cameraInfoCallback(self, camera_info: CameraInfo): - """ - Callback function for the camera info subscriber - - :param camera_info: from the camera info topic - """ - self.camera_info = camera_info - - @cached_property - def resolution_x(self) -> int: - """ - The X resolution of the camera or the width of the screen in pixels - - :return: width in pixels - """ - return self.camera_info.width - - @cached_property - def resolution_y(self): - """ - The Y resolution of the camera or the height of the screen in pixels - - :return: height in pixels - """ - return self.camera_info.height - - def findFloorCoordinate(self, pos: [int]) -> [int]: - """ - From a camera pixel, get a coordinate on the floor - - :param pos: The position on the screen in pixels (x, y) - :return: The 3D coordinate of the pixel as projected to the floor - """ - tx, ty = self.imageToWorldFrame(pos[0], pos[1]) - pixel_pose = Transformation(position=(self.focal_length, tx, ty)) - camera_pose = self.pose - pixel_world_pose = camera_pose @ pixel_pose - ratio = (camera_pose.position[2] - pixel_world_pose.position[2]) / self.pose.position[2] # TODO Fix divide by 0 problem - x_delta = (pixel_world_pose.position[0] - camera_pose.position[0]) / ratio - y_delta = (pixel_world_pose.position[1] - camera_pose.position[1]) / ratio - - return [x_delta + camera_pose.position[0], y_delta + camera_pose.position[1], 0] - - def findCameraCoordinate(self, pos: [int]) -> [int]: - """ - From a 3d position on the field, get the camera coordinate, opposite of :func:`~soccer_common.Camera.findFloorCoordinate` - - :param pos: The 3D coordinate of the object - :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera - """ - pos3d = Transformation(pos) - camera_pose = self.pose - pos3d_tr = np.linalg.inv(camera_pose) @ pos3d - - return self.findCameraCoordinateFixedCamera(pos3d_tr.position) - - def findCameraCoordinateFixedCamera(self, pos: [int]) -> [int]: - """ - Helper function for :func:`~soccer_common.Camera.findCameraCoordinate`, finds the camera coordinate if the camera were fixed at the origin - - :param pos: The 3D coordinate of the object - :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera and the camera is placed at the origin - """ - - pos = Transformation(pos) - - ratio = self.focal_length / pos.position[0] - - tx = pos.position[1] * ratio - ty = pos.position[2] * ratio - x, y = self.worldToImageFrame(tx, ty) - return [x, y] - - @cached_property - def verticalFOV(self): - """ - The vertical field of vision of the camera. - See `Field of View `_ - """ - return 2 * math.atan(math.tan(self.horizontalFOV * 0.5) * (self.resolution_y / self.resolution_x)) - - @cached_property - def imageSensorHeight(self): - """ - The height of the image sensor (m) - """ - return math.tan(self.verticalFOV / 2.0) * 2.0 * self.focal_length - - @cached_property - def imageSensorWidth(self): - """ - The width of the image sensor (m) - """ - return math.tan(self.horizontalFOV / 2.0) * 2.0 * self.focal_length - - @cached_property - def pixelHeight(self): - """ - The height of a pixel in real 3d measurements (m) - """ - return self.imageSensorHeight / self.resolution_y - - @cached_property - def pixelWidth(self): - """ - The wdith of a pixel in real 3d measurements (m) - """ - return self.imageSensorWidth / self.resolution_x - pass - - def imageToWorldFrame(self, pixel_x: int, pixel_y: int) -> tuple: - """ - From image pixel coordinates, get the coordinates of the pixel as if they have been projected ot the camera plane, which is - positioned at (0,0) in 3D world coordinates - https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163680c589a_0_0 - - :param pixel_x: x pixel of the camera - :param pixel_y: y pixel of the camera - :return: 3D position (X, Y) of the pixel in meters - """ - return ( - (self.resolution_x / 2.0 - (pixel_x + 0.5)) * self.pixelWidth, - (self.resolution_y / 2.0 - (pixel_y + 0.5)) * self.pixelHeight, - ) - - def worldToImageFrame(self, pos_x: float, pos_y: float) -> tuple: - """ - Reverse function for :func:`~soccer_common.Camera.imageToWorldFrame`, takes the 3D world coordinates of the camera plane - and returns pixels - - :param pos_x: X position of the pixel on the world plane in meters - :param pos_y: Y position of the pixel on the world plane in meters - :return: Tuple (x, y) of the pixel coordinates of in the image - """ - return ( - (self.resolution_x / 2.0 + pos_x / self.pixelWidth) - 0.5, - (self.resolution_y / 2.0 + pos_y / self.pixelHeight) - 0.5, - ) - - def calculateBoundingBoxesFromBall(self, ball_position: Transformation, ball_radius: float = 0.07): - """ - Takes a 3D ball transformation and returns the bounding boxes of the ball if seen on camera - - :param ball_position: 3D coordinates of the ball stored in the :class:`Transformation` format - :param ball_radius: The radious of the ball in centimeters - :return: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left - and bottom right of the bounding box respectively - """ - - camera_pose = self.pose - pos3d_tr = np.linalg.inv(camera_pose) @ ball_position - - x = pos3d_tr.position[0] - y = -pos3d_tr.position[1] - z = -pos3d_tr.position[2] - r = ball_radius - - thetay = math.atan2(y, x) - dy = math.sqrt(x**2 + y**2) - phiy = math.asin(r / dy) - - xyfar = [x - math.sin(thetay + phiy) * r, y + math.cos(thetay + phiy) * r] - xynear = [x + math.sin(thetay - phiy) * r, y - math.cos(thetay - phiy) * r] - - thetaz = math.atan2(z, x) - dz = math.sqrt(x**2 + z**2) - phiz = math.asin(r / dz) - - xzfar = [x - math.sin(thetaz + phiz) * r, z + math.cos(thetaz + phiz) * r] - xznear = [x + math.sin(thetaz - phiz) * r, z - math.cos(thetaz - phiz) * r] - - ball_right_point = [xyfar[0], xyfar[1], z] - ball_left_point = [xynear[0], xynear[1], z] - ball_bottom_point = [xzfar[0], y, xzfar[1]] - ball_top_point = [xznear[0], y, xznear[1]] - - ball_left_point_cam = self.findCameraCoordinateFixedCamera(ball_left_point) - ball_right_point_cam = self.findCameraCoordinateFixedCamera(ball_right_point) - ball_top_point_cam = self.findCameraCoordinateFixedCamera(ball_top_point) - ball_bottom_point_cam = self.findCameraCoordinateFixedCamera(ball_bottom_point) - - left_border_x = ball_left_point_cam[0] - right_border_x = ball_right_point_cam[0] - top_border_y = ball_top_point_cam[1] - bottom_border_y = ball_bottom_point_cam[1] - - bounding_box = [[left_border_x, top_border_y], [right_border_x, bottom_border_y]] - - return bounding_box - - def calculateBallFromBoundingBoxes(self, ball_radius: float = 0.07, bounding_boxes: [float] = []) -> Transformation: - """ - Reverse function for :func:`~soccer_common.Camera.calculateBoundingBoxesFromBall`, takes the bounding boxes - of the ball as seen on the camera and return the 3D position of the ball assuming that the ball is on the ground - - :param ball_radius: The radius of the ball in meters - :param bounding_boxes: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left and bottom right of the bounding box respectively - :return: 3D coordinates of the ball stored in the :class:`Transformation` format - """ - - # bounding boxes [(y1, z1), (y2, z2)] - r = ball_radius - - y1 = bounding_boxes[0][0] - z1 = bounding_boxes[0][1] - y2 = bounding_boxes[1][0] - z2 = bounding_boxes[1][1] - - # Assuming the ball is a sphere, the bounding box must be a square, averaging the borders - ym = (y1 + y2) / 2 - zm = (z1 + z2) / 2 - length = z2 - z1 - width = y2 - y1 - y1 = ym - (width / 2) - z1 = zm - (length / 2) - y2 = ym + (width / 2) - z2 = zm + (length / 2) - - y1w, z1w = self.imageToWorldFrame(y1, z1) - y2w, z2w = self.imageToWorldFrame(y2, z2) - y1w = -y1w - z1w = -z1w - y2w = -y2w - z2w = -z2w - - f = self.focal_length - - thetay1 = math.atan2(y1w, f) - thetay2 = math.atan2(y2w, f) - - thetayy = (thetay2 - thetay1) / 2 - thetay = thetay1 + thetayy - - dy = r / math.sin(thetayy) - - xy = (math.cos(thetay) * dy, math.sin(thetay) * dy) - - thetaz1 = math.atan2(z1w, f) - thetaz2 = math.atan2(z2w, f) - - thetazz = (thetaz2 - thetaz1) / 2 - thetaz = thetaz1 + thetazz - - dz = r / math.sin(thetazz) - - xz = (math.cos(thetaz) * dz, math.sin(thetaz) * dz) - - ball_x = xy[0] - ball_y = xy[1] - ball_z = xz[1] - - tr = Transformation([ball_x, -ball_y, -ball_z]) - tr_cam = self.pose @ tr - - return tr_cam - - def calculateHorizonCoverArea(self) -> int: - """ - Given the camera's position, return the area that is covered by the horizon (that is not the field area) in pixels from the top position - :return: Pixel length from the top of the image to the point where it meets the horizon - """ - - pitch = self.pose.orientation_euler[1] - d = math.sin(pitch) * self.focal_length - - (r, h) = self.worldToImageFrame(0, -d) - return int(min(max(0, h), self.resolution_y)) - - -from unittest.mock import MagicMock - -if __name__ == "__main__": - rospy.init_node("camera_test") - c = Camera("robot") - c.reset_position = MagicMock(from_world_frame=True) - print(c.pose) - print(Transformation([0, 0, 0], [0, 0, 0, 1])) diff --git a/soccer_perception/soccer_object_detection/videos/.gitkeep b/soccer_common/src/soccer_common/perception/__init__.py similarity index 100% rename from soccer_perception/soccer_object_detection/videos/.gitkeep rename to soccer_common/src/soccer_common/perception/__init__.py diff --git a/soccer_common/src/soccer_common/perception/camera_base.py b/soccer_common/src/soccer_common/perception/camera_base.py new file mode 100644 index 000000000..5e8cfda6a --- /dev/null +++ b/soccer_common/src/soccer_common/perception/camera_base.py @@ -0,0 +1,104 @@ +import math +from functools import cached_property + +import numpy as np +from sensor_msgs.msg import CameraInfo + +from soccer_common.transformation import Transformation + + +class CameraBase: + # TODO maybe put into a common percpetion + # TODO also could be split up into different files too many things + + def __init__(self, camera_info: CameraInfo = CameraInfo(height=480, width=640)): + # TODO why does this need pose shouldnt all the calcualtions be in the robots relative frame and + # then transformed into the world frame that would make it a lot less relient on + # knowledge of its global position + self.camera_info = camera_info + self.horizontalFOV = 1.39626 + self.focal_length = 3.67 #: Focal length of the camera (meters) distance to the camera plane as projected in 3D + + def image_to_world_frame(self, pixel_x: int, pixel_y: int) -> tuple: + """ + From image pixel coordinates, get the coordinates of the pixel as if they have been projected ot the camera plane, which is + positioned at (0,0) in 3D world coordinates + https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163680c589a_0_0 + + :param pixel_x: x pixel of the camera + :param pixel_y: y pixel of the camera + :return: 3D position (X, Y) of the pixel in meters + """ + return ( + (self.resolution_x / 2.0 - (pixel_x + 0.5)) * self.pixel_width, + (self.resolution_y / 2.0 - (pixel_y + 0.5)) * self.pixel_height, + ) + + def world_to_image_frame(self, pos_x: float, pos_y: float) -> tuple: + """ + Reverse function for :func:`~soccer_common.Camera.imageToWorldFrame`, takes the 3D world coordinates of the camera plane + and returns pixels + + :param pos_x: X position of the pixel on the world plane in meters + :param pos_y: Y position of the pixel on the world plane in meters + :return: Tuple (x, y) of the pixel coordinates of in the image + """ + return ( + (self.resolution_x / 2.0 + pos_x / self.pixel_width) - 0.5, + (self.resolution_y / 2.0 + pos_y / self.pixel_height) - 0.5, + ) + + # CACHED PROPERTIES ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + @cached_property + def resolution_x(self) -> int: + """ + The X resolution of the camera or the width of the screen in pixels + + :return: width in pixels + """ + return self.camera_info.width + + @cached_property + def resolution_y(self): + """ + The Y resolution of the camera or the height of the screen in pixels + + :return: height in pixels + """ + return self.camera_info.height + + @cached_property + def vertical_fov(self): + """ + The vertical field of vision of the camera. + See `Field of View `_ + """ + return 2 * math.atan(math.tan(self.horizontalFOV * 0.5) * (self.resolution_y / self.resolution_x)) + + @cached_property + def image_sensor_height(self): + """ + The height of the image sensor (m) + """ + return math.tan(self.vertical_fov / 2.0) * 2.0 * self.focal_length + + @cached_property + def image_sensor_width(self): + """ + The width of the image sensor (m) + """ + return math.tan(self.horizontalFOV / 2.0) * 2.0 * self.focal_length + + @cached_property + def pixel_height(self): + """ + The height of a pixel in real 3d measurements (m) + """ + return self.image_sensor_height / self.resolution_y + + @cached_property + def pixel_width(self): + """ + The wdith of a pixel in real 3d measurements (m) + """ + return self.image_sensor_width / self.resolution_x diff --git a/soccer_common/src/soccer_common/perception/camera_calculations.py b/soccer_common/src/soccer_common/perception/camera_calculations.py new file mode 100644 index 000000000..c89f58f29 --- /dev/null +++ b/soccer_common/src/soccer_common/perception/camera_calculations.py @@ -0,0 +1,216 @@ +import math + +import numpy as np + +from soccer_common import Transformation +from soccer_common.perception.camera_base import CameraBase + + +class CameraCalculations(CameraBase): + def __init__(self): + super(CameraCalculations, self).__init__() + self.pose = Transformation() + + def calculate_horizon_cover_area(self) -> int: + """ + Given the camera's position, return the area that is covered by the horizon (that is not the field area) in pixels from the top position + :return: Pixel length from the top of the image to the point where it meets the horizon + """ + # TODO verify this works cause there is some weird stuff going on in the resultant image + # TODO the relative frame needs some more thought since this will break some things so + # will focus on basics before that section + # TODO need to find a way to visualize or verify this works with out sim maybe a graph that could work + pitch = self.pose.orientation_euler[1] + d = math.sin(pitch) * self.focal_length + + (r, h) = self.world_to_image_frame(0, -d) + return int(min(max(0, h), self.resolution_y)) + + def reset_position(self, from_world_frame=False, camera_frame="/camera", skip_if_not_found=False): + """ + Resets the position of the camera, it uses a series of methods that fall back on each other to get the location of the camera + + :param from_world_frame: If this is set to true, the camera position transformation will be from the world instead of the robot odom frame + :param timestamp: What time do we want the camera tf frame, rospy.Time(0) if get the latest transform + :param camera_frame: The name of the camera frame + :param skip_if_not_found: If set to true, then will not wait if it cannot find the camera transform after the specified duration (1 second), it will just return + """ + + # same hardcoded values + if from_world_frame: + trans = [0, 0, 0.5] + rot = [0, 0, 0, 1] + self.pose = Transformation(trans, rot) + else: + trans = [0, 0, 0.5] # TODO find init height + rot = [0, 0, 0, 1] + self.pose = Transformation(trans, rot) + + # TODO maybe in localization + def find_floor_coordinate(self, pos: [int]) -> [int]: + """ + From a camera pixel, get a coordinate on the floor + + :param pos: The position on the screen in pixels (x, y) + :return: The 3D coordinate of the pixel as projected to the floor + """ + # TODO this actually might need an accruate pose, but is it truly necessay + # TODO verify the math + # TODO should be easy to verify if the relative conversion works since you have the answers + tx, ty = self.image_to_world_frame(pos[0], pos[1]) + pixel_pose = Transformation(position=(self.focal_length, tx, ty)) + camera_pose = self.pose + pixel_world_pose = camera_pose @ pixel_pose + ratio = (camera_pose.position[2] - pixel_world_pose.position[2]) / self.pose.position[2] # TODO Fix divide by 0 problem + x_delta = (pixel_world_pose.position[0] - camera_pose.position[0]) / ratio + y_delta = (pixel_world_pose.position[1] - camera_pose.position[1]) / ratio + + return [x_delta + camera_pose.position[0], y_delta + camera_pose.position[1], 0] + + def find_camera_coordinate(self, pos: [int]) -> [int]: + """ + From a 3d position on the field, get the camera coordinate, opposite of :func:`~soccer_common.Camera.findFloorCoordinate` + + :param pos: The 3D coordinate of the object + :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera + """ + pos3d = Transformation(pos) + camera_pose = self.pose + pos3d_tr = np.linalg.inv(camera_pose) @ pos3d + + return self.find_camera_coordinate_fixed_camera(pos3d_tr.position) + + def find_camera_coordinate_fixed_camera(self, pos: [int]) -> [int]: + """ + Helper function for :func:`~soccer_common.Camera.findCameraCoordinate`, finds the camera coordinate if the camera were fixed at the origin + + :param pos: The 3D coordinate of the object + :return: The 2D pixel (x, y) on the camera, if the object was projected on the camera and the camera is placed at the origin + """ + + pos = Transformation(pos) + + ratio = self.focal_length / pos.position[0] + + tx = pos.position[1] * ratio + ty = pos.position[2] * ratio + x, y = self.world_to_image_frame(tx, ty) + return [x, y] + + # TODO should these be here or in the node? + def calculate_bounding_boxes_from_ball(self, ball_position: Transformation, ball_radius: float = 0.07): + """ + Takes a 3D ball transformation and returns the bounding boxes of the ball if seen on camera + + :param ball_position: 3D coordinates of the ball stored in the :class:`Transformation` format + :param ball_radius: The radious of the ball in centimeters + :return: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left + and bottom right of the bounding box respectively + """ + # TODO make relative and make into smaller functions + camera_pose = self.pose + pos3d_tr = np.linalg.inv(camera_pose) @ ball_position + + x = pos3d_tr.position[0] + y = -pos3d_tr.position[1] + z = -pos3d_tr.position[2] + r = ball_radius + + theta_y = math.atan2(y, x) + dy = math.sqrt(x**2 + y**2) + phi_y = math.asin(r / dy) + + xy_far = [x - math.sin(theta_y + phi_y) * r, y + math.cos(theta_y + phi_y) * r] + xy_near = [x + math.sin(theta_y - phi_y) * r, y - math.cos(theta_y - phi_y) * r] + + theta_z = math.atan2(z, x) + dz = math.sqrt(x**2 + z**2) + phi_z = math.asin(r / dz) + + xz_far = [x - math.sin(theta_z + phi_z) * r, z + math.cos(theta_z + phi_z) * r] + xz_near = [x + math.sin(theta_z - phi_z) * r, z - math.cos(theta_z - phi_z) * r] + + ball_right_point = [xy_far[0], xy_far[1], z] + ball_left_point = [xy_near[0], xy_near[1], z] + ball_bottom_point = [xz_far[0], y, xz_far[1]] + ball_top_point = [xz_near[0], y, xz_near[1]] + + ball_left_point_cam = self.find_camera_coordinate_fixed_camera(ball_left_point) + ball_right_point_cam = self.find_camera_coordinate_fixed_camera(ball_right_point) + ball_top_point_cam = self.find_camera_coordinate_fixed_camera(ball_top_point) + ball_bottom_point_cam = self.find_camera_coordinate_fixed_camera(ball_bottom_point) + + left_border_x = ball_left_point_cam[0] + right_border_x = ball_right_point_cam[0] + top_border_y = ball_top_point_cam[1] + bottom_border_y = ball_bottom_point_cam[1] + + bounding_box = [[left_border_x, top_border_y], [right_border_x, bottom_border_y]] + + return bounding_box + + def calculate_ball_from_bounding_boxes(self, ball_radius: float = 0.07, bounding_boxes: [float] = []) -> Transformation: + """ + Reverse function for :func:`~soccer_common.Camera.calculateBoundingBoxesFromBall`, takes the bounding boxes + of the ball as seen on the camera and return the 3D position of the ball assuming that the ball is on the ground + + :param ball_radius: The radius of the ball in meters + :param bounding_boxes: The bounding boxes of the ball on the camera in the format [[x1,y1], [x1,y1]] which are the top left and bottom right of the bounding box respectively + :return: 3D coordinates of the ball stored in the :class:`Transformation` format + """ + + # bounding boxes [(y1, z1), (y2, z2)] + r = ball_radius + + y1 = bounding_boxes[0][0] + z1 = bounding_boxes[0][1] + y2 = bounding_boxes[1][0] + z2 = bounding_boxes[1][1] + + # Assuming the ball is a sphere, the bounding box must be a square, averaging the borders + ym = (y1 + y2) / 2 + zm = (z1 + z2) / 2 + length = z2 - z1 + width = y2 - y1 + y1 = ym - (width / 2) + z1 = zm - (length / 2) + y2 = ym + (width / 2) + z2 = zm + (length / 2) + + y1w, z1w = self.image_to_world_frame(y1, z1) + y2w, z2w = self.image_to_world_frame(y2, z2) + y1w = -y1w + z1w = -z1w + y2w = -y2w + z2w = -z2w + + f = self.focal_length + + theta_y1 = math.atan2(y1w, f) + theta_y2 = math.atan2(y2w, f) + + theta_yy = (theta_y2 - theta_y1) / 2 + theta_y = theta_y1 + theta_yy + + dy = r / math.sin(theta_yy) + + xy = (math.cos(theta_y) * dy, math.sin(theta_y) * dy) + + theta_z1 = math.atan2(z1w, f) + theta_z2 = math.atan2(z2w, f) + + theta_zz = (theta_z2 - theta_z1) / 2 + theta_z = theta_z1 + theta_zz + + dz = r / math.sin(theta_zz) + + xz = (math.cos(theta_z) * dz, math.sin(theta_z) * dz) + + ball_x = xy[0] + ball_y = xy[1] + ball_z = xz[1] + + tr = Transformation([ball_x, -ball_y, -ball_z]) + tr_cam = self.pose @ tr + + return tr_cam diff --git a/soccer_common/src/soccer_common/perception/camera_calculations_ros.py b/soccer_common/src/soccer_common/perception/camera_calculations_ros.py new file mode 100644 index 000000000..c8a5b7869 --- /dev/null +++ b/soccer_common/src/soccer_common/perception/camera_calculations_ros.py @@ -0,0 +1,87 @@ +import rospy +import scipy +import tf +import tf2_py +from rospy import Subscriber +from sensor_msgs.msg import CameraInfo +from tf import TransformListener + +from soccer_common import CameraCalculations, Transformation + + +# TODO fix this up +class CameraCalculationsRos(CameraCalculations): + def __init__(self, robot_name: str): + super(CameraCalculationsRos, self).__init__() + + self.pose_base_link_straight = Transformation() #: Pose of the camera + + self.robot_name = robot_name #: Name of the robot + self.camera_info_subscriber = Subscriber("/" + robot_name + "/camera/camera_info", CameraInfo, self.camera_info_callback) + + self.tf_listener = TransformListener() + + self.init_time = rospy.Time.now() + + def camera_info_callback(self, camera_info: CameraInfo): + """ + Callback function for the camera info subscriber + + :param camera_info: from the camera info topic + """ + self.camera_info = camera_info + + def reset_position(self, from_world_frame=False, timestamp=rospy.Time(0), camera_frame="/camera", skip_if_not_found=False): + """ + Resets the position of the camera, it uses a series of methods that fall back on each other to get the location of the camera + + :param from_world_frame: If this is set to true, the camera position transformation will be from the world instead of the robot odom frame + :param timestamp: What time do we want the camera tf frame, rospy.Time(0) if get the latest transform + :param camera_frame: The name of the camera frame + :param skip_if_not_found: If set to true, then will not wait if it cannot find the camera transform after the specified duration (1 second), it will just return + """ + if from_world_frame: + try: + self.tf_listener.waitForTransform("world", self.robot_name + camera_frame, timestamp, rospy.Duration(nsecs=1000000)) + (trans, rot) = self.tf_listener.lookupTransform("world", self.robot_name + camera_frame, timestamp) + self.pose = Transformation(trans, rot) + return + except ( + tf2_py.LookupException, + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + tf2_py.TransformException, + ) as ex: + rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") + pass + else: + + try: + # Find the odom to base_footprint and publish straight base footprint + self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp, rospy.Duration(secs=1)) + (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + "/base_footprint", timestamp) + world_to_base_link = Transformation(trans, rot) + e = world_to_base_link.orientation_euler + e[1] = 0 + e[2] = 0 + world_to_base_link.orientation_euler = e + self.pose_base_link_straight = world_to_base_link + + # Calculate the camera transformation + self.tf_listener.waitForTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp, rospy.Duration(secs=1)) + (trans, rot) = self.tf_listener.lookupTransform(self.robot_name + "/odom", self.robot_name + camera_frame, timestamp) + world_to_camera = Transformation(trans, rot) + + camera_to_base_link = scipy.linalg.inv(world_to_base_link) @ world_to_camera + + self.pose = camera_to_base_link + return + except ( + tf2_py.LookupException, + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + tf2_py.TransformException, + ) as ex: + rospy.logerr_throttle(5, f"Unable to find transformation from world to {self.robot_name + camera_frame}") diff --git a/soccer_common/src/soccer_common/test_common.py b/soccer_common/src/soccer_common/test_common.py deleted file mode 100644 index 6da2e531d..000000000 --- a/soccer_common/src/soccer_common/test_common.py +++ /dev/null @@ -1,42 +0,0 @@ -from unittest import TestCase - -import numpy as np -import rospy -from sensor_msgs.msg import CameraInfo - -from soccer_common.camera import Camera -from soccer_common.transformation import Transformation - - -class TestCommon(TestCase): - def test_transformation(self): - t = Transformation(quaternion=[0, 0, 1, 0]) @ Transformation(position=[1, 0, 0]) - assert np.all(t.quaternion == [0, 0, 1, 0]) - assert np.all(t.position == [-1, 0, 0]) - - def test_calculate_bounding_boxes_from_ball(self): - rospy.init_node("test") - - for cam_angle in [0, 0.1, -0.1]: - - for cam_position in [[0, 0, 0], [0, 0, 0.1], [0, 0, -0.1]]: - p = Transformation(cam_position, euler=[cam_angle, 0, 0]) - c = Camera("robot1") - c.pose = p - ci = CameraInfo() - ci.height = 360 - ci.width = 240 - c.camera_info = ci - - positions = [[0.5, 0, 0.1], [0.5, 0, 0], [0.5, 0, 0.1]] - for position in positions: - ball_pose = Transformation(position) - ball_radius = 0.07 - - bounding_boxes = c.calculateBoundingBoxesFromBall(ball_pose, ball_radius) - # [[135.87634651355825, 75.87634651355823], [224.12365348644175, 164.12365348644175]] - position = c.calculateBallFromBoundingBoxes(ball_radius, bounding_boxes) - - self.assertAlmostEqual(position.position[0], ball_pose.position[0], delta=0.001) - self.assertAlmostEqual(position.position[1], ball_pose.position[1], delta=0.001) - self.assertAlmostEqual(position.position[2], ball_pose.position[2], delta=0.001) diff --git a/soccer_common/test/test_camera.py b/soccer_common/test/test_camera.py new file mode 100644 index 000000000..f0fa433bb --- /dev/null +++ b/soccer_common/test/test_camera.py @@ -0,0 +1,77 @@ +import math +from unittest import TestCase + +from sensor_msgs.msg import CameraInfo + +from soccer_common.perception.camera_calculations import CameraCalculations +from soccer_common.transformation import Transformation + + +# TODO fix unit test +class TestCamera(TestCase): + def test_camera_find_floor_coordinate(self): + p = Transformation([0, 0, 0.5], euler=[0, math.pi / 4, 0]) + c = CameraCalculations() + c.pose = p + ci = CameraInfo() + ci.height = 240 + ci.width = 360 + c.camera_info = ci + + p2 = c.find_floor_coordinate([360 / 2, 240 / 2]) + self.assertAlmostEqual(p2[0], 0.5, delta=0.005) + self.assertAlmostEqual(p2[1], 0, delta=0.005) + self.assertAlmostEqual(p2[2], 0, delta=0.005) + + def test_camera_find_camera_coordinate(self): + p = Transformation([0, 0, 0.5], euler=[0, math.pi / 4, 0]) + c = CameraCalculations() + c.pose = p + ci = CameraInfo() + ci.height = 240 + ci.width = 360 + c.camera_info = ci + + p2 = c.find_camera_coordinate([0.5, 0, 0]) + self.assertAlmostEqual(p2[0], 360 / 2, delta=0.5) + self.assertAlmostEqual(p2[1], 240 / 2, delta=0.5) + + def test_camera_find_camera_coordinate_2(self): + p = Transformation([0, 0, 0.5], euler=[0, 0, 0]) + c = CameraCalculations() + c.pose = p + ci = CameraInfo() + ci.height = 240 + ci.width = 360 + c.camera_info = ci + + p3 = c.find_camera_coordinate([0.5, 0, 0.5]) + self.assertAlmostEqual(p3[0], 360 / 2, delta=0.5) + self.assertAlmostEqual(p3[1], 240 / 2, delta=0.5) + + def test_calculate_bounding_boxes_from_ball(self): + # rospy.init_node("test") + + for cam_angle in [0, 0.1, -0.1]: + + for cam_position in [[0, 0, 0], [0, 0, 0.1], [0, 0, -0.1]]: + p = Transformation(cam_position, euler=[cam_angle, 0, 0]) + c = CameraCalculations() + c.pose = p + ci = CameraInfo() + ci.height = 360 + ci.width = 240 + c.camera_info = ci + + positions = [[0.5, 0, 0.1], [0.5, 0, 0], [0.5, 0, 0.1]] + for position in positions: + ball_pose = Transformation(position) + ball_radius = 0.07 + + bounding_boxes = c.calculate_bounding_boxes_from_ball(ball_pose, ball_radius) + # [[135.87634651355825, 75.87634651355823], [224.12365348644175, 164.12365348644175]] + position = c.calculate_ball_from_bounding_boxes(ball_radius, bounding_boxes) + + self.assertAlmostEqual(position.position[0], ball_pose.position[0], delta=0.001) + self.assertAlmostEqual(position.position[1], ball_pose.position[1], delta=0.001) + self.assertAlmostEqual(position.position[2], ball_pose.position[2], delta=0.001) diff --git a/soccer_common/test/test_common.py b/soccer_common/test/test_common.py new file mode 100644 index 000000000..4dc71da57 --- /dev/null +++ b/soccer_common/test/test_common.py @@ -0,0 +1,14 @@ +from unittest import TestCase + +import numpy as np +import rospy +from sensor_msgs.msg import CameraInfo + +from soccer_common.transformation import Transformation + + +class TestCommon(TestCase): + def test_transformation(self): + t = Transformation(quaternion=[0, 0, 1, 0]) @ Transformation(position=[1, 0, 0]) + assert np.all(t.quaternion == [0, 0, 1, 0]) + assert np.all(t.position == [-1, 0, 0]) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/.ipynb_checkpoints/fhgj-checkpoint.ipynb b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/.ipynb_checkpoints/fhgj-checkpoint.ipynb deleted file mode 100644 index 8016bd956..000000000 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/.ipynb_checkpoints/fhgj-checkpoint.ipynb +++ /dev/null @@ -1,160 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "id": "initial_id", - "metadata": { - "collapsed": true - }, - "outputs": [], - "source": [ - "" - ] - }, - { - "metadata": { - "ExecuteTime": { - "end_time": "2024-07-26T20:27:26.357513Z", - "start_time": "2024-07-26T20:27:26.339991Z" - } - }, - "cell_type": "code", - "source": [ - "import mujoco\n", - "import mediapy as media\n", - "from IPython.display import clear_output\n", - "import matplotlib.pyplot as plt\n", - "\n", - "chaotic_pendulum = \"\"\"\n", - "\n", - " \n", - "\n", - " \n", - " \n", - " \n", - " \n", - "\n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - "\n", - "\"\"\"\n", - "model = mujoco.MjModel.from_xml_string(chaotic_pendulum)\n", - "data = mujoco.MjData(model)\n", - "height = 480\n", - "width = 640\n", - "\n", - "with mujoco.Renderer(model, height, width) as renderer:\n", - " mujoco.mj_forward(model, data)\n", - " renderer.update_scene(data, camera=\"fixed\")\n", - "\n", - " media.show_image(renderer.render())\n", - " " - ], - "id": "d2325b549cd7d10e", - "outputs": [ - { - "ename": "ModuleNotFoundError", - "evalue": "No module named 'mujoco'", - "output_type": "error", - "traceback": [ - "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m", - "\u001B[0;31mModuleNotFoundError\u001B[0m Traceback (most recent call last)", - "Cell \u001B[0;32mIn[3], line 1\u001B[0m\n\u001B[0;32m----> 1\u001B[0m \u001B[38;5;28;01mimport\u001B[39;00m \u001B[38;5;21;01mmujoco\u001B[39;00m\n\u001B[1;32m 2\u001B[0m \u001B[38;5;28;01mimport\u001B[39;00m \u001B[38;5;21;01mmediapy\u001B[39;00m \u001B[38;5;28;01mas\u001B[39;00m \u001B[38;5;21;01mmedia\u001B[39;00m\n\u001B[1;32m 3\u001B[0m \u001B[38;5;28;01mfrom\u001B[39;00m \u001B[38;5;21;01mIPython\u001B[39;00m\u001B[38;5;21;01m.\u001B[39;00m\u001B[38;5;21;01mdisplay\u001B[39;00m \u001B[38;5;28;01mimport\u001B[39;00m clear_output\n", - "\u001B[0;31mModuleNotFoundError\u001B[0m: No module named 'mujoco'" - ] - } - ], - "execution_count": 3 - }, - { - "metadata": { - "ExecuteTime": { - "end_time": "2024-07-26T20:27:27.341789Z", - "start_time": "2024-07-26T20:27:27.340199Z" - } - }, - "cell_type": "code", - "source": "", - "id": "a1e4e0175c70e866", - "outputs": [], - "execution_count": 3 - }, - { - "metadata": { - "ExecuteTime": { - "end_time": "2024-07-26T20:27:27.943871Z", - "start_time": "2024-07-26T20:27:27.942200Z" - } - }, - "cell_type": "code", - "source": "", - "id": "48cd10deb5af0b86", - "outputs": [], - "execution_count": 3 - }, - { - "metadata": { - "ExecuteTime": { - "end_time": "2024-07-26T20:27:28.142440Z", - "start_time": "2024-07-26T20:27:28.138875Z" - } - }, - "cell_type": "code", - "source": "", - "id": "1f98341c9c2caf8b", - "outputs": [], - "execution_count": 3 - }, - { - "metadata": {}, - "cell_type": "code", - "outputs": [], - "execution_count": null, - "source": "", - "id": "c7736f9160249cc" - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 2 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython2", - "version": "2.7.6" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/double_pendulum.xml b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/double_pendulum.xml new file mode 100644 index 000000000..8b9958ccc --- /dev/null +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/double_pendulum.xml @@ -0,0 +1,44 @@ + + diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/mujoco_view.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/mujoco_view.py new file mode 100644 index 000000000..14b1eba6b --- /dev/null +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/mujoco_view.py @@ -0,0 +1,60 @@ +"""Example of using the MuJoCo Python viewer UI for a simple control task""" + +import mujoco +import mujoco.viewer as viewer +import numpy as np +from scipy.linalg import solve_continuous_are + + +# Find a K matrix for a linearized double pendulum using LQR +def double_pendulum_lqr_K(m, d): + # Calculate system linearization + A = np.zeros((2 * m.nv + m.na, 2 * m.nv + m.na)) + B = np.zeros((2 * m.nv + m.na, m.nu)) + mujoco.mjd_transitionFD(m, d, 1e-7, 1, A=A, B=B, C=None, D=None) + + # Convert to continuous time + A = A - np.eye(A.shape[0]) + A = A / m.opt.timestep + B = B / m.opt.timestep + + Q = 1 * np.diag((10, 10, 100, 100)) + R = np.eye(B.shape[1]) + + S = solve_continuous_are(A, B, Q, R) + + K = np.linalg.inv(R) @ (B.T @ S) + return K + + +def double_pendulum_control(m, d, K): + x = np.concatenate((d.joint("shoulder").qpos, d.joint("elbow").qpos, d.joint("shoulder").qvel, d.joint("elbow").qvel)) + + u = -K @ x + d.actuator("shoulder").ctrl[0] = u + + +def load_callback(m=None, d=None): + # Clear the control callback before loading a new model + # or a Python exception is raised + mujoco.set_mjcb_control(None) + + # m = mujoco.MjModel.from_xml_path('./double_pendulum.xml') + m = mujoco.MjModel.from_xml_path("../../../../../soccer_description/bez2_description/urdf/bez2.urdf") + d = mujoco.MjData(m) + + # if m is not None: + # # Calculate K matrix + # K = double_pendulum_lqr_K(m, d) + # + # # Set some initial conditions + d.joint("left_arm_motor_1").qpos = 1 + d.joint("head_motor_0").qpos = 1 + # # Set the control callback + # mujoco.set_mjcb_control(lambda m, d: double_pendulum_control(m, d, K)) + + return m, d + + +if __name__ == "__main__": + viewer.launch(loader=load_callback) diff --git a/soccer_perception/data/.gitkeep b/soccer_perception/data/.gitkeep new file mode 100644 index 000000000..e69de29bb diff --git a/soccer_perception/images/fieldlines/img0_-0.6805418953941712_-0.19657546078618005_-1.049088190788794.png b/soccer_perception/images/fieldlines/img0_-0.6805418953941712_-0.19657546078618005_-1.049088190788794.png deleted file mode 100644 index 948092c3c..000000000 --- a/soccer_perception/images/fieldlines/img0_-0.6805418953941712_-0.19657546078618005_-1.049088190788794.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cd7cd4b78bb466284e1791fc7891c78ad2e372846d9a295eda45f3edc3204937 -size 525100 diff --git a/soccer_perception/images/fieldlines/img10_0.8641847711947435_0.18866567631476938_1.5249962322470259.png b/soccer_perception/images/fieldlines/img10_0.8641847711947435_0.18866567631476938_1.5249962322470259.png deleted file mode 100644 index d33853c02..000000000 --- a/soccer_perception/images/fieldlines/img10_0.8641847711947435_0.18866567631476938_1.5249962322470259.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e986326d2ef4753b8d4ab5f981f177336081294be415e91fca1020cc02b49335 -size 563378 diff --git a/soccer_perception/images/fieldlines/img11_0.0425320918554577_1.629702800190004_-2.915223713456875.png b/soccer_perception/images/fieldlines/img11_0.0425320918554577_1.629702800190004_-2.915223713456875.png deleted file mode 100644 index 89bec99a1..000000000 --- a/soccer_perception/images/fieldlines/img11_0.0425320918554577_1.629702800190004_-2.915223713456875.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:67752b61121736858333740c5daf606cfeff47518f30ab6d948ce338be0e4823 -size 543813 diff --git a/soccer_perception/images/fieldlines/img12_1.109509427721366_2.330417857677584_-0.15655553984103276.png b/soccer_perception/images/fieldlines/img12_1.109509427721366_2.330417857677584_-0.15655553984103276.png deleted file mode 100644 index 4b5fe397f..000000000 --- a/soccer_perception/images/fieldlines/img12_1.109509427721366_2.330417857677584_-0.15655553984103276.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:08a286ae3d829e7ec479d8c62242260a69815c8961eefcd6f42cbbec609ee6b2 -size 555723 diff --git a/soccer_perception/images/fieldlines/img13_-1.0020274648257552_-0.168873476816235_1.9748205753546078.png b/soccer_perception/images/fieldlines/img13_-1.0020274648257552_-0.168873476816235_1.9748205753546078.png deleted file mode 100644 index 2c4ac485c..000000000 --- a/soccer_perception/images/fieldlines/img13_-1.0020274648257552_-0.168873476816235_1.9748205753546078.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2cb14a38de8dcc7923441a583c671bcb29e2a11d4a74e4746aca06db36002980 -size 530868 diff --git a/soccer_perception/images/fieldlines/img14_-0.4107905429521215_2.4511358573282696_-0.8450607115290163.png b/soccer_perception/images/fieldlines/img14_-0.4107905429521215_2.4511358573282696_-0.8450607115290163.png deleted file mode 100644 index f23c833c4..000000000 --- a/soccer_perception/images/fieldlines/img14_-0.4107905429521215_2.4511358573282696_-0.8450607115290163.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d31414ff0204062a5db5feb729f0a0bce3027360ca52a1d8c5e3e6efa3d516c9 -size 547927 diff --git a/soccer_perception/images/fieldlines/img15_-1.4659361337858274_-0.5816355306389931_-0.502654929457147.png b/soccer_perception/images/fieldlines/img15_-1.4659361337858274_-0.5816355306389931_-0.502654929457147.png deleted file mode 100644 index ef7f62be8..000000000 --- a/soccer_perception/images/fieldlines/img15_-1.4659361337858274_-0.5816355306389931_-0.502654929457147.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cd201ebf19f80e9dda7473924bc312257e6f45647749531427bc14dcb3faa759 -size 544953 diff --git a/soccer_perception/images/fieldlines/img16_-0.7853100428576383_-1.5254301032646078_-2.984049153805578.png b/soccer_perception/images/fieldlines/img16_-0.7853100428576383_-1.5254301032646078_-2.984049153805578.png deleted file mode 100644 index e4e75b0a3..000000000 --- a/soccer_perception/images/fieldlines/img16_-0.7853100428576383_-1.5254301032646078_-2.984049153805578.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fab012ad577eac53cbaac7136f4740314756e39892e380912219335cf6f73f64 -size 539378 diff --git a/soccer_perception/images/fieldlines/img1_0.031962395374173_-0.8891447062075253_-2.658362073693127.png b/soccer_perception/images/fieldlines/img1_0.031962395374173_-0.8891447062075253_-2.658362073693127.png deleted file mode 100644 index 4c42328c9..000000000 --- a/soccer_perception/images/fieldlines/img1_0.031962395374173_-0.8891447062075253_-2.658362073693127.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7d5a18714c4f90722077e0ef136f8177481e232e2a877d28d0e993e618578d16 -size 525753 diff --git a/soccer_perception/images/fieldlines/img2_-0.19940776631633783_0.9547434357596831_0.22595902027652404.png b/soccer_perception/images/fieldlines/img2_-0.19940776631633783_0.9547434357596831_0.22595902027652404.png deleted file mode 100644 index 5aa4694f0..000000000 --- a/soccer_perception/images/fieldlines/img2_-0.19940776631633783_0.9547434357596831_0.22595902027652404.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c765a02fd78131f1295349b8eef44c53be5d0ae02a15b3b60fa28aa96acf2441 -size 551415 diff --git a/soccer_perception/images/fieldlines/img2_-1.170645594381017_1.6704487675687467_-1.7207161315253598.png b/soccer_perception/images/fieldlines/img2_-1.170645594381017_1.6704487675687467_-1.7207161315253598.png deleted file mode 100644 index ee5aefbbf..000000000 --- a/soccer_perception/images/fieldlines/img2_-1.170645594381017_1.6704487675687467_-1.7207161315253598.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9ac5a56e882e0a7ad472e642cc92a620888181bd9b95b5c08b0e07203502e021 -size 522347 diff --git a/soccer_perception/images/fieldlines/img3_-0.22778119664751917_0.7841084784936343_-0.3400951076777976.png b/soccer_perception/images/fieldlines/img3_-0.22778119664751917_0.7841084784936343_-0.3400951076777976.png deleted file mode 100644 index 862fbb1a8..000000000 --- a/soccer_perception/images/fieldlines/img3_-0.22778119664751917_0.7841084784936343_-0.3400951076777976.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ef8dde10c244ed5cf199ef095aa43b54f3ad84b5609b93a8723e64dbc1e15cc7 -size 551047 diff --git a/soccer_perception/images/fieldlines/img4_1.4594846269094557_1.495115016609354_-0.5027702268566427.png b/soccer_perception/images/fieldlines/img4_1.4594846269094557_1.495115016609354_-0.5027702268566427.png deleted file mode 100644 index 76c6a0579..000000000 --- a/soccer_perception/images/fieldlines/img4_1.4594846269094557_1.495115016609354_-0.5027702268566427.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ed87ca00f6f060665ba0e5ffe452699cc9616cd3fd4a7ea85aeb94c7fceb7870 -size 560120 diff --git a/soccer_perception/images/fieldlines/img5_-0.9405160632232356_-2.003114586611703_2.5573300113397712.png b/soccer_perception/images/fieldlines/img5_-0.9405160632232356_-2.003114586611703_2.5573300113397712.png deleted file mode 100644 index d5b58aaf7..000000000 --- a/soccer_perception/images/fieldlines/img5_-0.9405160632232356_-2.003114586611703_2.5573300113397712.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e8705d97b3b0d5e1f46d48f66b0813af502a278e60d1a9cae5b0a2ada01c88f7 -size 553236 diff --git a/soccer_perception/images/fieldlines/img6_-1.1265115856779753_-0.9692340250080882_-0.04040952933386244.png b/soccer_perception/images/fieldlines/img6_-1.1265115856779753_-0.9692340250080882_-0.04040952933386244.png deleted file mode 100644 index f551220ac..000000000 --- a/soccer_perception/images/fieldlines/img6_-1.1265115856779753_-0.9692340250080882_-0.04040952933386244.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:81fc8bc4dff1f56c001d668a4ba9d0df27fc53842342eeda8b3569c1308f14dd -size 550304 diff --git a/soccer_perception/images/fieldlines/img7_-0.5656089451711844_-1.973872686616836_-2.1962339783923808.png b/soccer_perception/images/fieldlines/img7_-0.5656089451711844_-1.973872686616836_-2.1962339783923808.png deleted file mode 100644 index 25d5fa70c..000000000 --- a/soccer_perception/images/fieldlines/img7_-0.5656089451711844_-1.973872686616836_-2.1962339783923808.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cf63d799671ba7ab1adf987d9f9c3d3575631e3d49429040f4d3acc1feb50a0b -size 519107 diff --git a/soccer_perception/images/fieldlines/img8_1.1242042670726646_0.5160025182546568_1.0373781171521408.png b/soccer_perception/images/fieldlines/img8_1.1242042670726646_0.5160025182546568_1.0373781171521408.png deleted file mode 100644 index 724a34549..000000000 --- a/soccer_perception/images/fieldlines/img8_1.1242042670726646_0.5160025182546568_1.0373781171521408.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:12cdd133b29dbf517182654f8d9bc29cd4ae3956fa3cde0a87f776764d797b02 -size 537319 diff --git a/soccer_perception/images/fieldlines/img9_-0.09214002310303626_-0.4566688643207222_-0.38770547262037836.png b/soccer_perception/images/fieldlines/img9_-0.09214002310303626_-0.4566688643207222_-0.38770547262037836.png deleted file mode 100644 index d990d414d..000000000 --- a/soccer_perception/images/fieldlines/img9_-0.09214002310303626_-0.4566688643207222_-0.38770547262037836.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2ab4cf71a2f762be4a118f53ff9d34c4cd7c1132395f6633938ac35af206c624 -size 550888 diff --git a/soccer_perception/soccer_object_detection/doc/ClassDiagrams.md b/soccer_perception/soccer_object_detection/doc/ClassDiagrams.md new file mode 100644 index 000000000..bfcc304ba --- /dev/null +++ b/soccer_perception/soccer_object_detection/doc/ClassDiagrams.md @@ -0,0 +1,40 @@ +# Documentation + +### + +```mermaid +%%{init: {"classDiagram": {"useMaxWidth": false}} }%% +classDiagram + direction LR + namespace Detection { + class ObjectDetectionNode + class ObjectDetectionNodeRos + + } + + namespace Camera { + class CameraBase + class CameraCalculations + class CameraCalculationsRos + + } + +%% Detector <|-- DetectorFieldline + ObjectDetectionNode <|-- ObjectDetectionNodeRos +%% DetectorFieldlineRos --|> DetectorFieldline + +%% CameraCalculations --|> CameraBase + CameraBase <|-- CameraCalculations + + CameraCalculations <|-- CameraCalculationsRos + +%% CameraCalculations <|--* DetectorFieldline + ObjectDetectionNode <|--* CameraCalculations + + ObjectDetectionNodeRos <|--* CameraCalculationsRos +%% CameraCalculationsRos <|--* DetectorFieldlineRos + + + + +``` diff --git a/soccer_perception/soccer_object_detection/doc/FlowCharts.md b/soccer_perception/soccer_object_detection/doc/FlowCharts.md new file mode 100644 index 000000000..9e8eb8a95 --- /dev/null +++ b/soccer_perception/soccer_object_detection/doc/FlowCharts.md @@ -0,0 +1,40 @@ +# Documentation + +### Image Callback + +```mermaid +%%{init: {"flowchart": {"useMaxWidth": false}} }%% +flowchart + + O1[/Image/] + O2[/Box msg/] + + I1([Image]) + + + A[Calc cover horizon] + B[Preprocess image] + C[Inference model] + D[Loop through all predictions] + E{{If class is robot}} + F[Set obstacle to true] + G[Add box to msg] + H[Render & filter detection image] + + + + I1 --> A + A --Mat--> B + B --Mat--> C + C --predictions--> D + D --yes--> E + E --yes--> F + E --no --> G + F --> G + G --> D + D --no --> H + H --tf--> O2 + H --Mat--> O1 + + +``` diff --git a/soccer_perception/soccer_object_detection/doc/README.md b/soccer_perception/soccer_object_detection/doc/README.md new file mode 100644 index 000000000..11596fa21 --- /dev/null +++ b/soccer_perception/soccer_object_detection/doc/README.md @@ -0,0 +1,4 @@ +| Class | Responsibility | +| ------------------------ | ------------------------------------------------------------- | +| `ObjectDetectionNode` | Class for classifying ball, robot, and publish bounding boxes | +| `ObjectDetectionNodeRos` | Ros bridge | diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/dfsv.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/dfsv.py deleted file mode 100644 index 96d39f065..000000000 --- a/soccer_perception/soccer_object_detection/src/soccer_object_detection/dfsv.py +++ /dev/null @@ -1,23 +0,0 @@ -import cv2 as cv -import numpy as np - -cap = cv.VideoCapture(4) -if not cap.isOpened(): - print("Cannot open camera") - exit() -while True: - # Capture frame-by-frame - ret, frame = cap.read() - # if frame is read correctly ret is True - if not ret: - print("Can't receive frame (stream end?). Exiting ...") - break - # Our operations on the frame come here - gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) - # Display the resulting frame - cv.imshow("frame", gray) - if cv.waitKey(1) == ord("q"): - break -# When everything done, release the capture -cap.release() -cv.destroyAllWindows() diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py index 5aa1f0131..af20c72c8 100755 --- a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py +++ b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py @@ -1,29 +1,16 @@ #!/usr/bin/env python3 import enum -import os -from math import nan import numpy as np -from matplotlib import pyplot as plt -from rospy.impl.tcpros_base import DEFAULT_BUFF_SIZE - -if "ROS_NAMESPACE" not in os.environ: - os.environ["ROS_NAMESPACE"] = "/robot1" - -from argparse import ArgumentParser - -import cv2 -import rospy import torch -from cv_bridge import CvBridge -from rospy import ROSException -from sensor_msgs.msg import Image +from cv2 import Mat -from soccer_common.camera import Camera -from soccer_msgs.msg import BoundingBox, BoundingBoxes, GameState, RobotState +from soccer_common import CameraCalculations +from soccer_msgs.msg import BoundingBox, BoundingBoxes +# TODO should be somewhere else class Label(enum.IntEnum): # Defines output channels of model # Refer to class name enumeration in soccer_object_detection/config/Torso21.yaml @@ -49,19 +36,16 @@ class bcolors: UNDERLINE = "\033[4m" -class ObjectDetectionNode(object): +class ObjectDetectionNode: """ - Detect ball, robot - publish bounding boxes + Class for classifying ball, robot, and publish bounding boxes input: 480x640x4 bgra8 -> output: 3x200x150 """ def __init__(self, model_path): - self.SOCCER_BALL = 0 - # ROS, replace with default - # self.CONFIDENCE_THRESHOLD = rospy.get_param("~ball_confidence_threshold", 0.75) self.CONFIDENCE_THRESHOLD = 0.75 + self.cover_horizon_up_threshold = 30 torch.hub._brvalidate_not_a_forked_repo = ( lambda a, b, c: True @@ -69,61 +53,19 @@ def __init__(self, model_path): self.model = torch.hub.load("ultralytics/yolov5", "custom", path=model_path) # ROS - # if torch.cuda.is_available(): - # rospy.loginfo(f"{bcolors.OKGREEN}Using CUDA for object detection{bcolors.ENDC}") - # self.model.cuda() - # else: - # rospy.logwarn("Not using CUDA") - - self.robot_name = rospy.get_namespace()[1:-1] # remove '/' - # self.camera = Camera(self.robot_name) - self.camera = Camera() - self.camera.reset_position() + if torch.cuda.is_available(): + self.model.cuda() # TODO does this do anything - # Params - self.br = CvBridge() + self.camera = CameraCalculations() + self.camera.reset_position() - # ROS - # self.pub_detection = rospy.Publisher("detection_image", Image, queue_size=1, latch=True) - # self.pub_boundingbox = rospy.Publisher("object_bounding_boxes", BoundingBoxes, queue_size=1, latch=True) - # self.image_subscriber = rospy.Subscriber( - # "camera/image_raw", Image, self.callback, queue_size=1, buff_size=DEFAULT_BUFF_SIZE * 64 - # ) # Large buff size (https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/) - # self.robot_state_subscriber = rospy.Subscriber("state", RobotState, self.robot_state_callback) - self.robot_state = RobotState() - # self.game_state_subscriber = rospy.Subscriber("gamestate", GameState, self.game_state_callback) - self.game_state = GameState() - - def game_state_callback(self, game_state: GameState): - self.game_state = game_state - - def robot_state_callback(self, robot_state: RobotState): - self.robot_state = robot_state - - def callback(self, image: Image): # msg -> image + def get_model_output(self, image: Mat) -> [Mat, BoundingBoxes]: # webots: 480x640x4pixels - if self.robot_state.status not in [ - RobotState.STATUS_LOCALIZING, - RobotState.STATUS_READY, - RobotState.ROLE_UNASSIGNED, - ]: - return - - if self.game_state.gameState != GameState.GAMESTATE_PLAYING: - return - - # ROS - # rospy.loginfo_once("Object Detection Receiving image") - # width x height x channels (bgra8) - # image = self.br.imgmsg_to_cv2(msg) - # self.camera.reset_position(timestamp=msg.header.stamp) # msg->image - # ROS, replace with default value # cover horizon to help robot ignore things outside field - # cover_horizon_up_threshold = rospy.get_param("cover_horizon_up_threshold", 30) - cover_horizon_up_threshold = 30 - h = max(self.camera.calculateHorizonCoverArea() - cover_horizon_up_threshold, 0) - + # TODO do we need a cover horizon? + h = max(self.camera.calculate_horizon_cover_area() - self.cover_horizon_up_threshold, 0) + # h = 0 if image is not None: # 1. preprocess image img = image[:, :, :3] # get rid of alpha channel @@ -133,10 +75,11 @@ def callback(self, image: Image): # msg -> image results = self.model(img) - # bbs_msg = BoundingBoxes() + # TODO should be a func + bbs_msg = BoundingBoxes() id = 0 for prediction in results.xyxy[0]: - x1, y1, x2, y2, confidence, img_class = prediction.cpu().numpy() + x1, y1, x2, y2, confidence, img_class = prediction.cpu().numpy() # TODO cuda? y1 += h + 1 y2 += h + 1 if img_class in [label.value for label in Label] and confidence > self.CONFIDENCE_THRESHOLD: @@ -161,58 +104,16 @@ def callback(self, image: Image): # msg -> image bb_msg.ybase = round(midpoint[1]) bb_msg.xbase = round(midpoint[0]) bb_msg.obstacle_detected = True - - # bbs_msg.bounding_boxes.append(bb_msg) + bbs_msg.bounding_boxes.append(bb_msg) id += 1 - # cv2.imshow("test", results) - - # Draw bounding box on image cv2 test - # cv2.rectangle(image, (bb_msg.xmin, bb_msg.ymin), (bb_msg.xmax, bb_msg.ymax), (0, 255, 0), 2) - # label = f'{bb_msg.Class}: {bb_msg.probability:.2f}' - # cv2.putText(image, label, (bb_msg.xmin, bb_msg.ymin - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, - # (0, 255, 0), 2) - # # id+=1 - # # Optionally, display or save the image - # cv2.imshow("Detected Objec ts", image) - # cv2.waitKey(1) - detection_image = np.squeeze(results.render()) - # detection_image = np.concatenate((np.zeros((h + 1, image.width, 3), detection_image.dtype), detection_image)) # msg -> image + # TODO needed for cover horizon but that might not be needed + detection_image = np.concatenate((np.zeros((h + 1, self.camera.camera_info.width, 3), detection_image.dtype), detection_image)) detection_image = detection_image[..., ::-1] - # cv2.imshow('Object Detection', detection_image) - - # ROS - # bbs_msg.header = msg.header # msg -> image - # try: - # if self.pub_detection.get_num_connections() > 0: - # detection_image = np.squeeze(results.render()) - # # detection_image = np.concatenate((np.zeros((h + 1, msg.width, 3), detection_image.dtype), detection_image)) # msg -> image - # - # detection_image = detection_image[..., ::-1] # convert rgb to bgr - # self.pub_detection.publish(self.br.cv2_to_imgmsg(detection_image, encoding="bgr8")) - - # if self.pub_boundingbox.get_num_connections() > 0 and len(bbs_msg.bounding_boxes) > 0: - # self.pub_boundingbox.publish(bbs_msg) - - # except ROSException as re: - # print(re) - # exit(0) - - return detection_image + return detection_image, bbs_msg if __name__ == "__main__": - parser = ArgumentParser() - parser.add_argument("--model", dest="model_path", default="../../models/July14.pt", help="pytorch model") - parser.add_argument("--num-feat", dest="num_feat", default=10, help="specify model size of the neural network") - args, unknown = parser.parse_known_args() - - rospy.init_node("object_detector") - my_node = ObjectDetectionNode(args.model_path) - - try: - rospy.spin() - except ROSException as rx: - exit(0) + pass diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py new file mode 100755 index 000000000..3cd97feb2 --- /dev/null +++ b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 + +import os +from os.path import expanduser + +from rospy.impl.tcpros_base import DEFAULT_BUFF_SIZE +from soccer_object_detection.object_detect_node import ObjectDetectionNode, bcolors + +from soccer_common.perception.camera_calculations_ros import CameraCalculationsRos + +if "ROS_NAMESPACE" not in os.environ: + os.environ["ROS_NAMESPACE"] = "/robot1" + +from argparse import ArgumentParser + +import rospy +import torch +from cv_bridge import CvBridge +from rospy import ROSException +from sensor_msgs.msg import Image + +from soccer_msgs.msg import BoundingBoxes + +# TODO should be somewhere else + + +class ObjectDetectionNodeRos(ObjectDetectionNode): + """ + Ros bridge + input: 480x640x4 bgra8 -> output: 3x200x150 + """ + + def __init__(self, model_path): + super(ObjectDetectionNodeRos, self).__init__(model_path) + self.cover_horizon_up_threshold = rospy.get_param("cover_horizon_up_threshold", 30) + self.CONFIDENCE_THRESHOLD = rospy.get_param("~ball_confidence_threshold", 0.75) + + if torch.cuda.is_available(): + rospy.loginfo(f"{bcolors.OKGREEN}Using CUDA for object detection{bcolors.ENDC}") + + self.robot_name = rospy.get_namespace()[1:-1] # remove '/' + self.camera = CameraCalculationsRos(self.robot_name) + self.camera.reset_position() + + # Params + self.br = CvBridge() + + # ROS + self.pub_detection = rospy.Publisher("detection_image", Image, queue_size=1, latch=True) + self.pub_boundingbox = rospy.Publisher("object_bounding_boxes", BoundingBoxes, queue_size=1, latch=True) + self.image_subscriber = rospy.Subscriber( + "camera/image_raw", Image, self.callback, queue_size=1, buff_size=DEFAULT_BUFF_SIZE * 64 + ) # Large buff size (https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/) + + # self.game_state_subscriber = rospy.Subscriber("gamestate", GameState, self.game_state_callback) + # self.game_state = GameState() + # + # def game_state_callback(self, game_state: GameState): + # self.game_state = game_state + + def callback(self, msg: Image): + # webots: 480x640x4pixels + # TODo should be in strategy + # if self.robot_state.status not in [ + # RobotState.STATUS_LOCALIZING, + # RobotState.STATUS_READY, + # RobotState.ROLE_UNASSIGNED, + # ]: + # return + # + # if self.game_state.gameState != GameState.GAMESTATE_PLAYING: + # return + + rospy.loginfo_once("Object Detection Receiving image") + # width x height x channels (bgra8) + image = self.br.imgmsg_to_cv2(msg) + self.camera.reset_position(timestamp=msg.header.stamp) # msg->image + + detection_image, bbs_msg = self.get_model_output(image) + + bbs_msg.header = msg.header + try: + if self.pub_detection.get_num_connections() > 0: + self.pub_detection.publish(self.br.cv2_to_imgmsg(detection_image, encoding="bgr8")) + + if self.pub_boundingbox.get_num_connections() > 0 and len(bbs_msg.bounding_boxes) > 0: + self.pub_boundingbox.publish(bbs_msg) + + except ROSException as re: + print(re) + exit(0) + + +if __name__ == "__main__": + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + parser = ArgumentParser() + parser.add_argument("--model", dest="model_path", default=model_path, help="pytorch model") + parser.add_argument("--num-feat", dest="num_feat", default=10, help="specify model size of the neural network") + args, unknown = parser.parse_known_args() + rospy.init_node("object_detector") + my_node = ObjectDetectionNodeRos(args.model_path) + + try: + rospy.spin() + except ROSException as rx: + exit(0) diff --git a/soccer_perception/soccer_object_detection/test/test_object_detection.py b/soccer_perception/soccer_object_detection/test/test_object_detection.py index 0b0a52d3a..7b02bd556 100644 --- a/soccer_perception/soccer_object_detection/test/test_object_detection.py +++ b/soccer_perception/soccer_object_detection/test/test_object_detection.py @@ -1,171 +1,91 @@ +import math import os import os.path import pickle import sys +from os.path import expanduser from unittest import TestCase -from unittest.mock import MagicMock import cv2 import numpy as np import pytest import rospy -import tf2_ros import yaml from cv2 import Mat -from cv_bridge import CvBridge -from sensor_msgs.msg import CameraInfo, Image from soccer_object_detection.object_detect_node import Label, ObjectDetectionNode -from soccer_common import Camera -from soccer_common.utils import download_dataset -from soccer_common.utils_rosparam import set_rosparam_from_yaml_file -from soccer_msgs.msg import GameState, RobotState - -set_rosparam_from_yaml_file() - - -def IoU(boxA, boxB): - # determine the (x, y)-coordinates of the intersection rectangle - xA = max(boxA[0], boxB[0]) - yA = max(boxA[1], boxB[1]) - xB = min(boxA[2], boxB[2]) - yB = min(boxA[3], boxB[3]) - # compute the area of intersection rectangle - interArea = max(0, xB - xA + 1) * max(0, yB - yA + 1) - # compute the area of both the prediction and ground-truth - # rectangles - boxAArea = (boxA[2] - boxA[0] + 1) * (boxA[3] - boxA[1] + 1) - boxBArea = (boxB[2] - boxB[0] + 1) * (boxB[3] - boxB[1] + 1) - # compute the intersection over union by taking the intersection - # area and dividing it by the sum of prediction + ground-truth - # areas - the interesection area - iou = interArea / float(boxAArea + boxBArea - interArea) - # return the intersection over union value - return iou +from soccer_common import Transformation +from soccer_common.utils import download_dataset, wrapToPi +from soccer_perception.soccer_object_detection.test.utils import check_bounding_box class TestObjectDetection(TestCase): - def test_object_detection_node(self): - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/simulation" - download_dataset("https://drive.google.com/uc?id=11nN58j8_PBoLNRAzOEdk7fMe1UK1diCc", folder_path=test_path) + def test_object_detection(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/images/simulation" - rospy.init_node("test") - - Camera.reset_position = MagicMock() + download_dataset("https://drive.google.com/uc?id=11nN58j8_PBoLNRAzOEdk7fMe1UK1diCc", folder_path=test_path) - src_path = os.path.dirname(os.path.realpath(__file__)) - model_path = src_path + "/../../models/half_5.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" n = ObjectDetectionNode(model_path=model_path) - n.robot_state.status = RobotState.STATUS_READY - n.game_state.gameState = GameState.GAMESTATE_PLAYING - - cvbridge = CvBridge() for file_name in os.listdir(f"{test_path}/images"): print(file_name) img: Mat = cv2.imread(os.path.join(f"{test_path}/images", file_name)) # ground truth box = (68, 89) (257, 275) - img_original_size = img.size img = cv2.resize(img, dsize=(640, 480)) - img_msg: Image = cvbridge.cv2_to_imgmsg(img) - - # Mock the detections - n.pub_detection = MagicMock() - n.pub_boundingbox = MagicMock() - n.pub_detection.get_num_connections = MagicMock(return_value=1) - n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) - n.pub_detection.publish = MagicMock() - n.pub_boundingbox.publish = MagicMock() - - ci = CameraInfo() - ci.height = img.shape[0] - ci.width = img.shape[1] - n.camera.camera_info = ci n.camera.pose.orientation_euler = [0, np.pi / 8, 0] - n.callback(img_msg) + detection_image, bbs_msg = n.get_model_output(img) with open(os.path.join(f"{test_path}/labels", file_name.replace("PNG", "txt"))) as f: lines = f.readlines() - if "DISPLAY" in os.environ: - mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) - cv2.imshow("Image", mat) - cv2.waitKey(1000) - cv2.destroyAllWindows() - # Check assertion - if n.pub_boundingbox.publish.call_args is not None: - for bounding_box in n.pub_boundingbox.publish.call_args[0][0].bounding_boxes: - if bounding_box.probability >= n.CONFIDENCE_THRESHOLD and int(bounding_box.Class) in [Label.BALL.value, Label.ROBOT.value]: - bounding_boxes = [ - bounding_box.xmin, - bounding_box.ymin, - bounding_box.xmax, - bounding_box.ymax, - ] - - best_iou = 0 - best_dimensions = None - for line in lines: - info = line.split(" ") - label = int(info[0]) - if label != int(bounding_box.Class): - continue - - x = float(info[1]) - y = float(info[2]) - width = float(info[3]) - height = float(info[4]) - - xmin = int((x - width / 2) * ci.width) - ymin = int((y - height / 2) * ci.height) - xmax = int((x + width / 2) * ci.width) - ymax = int((y + height / 2) * ci.height) - ground_truth_boxes = [xmin, ymin, xmax, ymax] - iou = IoU(bounding_boxes, ground_truth_boxes) - if iou > best_iou: - best_iou = iou - best_dimensions = ground_truth_boxes - - self.assertGreater(best_iou, 0.05, f"bounding boxes are off by too much! Image= {file_name} Best IOU={best_iou}") - if best_iou < 0.5: - rospy.logwarn(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") - - # if "DISPLAY" in os.environ: - # cv2.rectangle( - # img=mat, - # pt1=(best_dimensions[0], best_dimensions[1]), - # pt2=(best_dimensions[2], best_dimensions[3]), - # color=(255, 255, 255), - # ) - # if bounding_box.obstacle_detected is True: - # cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) + # TODO is this really necessary + # Also can it be a function + for bounding_box in bbs_msg.bounding_boxes: + if bounding_box.probability >= n.CONFIDENCE_THRESHOLD and int(bounding_box.Class) in [ + Label.BALL.value, + Label.ROBOT.value, + Label.GOALPOST.value, + Label.TOPBAR.value, + ]: + best_iou = check_bounding_box(bounding_box, lines, n.camera.camera_info.width, n.camera.camera_info.height) + + self.assertGreater(best_iou, 0.05, f"bounding boxes are off by too much! Image= {file_name}" f" Best IOU={best_iou}") + if best_iou < 0.5: + print(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") + # if "DISPLAY" in os.environ: + # cv2.rectangle( + # img=img, + # pt1=(bounding_box.xmin, bounding_box.ymin), + # pt2=(bounding_box.xmax, bounding_box.ymax), + # color=(255, 255, 255), + # ) + # if bounding_box.obstacle_detected is True: + # cv2.circle(img, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) if "DISPLAY" in os.environ: - cv2.imshow("Image", mat) + cv2.imshow("Image", detection_image) cv2.waitKey() cv2.destroyAllWindows() - def test_object_detection_node_cam(self): + def test_object_detection_vid(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/videos/robocup2023" - rospy.init_node("test") + download_dataset("https://drive.google.com/uc?id=1UTQ6Rz0yk8jpWwWoq3eSf7DOmG_j9An3", folder_path=test_path) - Camera.reset_position = MagicMock() - - src_path = os.path.dirname(os.path.realpath(__file__)) - model_path = src_path + "/../../models/half_5.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" n = ObjectDetectionNode(model_path=model_path) - n.robot_state.status = RobotState.STATUS_READY - n.game_state.gameState = GameState.GAMESTATE_PLAYING - cap = cv2.VideoCapture(4) + cap = cv2.VideoCapture(test_path + "/2023-07-08-124521.webm") if not cap.isOpened(): print("Cannot open camera") exit() - cvbridge = CvBridge() + while True: ret, frame = cap.read() if not ret: @@ -173,84 +93,45 @@ def test_object_detection_node_cam(self): break img = cv2.resize(frame, dsize=(640, 480)) - img_msg: Image = cvbridge.cv2_to_imgmsg(img) - - # Mock the detections - n.pub_detection = MagicMock() - n.pub_boundingbox = MagicMock() - n.pub_detection.get_num_connections = MagicMock(return_value=1) - n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) - n.pub_detection.publish = MagicMock() - n.pub_boundingbox.publish = MagicMock() - - ci = CameraInfo() - ci.height = img.shape[0] - ci.width = img.shape[1] - n.camera.camera_info = ci n.camera.pose.orientation_euler = [0, np.pi / 8, 0] - n.callback(img_msg) + detection_image, bbs_msg = n.get_model_output(img) # 0.01 if "DISPLAY" in os.environ: - mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) - cv2.imshow("Image", mat) - cv2.waitKey(1) - # cv2.destroyAllWindows() + cv2.imshow("Image", detection_image) + cv2.waitKey(24) # TODO why is this one so much faster + cv2.destroyAllWindows() - # Check assertion - if n.pub_boundingbox.publish.call_args is not None: - for bounding_box in n.pub_boundingbox.publish.call_args[0][0].bounding_boxes: - if bounding_box.probability >= n.CONFIDENCE_THRESHOLD and int(bounding_box.Class) in [Label.BALL.value, Label.ROBOT.value]: - bounding_boxes = [ - bounding_box.xmin, - bounding_box.ymin, - bounding_box.xmax, - bounding_box.ymax, - ] - - best_iou = 0 - best_dimensions = None - # for line in lines: - # info = line.split(" ") - # label = int(info[0]) - # if label != int(bounding_box.Class): - # continue - # - # x = float(info[1]) - # y = float(info[2]) - # width = float(info[3]) - # height = float(info[4]) - # - # xmin = int((x - width / 2) * ci.width) - # ymin = int((y - height / 2) * ci.height) - # xmax = int((x + width / 2) * ci.width) - # ymax = int((y + height / 2) * ci.height) - # ground_truth_boxes = [xmin, ymin, xmax, ymax] - # iou = IoU(bounding_boxes, ground_truth_boxes) - # if iou > best_iou: - # best_iou = iou - # best_dimensions = ground_truth_boxes - - # self.assertGreater(best_iou, 0.05, f"bounding boxes are off by too much! Image= {file_name} Best IOU={best_iou}") - # if best_iou < 0.5: - # rospy.logwarn(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") - - # if "DISPLAY" in os.environ: - # cv2.rectangle( - # img=mat, - # pt1=(best_dimensions[0], best_dimensions[1]), - # pt2=(best_dimensions[2], best_dimensions[3]), - # color=(255, 255, 255), - # ) - # if bounding_box.obstacle_detected is True: - # cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) + def test_object_detection_node_cam(self): + + src_path = os.path.dirname(os.path.realpath(__file__)) + + model_path = src_path + "/../models/half_5.pt" + + n = ObjectDetectionNode(model_path=model_path) + + cap = cv2.VideoCapture(4) + if not cap.isOpened(): + print("Cannot open camera") + exit() + + while True: + ret, frame = cap.read() + if not ret: + print("Can't receive frame (stream end?). Exiting ...") + break + img = cv2.resize(frame, dsize=(640, 480)) + + n.camera.pose.orientation_euler = [0, np.pi / 8, 0] + detection_image, bbs_msg = n.get_model_output(img) # 0.01 if "DISPLAY" in os.environ: - cv2.imshow("Image", mat) - cv2.waitKey(1) - # cv2.destroyAllWindows() + cv2.imshow("Image", detection_image) + cv2.waitKey(24) # TODO why is this one so much faster + cv2.destroyAllWindows() @pytest.mark.skip(reason="Only run locally") def test_visualize_annotations(self): + # TODO what does this do? src_path = os.path.dirname(os.path.realpath(__file__)) # Data downloaded from https://github.com/bit-bots/TORSO_21_dataset @@ -435,3 +316,96 @@ def test_visualize_annotations(self): sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") print(f"Current image number {i} name {f}\n") + + def test_goalpost_detection(self): + """ + Returns whether a point at a given field coordinate is visible to the robot + """ + + # TODO verify how this works + def get_point_visibility(robot_pose, point_coords): + robot_x, robot_y, robot_yaw = robot_pose + point_x, point_y = point_coords + + point_yaw = math.atan2(point_y - robot_y, point_x - robot_x) + camera_fov = 1.39626 # rads + + # Both yaw angles are between -pi and pi + delta_yaw = wrapToPi(point_yaw - robot_yaw) + + # Check if the point is within the view cone + # No equals case as the point wouldn't be fully visible + is_point_visible = -camera_fov / 2.0 < delta_yaw < camera_fov / 2.0 + + return is_point_visible + + """ + Returns a dictionary that stores booleans indicating whether each post is visible + Visual reference: https://www.desmos.com/calculator/b9lndsb1bl + Example: both posts of the left net are visible + visible_posts = { + "NEG_X_NET": { + "POS_Y_POST": True, + "NEG_Y_POST": True + }, + "POS_X_NET": { + "POS_Y_POST": False, + "NEG_Y_POST": False + } + } + """ + + def get_visible_posts(robot_x, robot_y, robot_yaw): + visible_posts = {"NEG_X_NET": {"POS_Y_POST": True, "NEG_Y_POST": True}, "POS_X_NET": {"POS_Y_POST": False, "NEG_Y_POST": False}} + + net_coords = { + "NEG_X_NET": {"POS_Y_POST": [-4.5, 1.3], "NEG_Y_POST": [-4.5, -1.3]}, + "POS_X_NET": {"POS_Y_POST": [4.5, 1.3], "NEG_Y_POST": [4.5, -1.3]}, + } + + for net in net_coords.keys(): + post_coords = net_coords[net] + for post in post_coords.keys(): + visible_posts[net][post] = get_point_visibility((robot_x, robot_y, robot_yaw), net_coords[net][post]) + + return visible_posts + + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/images/goal_net" + + download_dataset("https://drive.google.com/uc?id=17qdnW7egoopXHvakiNnUUufP2MOjyZ18", folder_path=test_path) + + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + n = ObjectDetectionNode(model_path=model_path) + + n.camera.pose = Transformation(position=[0, 0, 0.46]) + + # Loop through test images + for file_name in os.listdir(test_path): + # file_name = "img173_-0.852141317992289_3.15_-1.7125376246657054.png" + + print(f"Loading {file_name} from goal_net dataset") + file_name_no_ext = os.path.splitext(file_name)[0] + x, y, yaw = file_name_no_ext.split("_")[1:] + yaw = wrapToPi(float(yaw)) + if yaw < 0: + yaw = (yaw + np.pi) % np.pi + + n.camera.pose.orientation_euler = [yaw, 0, 0] + print(f"Parsed (x, y, yaw): ({x}, {y}, {yaw}) from filename.") + visible_posts = get_visible_posts(float(x), float(y), float(yaw)) + for net in visible_posts.keys(): + for post in visible_posts[net].keys(): + if visible_posts[net][post]: + print(f"{net}, {post} is visible") + + img: Mat = cv2.imread(os.path.join(test_path, file_name)) + + if "DISPLAY" in os.environ: + cv2.imshow("Before", img) + + detection_image, bbs_msg = n.get_model_output(img) + if "DISPLAY" in os.environ: + cv2.imshow("After", detection_image) + cv2.waitKey(0) diff --git a/soccer_perception/soccer_object_detection/test/test_object_detection_ros.py b/soccer_perception/soccer_object_detection/test/test_object_detection_ros.py new file mode 100644 index 000000000..9fac595eb --- /dev/null +++ b/soccer_perception/soccer_object_detection/test/test_object_detection_ros.py @@ -0,0 +1,89 @@ +import os +import os.path +from os.path import expanduser +from unittest import TestCase +from unittest.mock import MagicMock + +import cv2 +import numpy as np +import rospy +from cv2 import Mat +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from soccer_object_detection.object_detect_node import Label +from soccer_object_detection.object_detect_node_ros import ObjectDetectionNodeRos + +from soccer_common.perception.camera_calculations_ros import CameraCalculationsRos +from soccer_common.utils import download_dataset +from soccer_common.utils_rosparam import set_rosparam_from_yaml_file +from soccer_perception.soccer_object_detection.test.utils import check_bounding_box + +set_rosparam_from_yaml_file() + + +class TestObjectDetectionRos(TestCase): + # TODO should these just be simple usage + def test_object_detection_node_ros(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/images/simulation" + + download_dataset("https://drive.google.com/uc?id=11nN58j8_PBoLNRAzOEdk7fMe1UK1diCc", folder_path=test_path) + + model_path = src_path + "soccer_object_detection/models/half_5.pt" + rospy.init_node("test") + + CameraCalculationsRos.reset_position = MagicMock() + n = ObjectDetectionNodeRos(model_path=model_path) + + cvbridge = CvBridge() + for file_name in os.listdir(f"{test_path}/images"): + print(file_name) + img: Mat = cv2.imread(os.path.join(f"{test_path}/images", file_name)) # ground truth box = (68, 89) (257, 275) + img = cv2.resize(img, dsize=(640, 480)) + + img_msg: Image = cvbridge.cv2_to_imgmsg(img) + + # Mock the detections + n.pub_detection = MagicMock() + n.pub_boundingbox = MagicMock() + n.pub_detection.get_num_connections = MagicMock(return_value=1) + n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) + n.pub_detection.publish = MagicMock() + n.pub_boundingbox.publish = MagicMock() + + n.camera.pose.orientation_euler = [0, np.pi / 8, 0] + n.callback(img_msg) + + with open(os.path.join(f"{test_path}/labels", file_name.replace("PNG", "txt"))) as f: + lines = f.readlines() + + if "DISPLAY" in os.environ: + mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) + # cv2.imshow("Image", mat) + # cv2.waitKey(1000) + # cv2.destroyAllWindows() + + # Check assertion + if n.pub_boundingbox.publish.call_args is not None: + for bounding_box in n.pub_boundingbox.publish.call_args[0][0].bounding_boxes: + if bounding_box.probability >= n.CONFIDENCE_THRESHOLD and int(bounding_box.Class) in [Label.BALL.value, Label.ROBOT.value]: + best_iou = check_bounding_box(bounding_box, lines, n.camera.camera_info.width, n.camera.camera_info.height) + + self.assertGreater(best_iou, 0.05, f"bounding boxes are off by too much! Image= {file_name} Best IOU={best_iou}") + if best_iou < 0.5: + rospy.logwarn(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") + + # if "DISPLAY" in os.environ: + # cv2.rectangle( + # img=mat, + # pt1=(best_dimensions[0], best_dimensions[1]), + # pt2=(best_dimensions[2], best_dimensions[3]), + # color=(255, 255, 255), + # ) + # if bounding_box.obstacle_detected is True: + # cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) + + if "DISPLAY" in os.environ: + cv2.imshow("Image", mat) + cv2.waitKey() + cv2.destroyAllWindows() diff --git a/soccer_perception/soccer_object_detection/test/utils.py b/soccer_perception/soccer_object_detection/test/utils.py new file mode 100644 index 000000000..952591b84 --- /dev/null +++ b/soccer_perception/soccer_object_detection/test/utils.py @@ -0,0 +1,56 @@ +from typing import AnyStr, List + +from soccer_msgs.msg import BoundingBox + + +def calc_iou(box_a, box_b): + # determine the (x, y)-coordinates of the intersection rectangle + x_a = max(box_a[0], box_b[0]) + y_a = max(box_a[1], box_b[1]) + x_b = min(box_a[2], box_b[2]) + y_b = min(box_a[3], box_b[3]) + # compute the area of intersection rectangle + inter_area = max(0, x_b - x_a + 1) * max(0, y_b - y_a + 1) + # compute the area of both the prediction and ground-truth + # rectangles + box_a_area = (box_a[2] - box_a[0] + 1) * (box_a[3] - box_a[1] + 1) + box_b_area = (box_b[2] - box_b[0] + 1) * (box_b[3] - box_b[1] + 1) + # compute the intersection over union by taking the intersection + # area and dividing it by the sum of prediction + ground-truth + # areas - the interesection area + # return the intersection over union value + return inter_area / float(box_a_area + box_b_area - inter_area) + + +def check_bounding_box(bounding_box: BoundingBox, lines: List[AnyStr], cam_width: float, cam_height: float): + bounding_boxes = [ + bounding_box.xmin, + bounding_box.ymin, + bounding_box.xmax, + bounding_box.ymax, + ] + + best_iou = 0 + best_dimensions = None + for line in lines: + info = line.split(" ") + label = int(info[0]) + if label != int(bounding_box.Class): + continue + + x = float(info[1]) + y = float(info[2]) + width = float(info[3]) + height = float(info[4]) + + xmin = int((x - width / 2) * cam_width) + ymin = int((y - height / 2) * cam_height) + xmax = int((x + width / 2) * cam_width) + ymax = int((y + height / 2) * cam_height) + ground_truth_boxes = [xmin, ymin, xmax, ymax] + iou = calc_iou(bounding_boxes, ground_truth_boxes) + if iou > best_iou: + best_iou = iou + best_dimensions = ground_truth_boxes + + return best_iou diff --git a/soccer_perception/soccer_object_localization/doc/ClassDiagrams.md b/soccer_perception/soccer_object_localization/doc/ClassDiagrams.md new file mode 100644 index 000000000..470665187 --- /dev/null +++ b/soccer_perception/soccer_object_localization/doc/ClassDiagrams.md @@ -0,0 +1,42 @@ +# Documentation + +### + +```mermaid +%%{init: {"classDiagram": {"useMaxWidth": false}} }%% +classDiagram + direction LR + namespace Fieldline { + class Detector + class DetectorFieldline + class DetectorFieldlineRos + + } + + namespace Camera { + class CameraBase + class CameraCalculations + class CameraCalculationsRos + + } + + DetectorFieldline --|> Detector +%% Detector <|-- DetectorFieldline + DetectorFieldline <|-- DetectorFieldlineRos +%% DetectorFieldlineRos --|> DetectorFieldline + +%% CameraCalculations --|> CameraBase + CameraBase <|-- CameraCalculations + + CameraCalculations <|-- CameraCalculationsRos + +%% CameraCalculations <|--* DetectorFieldline + DetectorFieldline <|--* CameraCalculations + + DetectorFieldlineRos <|--* CameraCalculationsRos +%% CameraCalculationsRos <|--* DetectorFieldlineRos + + + + +``` diff --git a/soccer_perception/soccer_object_localization/doc/FlowCharts.md b/soccer_perception/soccer_object_localization/doc/FlowCharts.md new file mode 100644 index 000000000..3c08aa50e --- /dev/null +++ b/soccer_perception/soccer_object_localization/doc/FlowCharts.md @@ -0,0 +1,45 @@ +# Documentation + +### Image Callback + +```mermaid +%%{init: {"flowchart": {"useMaxWidth": false}} }%% +flowchart + + O1[/Image/] + O2[/tf/] + O3[/PointCloud/] + + I1([Image]) + + + A[Check groundtruth] + B[Convert image to mat] + C[Filter image] + D[Convert lines to points] + E[Filter points by spacing] + F[Find floor coordinate] + G[Filter by distance to point] + H[Publish straight base link] + J[Convert mat to image] + K[Publish image] + L[Publish point cloud] + + + I1 --> A + A --Image--> B + B --Mat--> C + C --Mat--> J + J --Image--> K + K --Image--> O1 + C --Mat--> D + D --points--> E + E --points--> F + F --points--> G + G --points--> L + L --points--> O3 + L --> H + H --tf--> O2 + + +``` diff --git a/soccer_perception/soccer_object_localization/doc/README.md b/soccer_perception/soccer_object_localization/doc/README.md new file mode 100644 index 000000000..ce4493d19 --- /dev/null +++ b/soccer_perception/soccer_object_localization/doc/README.md @@ -0,0 +1,5 @@ +| Class | Responsibility | +| ---------------------- | -------------------------------------------------------------------------- | +| `Detector` | Base class for object-localization to group common functionality together. | +| `DetectorFieldline` | Class for detecting field lines and converting to pointclouds. | +| `DetectorFieldlineRos` | Ros bridge for detecting fieldlines. | diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector.py index 8f5e6e731..e72676bea 100644 --- a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector.py +++ b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector.py @@ -2,13 +2,16 @@ import numpy as np from sensor_msgs.msg import Image -from soccer_common.camera import Camera -from soccer_msgs.msg import RobotState +from soccer_common.perception.camera_calculations import CameraCalculations class Detector: + """ + Base class for object-localization to group common functionality together. + """ + def __init__(self): - self.camera = Camera() + self.camera = CameraCalculations() self.camera.reset_position() # TODO remove all mentions of state for strategy @@ -16,11 +19,10 @@ def __init__(self): def circular_mask(radius: int): return cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (radius, radius)) - def grass_mask(self, image: Image, h: int): + def grass_mask(self, image: Image, h: int, debug: bool = False): # Grass Mask # Hue > 115 needed grass_only = cv2.inRange(image, (35, 85, 0), (115, 255, 255)) - grass_only = cv2.vconcat([np.zeros((h + 1, grass_only.shape[1]), dtype=grass_only.dtype), grass_only]) # Use odd numbers for all circular masks otherwise the line will shift location grass_only_0 = cv2.morphologyEx(grass_only, cv2.MORPH_OPEN, self.circular_mask(5)) @@ -31,4 +33,20 @@ def grass_mask(self, image: Image, h: int): grass_only_morph = cv2.morphologyEx(grass_only_3, cv2.MORPH_ERODE, self.circular_mask(9)) grass_only_flipped = cv2.bitwise_not(grass_only) - return grass_only, grass_only_0, grass_only_1, grass_only_2, grass_only_3, grass_only_morph, grass_only_flipped + if debug: + cv2.imshow("grass_only", grass_only) + cv2.imwrite("/tmp/grass_only.png", grass_only) + cv2.imshow("grass_only_0", grass_only_0) + cv2.imwrite("/tmp/grass_only_0.png", grass_only_0) + cv2.imshow("grass_only_1", grass_only_1) + cv2.imwrite("/tmp/grass_only_1.png", grass_only_1) + cv2.imshow("grass_only_2", grass_only_2) + cv2.imwrite("/tmp/grass_only_2.png", grass_only_2) + cv2.imshow("grass_only_3", grass_only_3) + cv2.imwrite("/tmp/grass_only_3.png", grass_only_3) + cv2.imshow("grass_only_morph", grass_only_morph) + cv2.imwrite("/tmp/grass_only_morph.png", grass_only_morph) + cv2.imshow("grass_only_flipped", grass_only_flipped) + cv2.imwrite("/tmp/grass_only_flipped.png", grass_only_flipped) + + return grass_only_morph, grass_only_flipped diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py index 65689c0e7..f7a09f44c 100755 --- a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py +++ b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py @@ -1,84 +1,38 @@ #!/usr/bin/env python3 -import os -import time -from rospy.impl.tcpros_base import DEFAULT_BUFF_SIZE -from tf import TransformBroadcaster - -from soccer_common.transformation import Transformation -from soccer_msgs.msg import RobotState - -if "ROS_NAMESPACE" not in os.environ: - os.environ["ROS_NAMESPACE"] = "/robot1" import cv2 import numpy as np -import rospy -import sensor_msgs.point_cloud2 as pcl2 -from cv_bridge import CvBridge -from geometry_msgs.msg import PoseWithCovarianceStamped -from sensor_msgs.msg import Image, PointCloud2 +from cv2 import Mat from soccer_object_localization.detector import Detector -from std_msgs.msg import Bool, Header + +from soccer_common.transformation import Transformation class DetectorFieldline(Detector): + """ + Class for detecting field lines and converting to pointclouds. + """ + def __init__(self): super().__init__() - - # self.initial_pose_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.initial_pose_callback, queue_size=1) - # self.image_subscriber = rospy.Subscriber( - # "camera/image_raw", Image, self.image_callback, queue_size=1, buff_size=DEFAULT_BUFF_SIZE * 64 - # ) # Large buff size (https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/) - # self.image_publisher = rospy.Publisher("camera/line_image", Image, queue_size=1) - # self.point_cloud_publisher = rospy.Publisher("field_point_cloud", PointCloud2, queue_size=1) - self.tf_broadcaster = TransformBroadcaster() - - # self.point_cloud_max_distance = rospy.get_param("point_cloud_max_distance", 5) - # self.point_cloud_spacing = rospy.get_param("point_cloud_spacing", 30) - self.publish_point_cloud = False + self.point_cloud_max_distance = 5 + self.point_cloud_spacing = 30 + self.publish_point_cloud = True # TODO is this necessary? self.ground_truth = False cv2.setRNGSeed(12345) - pass - - def initial_pose_callback(self, initial_pose: PoseWithCovarianceStamped): - self.publish_point_cloud = True - - def image_callback(self, image: Image, debug=False): - - t_start = time.time() - - if self.robot_state.status not in [ - RobotState.STATUS_READY, - RobotState.STATUS_LOCALIZING, - RobotState.STATUS_WALKING, - RobotState.STATUS_DETERMINING_SIDE, - ]: - return - - if not self.camera.ready(): - return - - # TODO ros - # if self.ground_truth: - # if not self.publish_point_cloud: - # self.camera.reset_position(timestamp=img.header.stamp) - # else: - # self.camera.reset_position(timestamp=img.header.stamp, from_world_frame=True, camera_frame="/camera_gt") - # else: - # self.camera.reset_position(timestamp=img.header.stamp) - # Uncomment for ground truth - # rospy.loginfo_once("Started Publishing Fieldlines") - - # image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") - h = self.camera.calculateHorizonCoverArea() + + def image_filter(self, image: Mat, debug=False) -> Mat: + # TODO could do a cover horizon based on distance to green + h = self.camera.calculate_horizon_cover_area() if h + 1 >= self.camera.resolution_y: - return + return image + image_crop = image[h + 1 :, :, :] # image_crop_blurred = cv2.GaussianBlur(image_crop, (3, 3), 0) image_crop_blurred = cv2.bilateralFilter(image, 9, 75, 75) - hsv = cv2.cvtColor(src=image_crop_blurred, code=cv2.COLOR_BGR2HSV) + # hsv = cv2.cvtColor(src=image_crop_blurred, code=cv2.COLOR_BGR2HSV) if debug: cv2.imshow("CVT Color", image_crop) @@ -86,86 +40,39 @@ def image_callback(self, image: Image, debug=False): cv2.waitKey(0) # Grass Mask - grass_only, grass_only_0, grass_only_1, grass_only_2, grass_only_3, grass_only_morph, grass_only_flipped = self.grass_mask(hsv, h) + grass_only_morph, grass_only_flipped = self.grass_mask(image_crop_blurred, h, debug) lines_only = cv2.bitwise_and(grass_only_flipped, grass_only_flipped, mask=grass_only_morph) lines_only = cv2.morphologyEx(lines_only, cv2.MORPH_CLOSE, self.circular_mask(5)) # TODO is this needed or should this be in a unit test if debug: - cv2.imshow("grass_only", grass_only) - cv2.imwrite("/tmp/grass_only.png", grass_only) - cv2.imshow("grass_only_0", grass_only_0) - cv2.imwrite("/tmp/grass_only_0.png", grass_only_0) - cv2.imshow("grass_only_1", grass_only_1) - cv2.imwrite("/tmp/grass_only_1.png", grass_only_1) - cv2.imshow("grass_only_2", grass_only_2) - cv2.imwrite("/tmp/grass_only_2.png", grass_only_2) - cv2.imshow("grass_only_3", grass_only_3) - cv2.imwrite("/tmp/grass_only_3.png", grass_only_3) - cv2.imshow("grass_only_morph", grass_only_morph) - cv2.imwrite("/tmp/grass_only_morph.png", grass_only_morph) - cv2.imshow("grass_only_flipped", grass_only_flipped) - cv2.imwrite("/tmp/grass_only_flipped.png", grass_only_flipped) cv2.imshow("lines_only", lines_only) cv2.imwrite("/tmp/lines_only.png", lines_only) cv2.waitKey(0) - # No line detection simply publish all white points - pts_x, pts_y = np.where(lines_only == 255) + return lines_only - # cv2.imshow("After", lines_only) imshow in test_object_localization instead + def img_to_points(self, lines_only: Mat) -> list: - # ROS - # if self.image_publisher.get_num_connections() > 0: - # img_out = CvBridge().cv2_to_imgmsg(lines_only) - # img_out.header = img.header - # self.image_publisher.publish(img_out) + # No line detection simply publish all white points + pts_x, pts_y = np.where(lines_only == 255) + points3d = [] # TODO own function - if self.publish_point_cloud and self.point_cloud_publisher.get_num_connections() > 0: - points3d = [] - + if self.publish_point_cloud: i = 0 for px, py in zip(pts_y, pts_x): - i = i + 1 + i += 1 if i % self.point_cloud_spacing == 0: - camToPoint = Transformation(self.camera.findFloorCoordinate([px, py])) + cam_to_point = Transformation(self.camera.find_floor_coordinate([px, py])) # Exclude points too far away - if camToPoint.norm_squared < self.point_cloud_max_distance**2: - points3d.append(camToPoint.position) - - # TODO ros - # Publish straight base link - self.tf_broadcaster.sendTransform( - self.camera.pose_base_link_straight.position, - self.camera.pose_base_link_straight.quaternion, - image.header.stamp, - self.robot_name + "/base_footprint_straight", - self.robot_name + "/odom", - ) - - # TODO ros - # Publish fieldlines in laserscan format - header = Header() - header.stamp = image.header.stamp - header.frame_id = self.robot_name + "/base_footprint_straight" - if self.ground_truth: - if not self.publish_point_cloud: - header.frame_id = self.robot_name + "/base_footprint_straight" - else: - header.frame_id = "world" - # point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d) - # self.point_cloud_publisher.publish(point_cloud_msg) - - t_end = time.time() - # rospy.loginfo_throttle(60, "Fieldline detection rate: " + str(t_end - t_start)) + if cam_to_point.norm_squared < self.point_cloud_max_distance**2: + points3d.append(cam_to_point.position) - return lines_only + return points3d if __name__ == "__main__": - rospy.init_node("detector_fieldline") fieldline_detector = DetectorFieldline() - rospy.spin() diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline_ros.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline_ros.py new file mode 100755 index 000000000..28fb561a6 --- /dev/null +++ b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline_ros.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +import os +import time + +from cv2 import Mat +from rospy.impl.tcpros_base import DEFAULT_BUFF_SIZE +from soccer_object_localization.detector_fieldline import DetectorFieldline +from tf import TransformBroadcaster + +from soccer_common.perception.camera_calculations_ros import CameraCalculationsRos + +if "ROS_NAMESPACE" not in os.environ: + os.environ["ROS_NAMESPACE"] = "/robot1" +import rospy +import sensor_msgs.point_cloud2 as pcl2 +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, PointCloud2 +from std_msgs.msg import Header + + +class DetectorFieldlineRos(DetectorFieldline): + """ + Ros bridge for detecting fieldlines. + """ + + def __init__(self): + super().__init__() + self.robot_name = rospy.get_namespace()[1:-1] + # TODO fix this up + self.camera = CameraCalculationsRos(self.robot_name) + self.camera.reset_position() + # self.initial_pose_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, + # self.initial_pose_callback, queue_size=1) + self.image_subscriber = rospy.Subscriber( + "camera/image_raw", Image, self.image_callback, queue_size=1, buff_size=DEFAULT_BUFF_SIZE * 64 + ) # Large buff size (https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/) + self.image_publisher = rospy.Publisher("camera/line_image", Image, queue_size=1) + self.point_cloud_publisher = rospy.Publisher("field_point_cloud", PointCloud2, queue_size=1) + self.tf_broadcaster = TransformBroadcaster() + + self.point_cloud_max_distance = rospy.get_param("point_cloud_max_distance", 5) + self.point_cloud_spacing = rospy.get_param("point_cloud_spacing", 30) + + # TODO look into + # def initial_pose_callback(self, initial_pose: PoseWithCovarianceStamped): + # self.publish_point_cloud = True + + def image_callback(self, img: Image, debug=False): + + t_start = time.time() + + # if self.robot_state.status not in [ + # RobotState.STATUS_READY, + # RobotState.STATUS_LOCALIZING, + # RobotState.STATUS_WALKING, + # RobotState.STATUS_DETERMINING_SIDE, + # ]: + # return + # + # TODO shouldnt need since this is only called if we get an image + # if not self.camera.ready: + # return + # TODO ros + if self.ground_truth: + if not self.publish_point_cloud: + self.camera.reset_position(timestamp=img.header.stamp) + else: + self.camera.reset_position(timestamp=img.header.stamp, from_world_frame=True, camera_frame="/camera_gt") + else: + self.camera.reset_position(timestamp=img.header.stamp) + # Uncomment for ground truth + rospy.loginfo_once("Started Publishing Fieldlines") + + image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") + lines_only = self.image_filter(image, debug) + + # ROS + if self.image_publisher.get_num_connections() > 0: + img_out = CvBridge().cv2_to_imgmsg(lines_only) + img_out.header = img.header + self.image_publisher.publish(img_out) + + self.pub_pointcloud(lines_only, img.header.stamp) + + t_end = time.time() + rospy.loginfo_throttle(60, "Fieldline detection rate: " + str(t_end - t_start)) + + def pub_pointcloud(self, lines_only: Mat, stamp: rospy.Time): + # TODO should this be a super or a new func? + points3d = self.img_to_points(lines_only) + + # TODO own function + if self.publish_point_cloud and self.point_cloud_publisher.get_num_connections() > 0: + + # Publish straight base link TODO why is this here + self.tf_broadcaster.sendTransform( + self.camera.pose_base_link_straight.position, + self.camera.pose_base_link_straight.quaternion, + stamp, + self.robot_name + "/base_footprint_straight", + self.robot_name + "/odom", + ) + + # TODO ros + # Publish fieldlines in laserscan format + header = Header() + header.stamp = stamp + header.frame_id = self.robot_name + "/base_footprint_straight" + if self.ground_truth: + if not self.publish_point_cloud: + header.frame_id = self.robot_name + "/base_footprint_straight" + else: + header.frame_id = "world" + point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d) + self.point_cloud_publisher.publish(point_cloud_msg) + + +if __name__ == "__main__": + rospy.init_node("detector_fieldline") + fieldline_detector = DetectorFieldlineRos() + rospy.spin() diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_goalpost.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_goalpost.py deleted file mode 100755 index f26249065..000000000 --- a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_goalpost.py +++ /dev/null @@ -1,167 +0,0 @@ -import math -import os -import time - -import cv2 -import numpy as np -from cv2 import Mat - -# import rospy -# from cv_bridge import CvBridge -# from sensor_msgs.msg import Image -from soccer_object_localization.detector import Detector - -# tmp -from soccer_common.utils import download_dataset, wrapToPi -from soccer_msgs.msg import RobotState - - -class DetectorGoalPost(Detector): - def __init__(self): - super().__init__() - - # self.image_subscriber = rospy.Subscriber( - # "camera/image_raw", Image, self.image_callback, queue_size=1, buff_size=DEFAULT_BUFF_SIZE * 64 - # ) # Large buff size (https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/) - # self.image_publisher = rospy.Publisher("camera/goal_image", Image, queue_size=1) - self.img_out = None - cv2.setRNGSeed(12345) - pass - - def image_callback(self, img: Mat, debug=False): - t_start = time.time() - - if self.robot_state.status not in [RobotState.STATUS_DETERMINING_SIDE, RobotState.STATUS_LOCALIZING]: - return - - if not self.camera.ready(): - return - - self.camera.reset_position() - - # image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") - vertical_lines, image_out = self.get_vlines_from_img(img, debug=debug) - if vertical_lines is None: - return - - # Get vertical line in 3D position to the camera assuming that the lower point of the line is on the grass - closest_distance = np.inf - closest_line = None - closest_line_relative_position_post_to_robot = None - min_horizon_pixel = self.camera.calculateHorizonCoverArea() - # TODO should be its own function - - for line in vertical_lines: - if line[1] > line[3]: - lower_point = [line[0], line[1]] - else: - lower_point = [line[2], line[3]] - - if lower_point[1] < min_horizon_pixel: - continue - - relative_position_post_to_robot = self.camera.findFloorCoordinate(lower_point) - if np.linalg.norm(relative_position_post_to_robot) < closest_distance: - closest_distance = np.linalg.norm(relative_position_post_to_robot) - closest_line = line - closest_line_relative_position_post_to_robot = relative_position_post_to_robot - - if closest_line is not None: - cv2.line(image_out, (closest_line[0], closest_line[1]), (closest_line[2], closest_line[3]), (255, 0, 0), 2) - # self.br.sendTransform( - # closest_line_relative_position_post_to_robot, - # (0, 0, 0, 1), - # img.header.stamp, - # self.robot_name + "/goal_post", - # self.robot_name + "/base_footprint", - # ) - - self.img_out = image_out - # if self.image_publisher.get_num_connections() > 0: - # img_out = CvBridge().cv2_to_imgmsg(image_out) - # img_out.header = img.header - # self.image_publisher.publish(img_out) - - t_end = time.time() - # rospy.loginfo_throttle(60, "GoalPost detection rate: " + str(t_end - t_start)) - - """ - Retrieves the vertical lines in image by isolating the grass in the field, removing it and then uses - Canny and Hough to detect the lines using hough_theta, hough_threshold, hough_min_line_length, - hough_max_line_gap and returns lines that are within angle_tol_deg of vertical. - Returns vertical lines as a list of lists where each line's coordinates are stored as: - [x_start, y_start, x_end, y_end] - and the original image with the vertical lines drawn on it - """ - - def get_vlines_from_img( - self, image, debug=False, angle_tol_deg=3, hough_theta=np.pi / 180, hough_threshold=30, hough_min_line_length=30, hough_max_line_gap=10 - ): - # Isolate and remove field from image - image_blurred = cv2.bilateralFilter(image, 9, 75, 75) - image_hsv = cv2.cvtColor(src=image_blurred, code=cv2.COLOR_BGR2HSV) - image_hsv_filter = cv2.inRange(image_hsv, (0, 0, 150), (255, 50, 255)) - h = self.camera.calculateHorizonCoverArea() - image_crop = image_hsv[h + 1 :, :, :] - # grass_only = cv2.inRange(image_crop, (35, 85, 0), (115, 255, 255)) - # grass_only = cv2.vconcat([np.zeros((h + 1, grass_only.shape[1]), dtype=grass_only.dtype), grass_only]) - # # Use odd numbers for all circular masks otherwise the line will shift location - # grass_only_0 = cv2.morphologyEx(grass_only, cv2.MORPH_OPEN, self.circular_mask(5)) - # grass_only_1 = cv2.morphologyEx(grass_only, cv2.MORPH_CLOSE, self.circular_mask(5)) - # grass_only_2 = cv2.morphologyEx(grass_only_1, cv2.MORPH_OPEN, self.circular_mask(21)) - # grass_only_3 = cv2.morphologyEx(grass_only_2, cv2.MORPH_CLOSE, self.circular_mask(61)) - # grass_only_morph = cv2.morphologyEx(grass_only_3, cv2.MORPH_ERODE, self.circular_mask(9)) - # grass_only_flipped = cv2.bitwise_not(grass_only_morph) - - grass_only, grass_only_0, grass_only_1, grass_only_2, grass_only_3, grass_only_morph, grass_only_flipped = self.grass_mask(image_crop, h) - - image_bw = cv2.bitwise_and(image, image, mask=image_hsv_filter) - image_bw = cv2.bitwise_and(image_bw, image_bw, mask=grass_only_flipped) - image_bw = cv2.cvtColor(image_bw, cv2.COLOR_BGR2GRAY) - image_bw_eroded = cv2.morphologyEx(image_bw, cv2.MORPH_ERODE, self.circular_mask(5)) - - # Isolate all lines using Canny edge detection and Hough lines with the provided settings - image_edges = cv2.Canny(image_bw_eroded, 50, 150, apertureSize=3) - lines = cv2.HoughLinesP( - image_edges, rho=1, theta=hough_theta, threshold=hough_threshold, minLineLength=hough_min_line_length, maxLineGap=hough_max_line_gap - ) - if lines is None: - return None, None - if debug: - image_hough = image.copy() - for line in lines: - x1, y1, x2, y2 = line[0] - cv2.line(image_hough, (x1, y1), (x2, y2), (0, 255, 0), 2) - - # Isolate vertical lines - angle_tol_rad = np.radians(angle_tol_deg) # +-deg from vertical - image_out = image.copy() - - vertical_lines = [] - for line in lines: - x1, y1, x2, y2 = line[0] - abs_line_angle = math.atan2(y2 - y1, x2 - x1) - if abs(abs_line_angle - np.pi / 2) < angle_tol_rad: - vertical_lines.append(line[0]) - cv2.line(image_out, (x1, y1), (x2, y2), (0, 255, 0), 2) - - if debug and "DISPLAY" in os.environ: - cv2.imshow("image_blurred", image_blurred) - cv2.imshow("image_hsv", image_hsv) - cv2.imshow("image_hsv_filter", image_hsv_filter) - cv2.imshow("grass_only", grass_only_flipped) - - cv2.imshow("image_bw", image_bw) - cv2.imshow("image_bw_eroded", image_bw_eroded) - cv2.imshow("image_edges", image_edges) - cv2.imshow("image_hough", image_hough) - - cv2.waitKey(0) - return vertical_lines, image_out - - -if __name__ == "__main__": - # rospy.init_node("detector_goalpost") - # fieldline_detector = DetectorGoalPost() - # rospy.spin() - pass diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_objects.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_objects.py index 4b0f1004e..c060cadf3 100755 --- a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_objects.py +++ b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_objects.py @@ -1,151 +1,165 @@ #!/usr/bin/env python3 - -import os -import time - -import numpy as np - -np.set_printoptions(precision=3) - -from soccer_msgs.msg import RobotState - -if "ROS_NAMESPACE" not in os.environ: - os.environ["ROS_NAMESPACE"] = "/robot1" - -import rospy -from geometry_msgs.msg import Pose2D -from sensor_msgs.msg import JointState -from soccer_object_localization.detector import Detector - -from soccer_common.transformation import Transformation -from soccer_msgs.msg import BoundingBox, BoundingBoxes - - -class DetectorObjects(Detector): - def __init__(self): - super().__init__() - self.joint_states_sub = rospy.Subscriber("joint_states", JointState, self.jointStatesCallback, queue_size=1) - self.bounding_boxes_sub = rospy.Subscriber("object_bounding_boxes", BoundingBoxes, self.objectDetectorCallback, queue_size=1) - self.ball_pixel_publisher = rospy.Publisher("ball_pixel", Pose2D, queue_size=1) - self.head_motor_1_angle = 0 - self.last_ball_pose = None - self.last_ball_pose_counter = 0 - - def jointStatesCallback(self, msg: JointState): - if len(msg.name) != 0: - index = msg.name.index("head_motor_1") - self.head_motor_1_angle = msg.position[index] - - def objectDetectorCallback(self, msg: BoundingBoxes): - if self.robot_state.status not in [RobotState.STATUS_LOCALIZING, RobotState.STATUS_READY]: - return - - if not self.camera.ready: - return - - s = time.time() - self.camera.reset_position(timestamp=msg.header.stamp, from_world_frame=True, skip_if_not_found=True) - e = time.time() - if e - s > 1: - rospy.logerr_throttle(1, f"Resetting camera position took longer than usual ({e-s}) seconds") - - # Ball - max_detection_size = 0 - final_camera_to_ball: Transformation = None - final_ball_pixel = None - candidate_ball_counter = 1 - obstacle_counter = 0 - for box in msg.bounding_boxes: - if box.Class == "0": - # Exclude weirdly shaped balls - ratio = (box.ymax - box.ymin) / (box.xmax - box.xmin) - if ratio > 2 or ratio < 0.5: - rospy.logwarn_throttle(1, f"Excluding weirdly shaped ball {box.ymax - box.ymin} x {box.xmax - box.xmin}") - continue - - boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] - ball_pose = self.camera.calculateBallFromBoundingBoxes(0.07, boundingBoxes) - - # Ignore balls outside of the field - camera_to_ball = np.linalg.inv(self.camera.pose) @ ball_pose - detection_size = (box.ymax - box.ymin) * (box.xmax - box.xmin) - - rospy.loginfo_throttle( - 5, - f"Candidate Ball Position { candidate_ball_counter }: { ball_pose.position[0] } { ball_pose.position[1] }, detection_size { detection_size }", - ) - candidate_ball_counter = candidate_ball_counter + 1 - - # Exclude balls outside the field + extra space in the net - if abs(ball_pose.position[0]) > 5.2 or abs(ball_pose.position[1]) > 3.5: - continue - - # If it is the first ball, we need high confidence - if self.last_ball_pose is None: - if box.probability < rospy.get_param("~first_ball_detection_confidence_threshold", 0.78): - rospy.logwarn_throttle(0.5, f"Ignoring first pose of ball with low confidence threshold {box.probability}") - continue - - # Exclude balls that are too far from the previous location - if self.last_ball_pose is not None: - if np.linalg.norm(ball_pose.position[0:2]) < 0.1: # In the start position - pass - elif np.linalg.norm(ball_pose.position[0:2] - self.last_ball_pose.position[0:2]) > 3: # meters from previous position - rospy.logwarn_throttle( - 0.5, - f"Detected a ball too far away ({ self.last_ball_pose_counter }), Last Location {self.last_ball_pose.position[0:2]} Detected Location {ball_pose.position[0:2] }", - ) - self.last_ball_pose_counter = self.last_ball_pose_counter + 1 - if self.last_ball_pose_counter > 5: # Counter to prevent being stuck when the ball is in a different location - self.last_ball_pose_counter = 0 - self.last_ball_pose = None - continue - - # Get the largest detection - if detection_size > max_detection_size: - final_camera_to_ball = camera_to_ball - final_ball_pixel = Pose2D() - final_ball_pixel.x = (box.xmax - box.xmin) * 0.5 + box.xmin - final_ball_pixel.y = (box.ymax - box.ymin) * 0.5 + box.ymin - self.last_ball_pose = ball_pose - self.last_ball_pose_counter = 0 - max_detection_size = detection_size - pass - elif box.Class == "2": - if box.probability > 0.78: - if box.obstacle_detected: - pos = [box.xbase, box.ybase] - - floor_coordinate_robot = self.camera.findFloorCoordinate(pos) - world_to_obstacle = Transformation(position=floor_coordinate_robot) - camera_to_obstacle = np.linalg.inv(self.camera.pose) @ world_to_obstacle - - # self.br.sendTransform( - # camera_to_obstacle.position, - # camera_to_obstacle.quaternion, - # msg.header.stamp, - # self.robot_name + f"/obstacle_{obstacle_counter}", - # self.robot_name + "/camera", - # ) - # rospy.loginfo(f"Obstacle {obstacle_counter} detected at [{pos}] {floor_coordinate_robot} {camera_to_obstacle.position}") - obstacle_counter += 1 - - if final_camera_to_ball is not None: - self.ball_pixel_publisher.publish(final_ball_pixel) - # self.br.sendTransform( - # final_camera_to_ball.position, - # final_camera_to_ball.quaternion, - # msg.header.stamp, - # self.robot_name + "/ball", - # self.robot_name + "/camera", - # ) - # rospy.loginfo_throttle( - # 1, - # f"\u001b[1m\u001b[34mBall detected [{self.last_ball_pose.position[0]:.3f}, {self.last_ball_pose.position[1]:.3f}] \u001b[0m", - # ) - - -# if __name__ == "__main__": -# rospy.init_node("ball_detector") -# ball_detector = DetectorObjects() -# rospy.spin() +# TODO rework +# +# import os +# import time +# +# import numpy as np +# +# np.set_printoptions(precision=3) +# +# from soccer_msgs.msg import RobotState +# +# import rospy +# from geometry_msgs.msg import Pose2D +# from sensor_msgs.msg import JointState +# from soccer_object_localization.detector import Detector +# +# from soccer_common.transformation import Transformation +# from soccer_msgs.msg import BoundingBox, BoundingBoxes +# +# +# # if "ROS_NAMESPACE" not in os.environ: +# # os.environ["ROS_NAMESPACE"] = "/robot1" +# +# # TODO is this really necessary? +# +# class DetectorObjects(Detector): +# def __init__(self): +# super().__init__() +# # self.bounding_boxes_sub = rospy.Subscriber("object_bounding_boxes", BoundingBoxes, self.objectDetectorCallback, +# # queue_size=1) +# # self.ball_pixel_publisher = rospy.Publisher("ball_pixel", Pose2D, queue_size=1) +# +# self.last_ball_pose = None +# self.last_ball_pose_counter = 0 +# self.candidate_ball_counter = 0 +# self.max_detection_size = 0 +# self.final_camera_to_ball: Transformation = None +# +# def detect_ball(self, box: BoundingBox): +# # TODO needs to be reworked +# # Exclude weirdly shaped balls +# ratio = (box.ymax - box.ymin) / (box.xmax - box.xmin) +# if ratio > 2 or ratio < 0.5: +# # rospy.logwarn_throttle(1, +# # f"Excluding weirdly shaped ball {box.ymax - box.ymin} x {box.xmax - box.xmin}") +# return True +# +# boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] +# ball_pose = self.camera.calculate_ball_from_bounding_boxes(0.07, boundingBoxes) +# +# # Ignore balls outside of the field +# camera_to_ball = np.linalg.inv(self.camera.pose) @ ball_pose +# detection_size = (box.ymax - box.ymin) * (box.xmax - box.xmin) +# +# # rospy.loginfo_throttle( +# # 5, +# # f"Candidate Ball Position {candidate_ball_counter}: {ball_pose.position[0]} {ball_pose.position[1]}, detection_size {detection_size}", +# # ) +# self.candidate_ball_counter = self.candidate_ball_counter + 1 +# +# # Exclude balls outside the field + extra space in the net +# if abs(ball_pose.position[0]) > 5.2 or abs(ball_pose.position[1]) > 3.5: +# return True +# +# # If it is the first ball, we need high confidence +# if self.last_ball_pose is None: +# if box.probability < rospy.get_param("~first_ball_detection_confidence_threshold", 0.78): +# # rospy.logwarn_throttle(0.5, +# # f"Ignoring first pose of ball with low confidence threshold {box.probability}") +# return True +# +# # Exclude balls that are too far from the previous location +# if self.last_ball_pose is not None: +# if np.linalg.norm(ball_pose.position[0:2]) < 0.1: # In the start position +# pass +# elif np.linalg.norm(ball_pose.position[0:2] - self.last_ball_pose.position[ +# 0:2]) > 3: # meters from previous position +# # rospy.logwarn_throttle( +# # 0.5, +# # f"Detected a ball too far away ({self.last_ball_pose_counter}), Last Location {self.last_ball_pose.position[0:2]} Detected Location {ball_pose.position[0:2]}", +# # ) +# self.last_ball_pose_counter = self.last_ball_pose_counter + 1 +# if self.last_ball_pose_counter > 5: # Counter to prevent being stuck when the ball is in a different location +# self.last_ball_pose_counter = 0 +# self.last_ball_pose = None +# return True +# +# # Get the largest detection +# if detection_size > self.max_detection_size: +# self.final_camera_to_ball = camera_to_ball +# final_ball_pixel = Pose2D() +# final_ball_pixel.x = (box.xmax - box.xmin) * 0.5 + box.xmin +# final_ball_pixel.y = (box.ymax - box.ymin) * 0.5 + box.ymin +# self.last_ball_pose = ball_pose +# self.last_ball_pose_counter = 0 +# self.max_detection_size = detection_size +# +# return False +# +# def objectDetectorCallback(self, msg: BoundingBoxes): +# # if self.robot_state.status not in [RobotState.STATUS_LOCALIZING, RobotState.STATUS_READY]: +# # return +# +# # if not self.camera.ready(): +# # return +# +# # s = time.time() +# self.camera.reset_position(timestamp=msg.header.stamp, from_world_frame=True, skip_if_not_found=True) +# # e = time.time() +# # if e - s > 1: +# # rospy.logerr_throttle(1, f"Resetting camera position took longer than usual ({e - s}) seconds") +# +# # Ball +# self.max_detection_size = 0 +# self.final_camera_to_ball: Transformation = None +# final_ball_pixel = None +# self.candidate_ball_counter = 1 +# obstacle_counter = 0 +# camera_to_obstacle = None +# # TODO needs to be reworked also some of this should be in strategy and detector node +# for box in msg.bounding_boxes: +# if box.Class == "0": +# if self.detect_ball(box): +# continue +# elif box.Class == "2": +# if box.probability > 0.78: +# if box.obstacle_detected: +# pos = [box.xbase, box.ybase] +# +# floor_coordinate_robot = self.camera.find_floor_coordinate(pos) +# world_to_obstacle = Transformation(position=floor_coordinate_robot) +# camera_to_obstacle = np.linalg.inv(self.camera.pose) @ world_to_obstacle +# +# # TODO do we really need will add back in later +# # self.br.sendTransform( +# # camera_to_obstacle.position, +# # camera_to_obstacle.quaternion, +# # msg.header.stamp, +# # self.robot_name + f"/obstacle_{obstacle_counter}", +# # self.robot_name + "/camera", +# # ) +# # rospy.loginfo(f"Obstacle {obstacle_counter} detected at [{pos}] {floor_coordinate_robot} {camera_to_obstacle.position}") +# obstacle_counter += 1 +# +# if self.final_camera_to_ball is not None: +# pass +# # self.ball_pixel_publisher.publish(final_ball_pixel) +# # self.br.sendTransform( +# # self.final_camera_to_ball.position, +# # self.final_camera_to_ball.quaternion, +# # msg.header.stamp, +# # self.robot_name + "/ball", +# # self.robot_name + "/camera", +# # ) +# # rospy.loginfo_throttle( +# # 1, +# # f"\u001b[1m\u001b[34mBall detected [{self.last_ball_pose.position[0]:.3f}, {self.last_ball_pose.position[1]:.3f}] \u001b[0m", +# # ) +# return final_ball_pixel, camera_to_obstacle, obstacle_counter +# +# # if __name__ == "__main__": +# # rospy.init_node("ball_detector") +# # ball_detector = DetectorObjects() +# # rospy.spin() diff --git a/soccer_perception/soccer_object_localization/test/test_camera.py b/soccer_perception/soccer_object_localization/test/test_camera.py deleted file mode 100644 index 43630210a..000000000 --- a/soccer_perception/soccer_object_localization/test/test_camera.py +++ /dev/null @@ -1,51 +0,0 @@ -import math -import os -from unittest import TestCase - -from sensor_msgs.msg import CameraInfo - -from soccer_common.camera import Camera -from soccer_common.transformation import Transformation - - -# TODO fix unit test -class TestCamera(TestCase): - def test_camera_find_floor_coordinate(self): - p = Transformation([0, 0, 0.5], euler=[0, math.pi / 4, 0]) - c = Camera() - c.pose = p - ci = CameraInfo() - ci.height = 240 - ci.width = 360 - c.camera_info = ci - - p2 = c.findFloorCoordinate([360 / 2, 240 / 2]) - self.assertAlmostEqual(p2[0], 0.5, delta=0.005) - self.assertAlmostEqual(p2[1], 0, delta=0.005) - self.assertAlmostEqual(p2[2], 0, delta=0.005) - - def test_camera_find_camera_coordinate(self): - p = Transformation([0, 0, 0.5], euler=[0, math.pi / 4, 0]) - c = Camera() - c.pose = p - ci = CameraInfo() - ci.height = 240 - ci.width = 360 - c.camera_info = ci - - p2 = c.findCameraCoordinate([0.5, 0, 0]) - self.assertAlmostEqual(p2[0], 360 / 2, delta=0.5) - self.assertAlmostEqual(p2[1], 240 / 2, delta=0.5) - - def test_camera_find_camera_coordinate_2(self): - p = Transformation([0, 0, 0.5], euler=[0, 0, 0]) - c = Camera() - c.pose = p - ci = CameraInfo() - ci.height = 240 - ci.width = 360 - c.camera_info = ci - - p3 = c.findCameraCoordinate([0.5, 0, 0.5]) - self.assertAlmostEqual(p3[0], 360 / 2, delta=0.5) - self.assertAlmostEqual(p3[1], 240 / 2, delta=0.5) diff --git a/soccer_perception/soccer_object_localization/test/test_object_localization.py b/soccer_perception/soccer_object_localization/test/test_object_localization.py index 0a8046db4..4d529e10c 100644 --- a/soccer_perception/soccer_object_localization/test/test_object_localization.py +++ b/soccer_perception/soccer_object_localization/test/test_object_localization.py @@ -1,81 +1,24 @@ import os - -from soccer_object_detection.object_detect_node import Label, ObjectDetectionNode - -# ROS -# from soccer_object_detection.test_object_detection import IoU -from soccer_object_localization.detector_objects import DetectorObjects - -os.environ["ROS_NAMESPACE"] = "/robot1" - -import math +from os.path import expanduser from unittest import TestCase -from unittest.mock import MagicMock import cv2 -import numpy as np -import pytest - -# import rosbag -# import rospy -# import tf2_ros from cv2 import Mat -from cv_bridge import CvBridge -from sensor_msgs.msg import CameraInfo, Image from soccer_object_localization.detector_fieldline import DetectorFieldline -from soccer_object_localization.detector_goalpost import DetectorGoalPost -from soccer_common.camera import Camera -from soccer_common.transformation import Transformation -from soccer_common.utils import download_dataset, wrapToPi -from soccer_msgs.msg import GameState, RobotState +from soccer_common.utils import download_dataset # TODO fix unit test class TestObjectLocalization(TestCase): - def test_calculate_bounding_boxes_from_ball(self): - for cam_angle in [0, 0.1, -0.1]: - for cam_position in [[0, 0, 0], [0, 0, 0.1], [0, 0, -0.1]]: - p = Transformation(cam_position, euler=[cam_angle, 0, 0]) - c = Camera() - c.pose = p - ci = CameraInfo() - ci.height = 240 - ci.width = 360 - c.camera_info = ci - - positions = [[0.5, 0, 0.1], [0.5, 0, 0], [0.5, 0, 0.1]] - for position in positions: - ball_pose = Transformation(position) - ball_radius = 0.07 - - bounding_boxes = c.calculateBoundingBoxesFromBall(ball_pose, ball_radius) - # [[135.87634651355825, 75.87634651355823], [224.12365348644175, 164.12365348644175]] - position = c.calculateBallFromBoundingBoxes(ball_radius, bounding_boxes) - - self.assertAlmostEqual(position.position[0], ball_pose.position[0], delta=0.001) - self.assertAlmostEqual(position.position[1], ball_pose.position[1], delta=0.001) - self.assertAlmostEqual(position.position[2], ball_pose.position[2], delta=0.001) def test_fieldline_detection(self): - # rospy.init_node("test") - - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/fieldlines" - download_dataset(url="https://drive.google.com/uc?id=1nJX6ySks_a7mESvCm3sNllmJTNpm-x2_", folder_path=test_path) + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/images/fieldlines" - Camera.reset_position = MagicMock() - Camera.ready = MagicMock() + download_dataset("https://drive.google.com/uc?id=1nJX6ySks_a7mESvCm3sNllmJTNpm-x2_", folder_path=test_path) d = DetectorFieldline() - d.robot_state.status = RobotState.STATUS_READY - - # ROS - # d.image_publisher.get_num_connections = MagicMock(return_value=1) - # d.publish_point_cloud = True - # d.point_cloud_publisher.get_num_connections = MagicMock(return_value=1) - - cvbridge = CvBridge() for file_name in os.listdir(test_path): # file_name = "img160_-1.452993567956688_-3.15_0.7763055830612666.png" @@ -83,435 +26,139 @@ def test_fieldline_detection(self): print(file_name) img: Mat = cv2.imread(os.path.join(test_path, file_name)) - c = CameraInfo() - c.height = img.shape[0] - c.width = img.shape[1] - d.camera.camera_info = c - - # ROS - # img_msg: Image = cvbridge.cv2_to_imgmsg(img, encoding="rgb8") - # d.image_publisher.publish = MagicMock() - lines_only = d.image_callback(img, debug=False) + lines_only = d.image_filter(img, debug=False) + points3d = d.img_to_points(lines_only) + print(points3d) # TODO need way to vis and verify if "DISPLAY" in os.environ: cv2.imshow("Before", img) - cv2.imwrite("/tmp/before.png", img) + # cv2.imwrite("/tmp/before.png", img) # TODO why cv2.imshow("After", lines_only) - # ROS - # if d.image_publisher.publish.call_count != 0: - # img_out = cvbridge.imgmsg_to_cv2(d.image_publisher.publish.call_args[0][0]) - # cv2.imshow("After", img_out) - # cv2.imwrite("/tmp/after.png", img_out) - cv2.waitKey(0) cv2.destroyAllWindows() def test_fieldline_detection_cam(self): - # rospy.init_node("test") - - Camera.reset_position = MagicMock() - Camera.ready = MagicMock() d = DetectorFieldline() - d.robot_state.status = RobotState.STATUS_READY - d.image_publisher.get_num_connections = MagicMock(return_value=1) - # d.publish_point_cloud = True - # d.point_cloud_publisher.get_num_connections = MagicMock(return_value=1) cap = cv2.VideoCapture(4) if not cap.isOpened(): print("Cannot open camera") exit() - cvbridge = CvBridge() + while True: ret, frame = cap.read() if not ret: print("Can't receive frame (stream end?). Exiting ...") break - img = cv2.resize(frame, dsize=(640, 480)) - c = CameraInfo() - c.height = img.shape[0] - c.width = img.shape[1] - d.camera.camera_info = c + img = cv2.resize(frame, dsize=(640, 480)) - img_msg: Image = cvbridge.cv2_to_imgmsg(img, encoding="rgb8") - d.image_publisher.publish = MagicMock() - d.image_callback(img_msg, debug=False) + lines_only = d.image_filter(img, debug=False) if "DISPLAY" in os.environ: cv2.imshow("Before", img) - # cv2.imwrite("/tmp/before.png", img) + # cv2.imwrite("/tmp/before.png", img) # TODO why - if d.image_publisher.publish.call_count != 0: - img_out = cvbridge.imgmsg_to_cv2(d.image_publisher.publish.call_args[0][0]) - cv2.imshow("After", img_out) - # cv2.imwrite("/tmp/after.png", img_out) + cv2.imshow("After", lines_only) - cv2.waitKey(1) + cv2.waitKey(0) cv2.destroyAllWindows() def test_fieldline_detection_vid(self): - # rospy.init_node("test") - os.environ["LD_LIBRARY_PATH"] = "$LD_LIBRARY_PATH:/opt/ros/noetic/lib/" - - src_path = os.path.dirname(os.path.realpath(__file__)) - - Camera.reset_position = MagicMock() - Camera.ready = MagicMock() d = DetectorFieldline() - d.robot_state.status = RobotState.STATUS_READY - d.image_publisher.get_num_connections = MagicMock(return_value=1) - # d.publish_point_cloud = True - # d.point_cloud_publisher.get_num_connections = MagicMock(return_value=1) + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/videos/robocup2023" + + download_dataset("https://drive.google.com/uc?id=1UTQ6Rz0yk8jpWwWoq3eSf7DOmG_j9An3", folder_path=test_path) - cap = cv2.VideoCapture(src_path + "/../../../soccer_object_detection/videos/2023-07-08-124521.webm") + cap = cv2.VideoCapture(test_path + "/2023-07-08-124521.webm") if not cap.isOpened(): print("Cannot open camera") exit() - cvbridge = CvBridge() + while True: ret, frame = cap.read() if not ret: print("Can't receive frame (stream end?). Exiting ...") break - img = cv2.resize(frame, dsize=(640, 480)) - - c = CameraInfo() - c.height = img.shape[0] - c.width = img.shape[1] - d.camera.camera_info = c - img_msg: Image = cvbridge.cv2_to_imgmsg(img, encoding="rgb8") - d.image_publisher.publish = MagicMock() - d.image_callback(img_msg, debug=False) + img = cv2.resize(frame, dsize=(640, 480)) + lines_only = d.image_filter(img, debug=False) # 0.05 + points3d = d.img_to_points(lines_only) + print(points3d) # TODO need way to vis and verify if "DISPLAY" in os.environ: cv2.imshow("Before", img) - # cv2.imwrite("/tmp/before.png", img) + # cv2.imwrite("/tmp/before.png", img) # TODO why - if d.image_publisher.publish.call_count != 0: - img_out = cvbridge.imgmsg_to_cv2(d.image_publisher.publish.call_args[0][0]) - cv2.imshow("After", img_out) - # cv2.imwrite("/tmp/after.png", img_out) + cv2.imshow("After", lines_only) cv2.waitKey(1) cv2.destroyAllWindows() - def test_goalpost_detection(self): - """ - Returns whether a point at a given field coordinate is visible to the robot - """ - - def get_point_visibility(robot_pose, point_coords): - robot_x, robot_y, robot_yaw = robot_pose - point_x, point_y = point_coords - - point_yaw = math.atan2(point_y - robot_y, point_x - robot_x) - camera_fov = 1.39626 # rads - - # Both yaw angles are between -pi and pi - delta_yaw = wrapToPi(point_yaw - robot_yaw) - - # Check if the point is within the view cone - # No equals case as the point wouldn't be fully visible - is_point_visible = -camera_fov / 2.0 < delta_yaw < camera_fov / 2.0 - - return is_point_visible - - """ - Returns a dictionary that stores booleans indicating whether each post is visible - Visual reference: https://www.desmos.com/calculator/b9lndsb1bl - Example: both posts of the left net are visible - visible_posts = { - "NEG_X_NET": { - "POS_Y_POST": True, - "NEG_Y_POST": True - }, - "POS_X_NET": { - "POS_Y_POST": False, - "NEG_Y_POST": False - } - } - """ - - def get_visible_posts(robot_x, robot_y, robot_yaw): - visible_posts = {"NEG_X_NET": {"POS_Y_POST": True, "NEG_Y_POST": True}, "POS_X_NET": {"POS_Y_POST": False, "NEG_Y_POST": False}} - - net_coords = { - "NEG_X_NET": {"POS_Y_POST": [-4.5, 1.3], "NEG_Y_POST": [-4.5, -1.3]}, - "POS_X_NET": {"POS_Y_POST": [4.5, 1.3], "NEG_Y_POST": [4.5, -1.3]}, - } - - for net in net_coords.keys(): - post_coords = net_coords[net] - for post in post_coords.keys(): - visible_posts[net][post] = get_point_visibility((robot_x, robot_y, robot_yaw), net_coords[net][post]) - - return visible_posts - - # Setup test environment: - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/goal_net" - download_dataset(url="https://drive.google.com/uc?id=17qdnW7egoopXHvakiNnUUufP2MOjyZ18", folder_path=test_path) - - # Camera.reset_position = MagicMock() - # Camera.ready = MagicMock() - d = DetectorGoalPost() - d.robot_state.status = RobotState.STATUS_DETERMINING_SIDE - d.camera.pose = Transformation(position=[0, 0, 0.46]) - # d.image_publisher.get_num_connections = MagicMock(return_value=1) - - # cvbridge = CvBridge() - - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/goal_net" - - # Loop through test images - for file_name in os.listdir(test_path): - # file_name = "img173_-0.852141317992289_3.15_-1.7125376246657054.png" - - print(f"Loading {file_name} from goal_net dataset") - file_name_no_ext = os.path.splitext(file_name)[0] - x, y, yaw = file_name_no_ext.split("_")[1:] - yaw = wrapToPi(float(yaw)) - if yaw < 0: - yaw = (yaw + np.pi) % (np.pi) - - d.camera.pose.orientation_euler = [yaw, 0, 0] - print(f"Parsed (x, y, yaw): ({x}, {y}, {yaw}) from filename.") - visible_posts = get_visible_posts(float(x), float(y), float(yaw)) - for net in visible_posts.keys(): - for post in visible_posts[net].keys(): - if visible_posts[net][post]: - print(f"{net}, {post} is visible") - - img: Mat = cv2.imread(os.path.join(test_path, file_name)) - - if "DISPLAY" in os.environ: - cv2.imshow("Before", img) - - c = CameraInfo() - c.height = img.shape[0] - c.width = img.shape[1] - d.camera.camera_info = c - - # img_msg: Image = cvbridge.cv2_to_imgmsg(img, encoding="rgb8") - # d.image_publisher.publish = MagicMock() - d.image_callback(img, debug=False) - - if "DISPLAY" in os.environ: - # if d.image_publisher.publish.call_count != 0: - # # img_out = cvbridge.imgmsg_to_cv2(d.image_publisher.publish.call_args[0][0]) - cv2.imshow("After", d.img_out) - cv2.waitKey(0) - - # @pytest.mark.skip - def test_goalpost_detection_tune(self): - """ - Used for tuning vertical line detection parameters using sliders. - """ - - # rospy.init_node("test") - - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/goal_net" - download_dataset(url="https://drive.google.com/uc?id=17qdnW7egoopXHvakiNnUUufP2MOjyZ18", folder_path=test_path) - - d = DetectorGoalPost() - - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../images/goal_net" - file_name = "img341_0.960470105738711_-3.15_1.3585673890175494.png" - - print(f"Loading {file_name} from goal_net dataset") - file_name_no_ext = os.path.splitext(file_name)[0] - x, y, yaw = file_name_no_ext.split("_")[1:] - print(f"Parsed (x, y, yaw): ({x}, {y}, {yaw}) from filename.") - - img: Mat = cv2.imread(os.path.join(test_path, file_name)) - - c = CameraInfo() - c.height = img.shape[0] - c.width = img.shape[1] - d.camera.camera_info = c - - # Create a window - cv2.namedWindow("image") - - def nothing(x): - pass - - # create trackbars for hough line parameters - default_vline_angle_tol_deg = 3 - default_theta = np.pi / 180 - default_threshold = 30 - default_min_line_length = 30 - default_max_line_gap = 10 - cv2.createTrackbar("vLineAngleTolDeg", "image", 0, 15, nothing) - cv2.createTrackbar("threshold", "image", 0, 255, nothing) - cv2.createTrackbar("minLineLength", "image", 0, 250, nothing) - cv2.createTrackbar("maxLineGap", "image", 0, 20, nothing) - - # Set default value for MAX HSV trackbars. - cv2.setTrackbarPos("vLineAngleTolDeg", "image", default_vline_angle_tol_deg) - cv2.setTrackbarPos("threshold", "image", default_threshold) - cv2.setTrackbarPos("minLineLength", "image", default_min_line_length) - cv2.setTrackbarPos("maxLineGap", "image", default_max_line_gap) - - while True: - # get current positions of all trackbars - vline_angle_tol_deg = cv2.getTrackbarPos("vLineAngleTolDeg", "image") - threshold = cv2.getTrackbarPos("threshold", "image") - min_line_length = cv2.getTrackbarPos("minLineLength", "image") - max_line_gap = cv2.getTrackbarPos("maxLineGap", "image") - - vertical_lines, img_out = d.get_vlines_from_img( - img, - debug=False, - angle_tol_deg=vline_angle_tol_deg, - hough_theta=default_theta, - hough_threshold=threshold, - hough_min_line_length=min_line_length, - hough_max_line_gap=max_line_gap, - ) - cv2.imshow("image", img_out) - - if cv2.waitKey(33) & 0xFF == ord("n"): - file_name = "newfile" - break - - @pytest.mark.skip - def test_hsv_filter(self): - import cv2 - import numpy as np - - def nothing(x): - pass - - # Create a window - cv2.namedWindow("image") - - # create trackbars for color change - cv2.createTrackbar("HMin", "image", 0, 179, nothing) # Hue is from 0-179 for Opencv - cv2.createTrackbar("SMin", "image", 0, 255, nothing) - cv2.createTrackbar("VMin", "image", 0, 255, nothing) - cv2.createTrackbar("HMax", "image", 0, 179, nothing) - cv2.createTrackbar("SMax", "image", 0, 255, nothing) - cv2.createTrackbar("VMax", "image", 0, 255, nothing) - - # Set default value for MAX HSV trackbars. - cv2.setTrackbarPos("HMax", "image", 179) - cv2.setTrackbarPos("SMax", "image", 255) - cv2.setTrackbarPos("VMax", "image", 255) - - # Initialize to check if HSV min/max value changes - hMin = sMin = vMin = hMax = sMax = vMax = 0 - phMin = psMin = pvMin = phMax = psMax = pvMax = 0 - - img = cv2.imread("../../images/goal_net/img160_-1.452993567956688_-3.15_0.7763055830612666.png") - output = img - waitTime = 33 - - while 1: - - # get current positions of all trackbars - hMin = cv2.getTrackbarPos("HMin", "image") - sMin = cv2.getTrackbarPos("SMin", "image") - vMin = cv2.getTrackbarPos("VMin", "image") - - hMax = cv2.getTrackbarPos("HMax", "image") - sMax = cv2.getTrackbarPos("SMax", "image") - vMax = cv2.getTrackbarPos("VMax", "image") - - # Set minimum and max HSV values to display - lower = np.array([hMin, sMin, vMin]) - upper = np.array([hMax, sMax, vMax]) - - # Create HSV Image and threshold into a range. - hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - mask = cv2.inRange(hsv, lower, upper) - output = cv2.bitwise_and(img, img, mask=mask) - - # Print if there is a change in HSV value - if (phMin != hMin) | (psMin != sMin) | (pvMin != vMin) | (phMax != hMax) | (psMax != sMax) | (pvMax != vMax): - print("(hMin = %d , sMin = %d, vMin = %d), (hMax = %d , sMax = %d, vMax = %d)" % (hMin, sMin, vMin, hMax, sMax, vMax)) - phMin = hMin - psMin = sMin - pvMin = vMin - phMax = hMax - psMax = sMax - pvMax = vMax - - # Display output image - cv2.imshow("image", output) - - # Wait longer to prevent freeze for videos. - if cv2.waitKey(waitTime) & 0xFF == ord("q"): - break - - cv2.destroyAllWindows() - - def test_robot_detection(self): - src_path = os.path.dirname(os.path.realpath(__file__)) - test_path = src_path + "/../../../soccer_object_detection/images/simulation" - download_dataset("https://drive.google.com/uc?id=11nN58j8_PBoLNRAzOEdk7fMe1UK1diCc", folder_path=test_path) - - # ROS - # rospy.init_node("test") - - Camera.reset_position = MagicMock() - - src_path = os.path.dirname(os.path.realpath(__file__)) - model_path = src_path + "/../../../soccer_object_detection/models/half_5.pt" - - n = ObjectDetectionNode(model_path=model_path) - n.robot_state.status = RobotState.STATUS_READY - n.game_state.gameState = GameState.GAMESTATE_PLAYING - - Camera.reset_position = MagicMock() - Camera.ready = MagicMock() - do = DetectorObjects() - do.robot_state.status = RobotState.STATUS_READY - - cvbridge = CvBridge() - for file_name in os.listdir(f"{test_path}/images"): - print(file_name) - img: Mat = cv2.imread(os.path.join(f"{test_path}/images", file_name)) # ground truth box = (68, 89) (257, 275) - img_original_size = img.size - img = cv2.resize(img, dsize=(640, 480)) - img_msg: Image = cvbridge.cv2_to_imgmsg(img) - - # Mock the detections - n.pub_detection = MagicMock() - n.pub_boundingbox = MagicMock() - n.pub_detection.get_num_connections = MagicMock(return_value=1) - n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) - n.pub_detection.publish = MagicMock() - n.pub_boundingbox.publish = MagicMock() - - ci = CameraInfo() - ci.height = img.shape[0] - ci.width = img.shape[1] - n.camera.camera_info = ci - do.camera.camera_info = ci - n.camera.pose = Transformation(position=[0, 0, 0.46], orientation_euler=[0, np.pi / 8, 0]) - do.camera.pose = n.camera.pose - - # n.callback(img_msg) - detection_image = n.callback(img) # send the image directly - - # Check assertion - # if n.pub_boundingbox.publish.call_args is not None: - # bounding_boxes = n.pub_boundingbox.publish.call_args[0][0] - # do.objectDetectorCallback(bounding_boxes) - - if "DISPLAY" in os.environ: - # mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) - # cv2.imshow("Image", mat) - # cv2.imshow("Image", img) - cv2.imshow("Detections", detection_image) - cv2.waitKey() - - cv2.destroyAllWindows() + # TODO this needs to be reworked + # def test_robot_detection(self): + # src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + # test_path = src_path + "data/images/simulation" + # + # download_dataset("https://drive.google.com/uc?id=11nN58j8_PBoLNRAzOEdk7fMe1UK1diCc", folder_path=test_path) + # + # # ROS + # # rospy.init_node("test") + # + # Camera.reset_position = MagicMock() + # + # src_path = os.path.dirname(os.path.realpath(__file__)) + # model_path = src_path + "/../../../soccer_object_detection/models/half_5.pt" + # + # n = ObjectDetectionNode(model_path=model_path) + # n.robot_state.status = RobotState.STATUS_READY + # n.game_state.gameState = GameState.GAMESTATE_PLAYING + # + # Camera.reset_position = MagicMock() + # Camera.ready = MagicMock() + # do = DetectorObjects() + # do.robot_state.status = RobotState.STATUS_READY + # + # cvbridge = CvBridge() + # for file_name in os.listdir(f"{test_path}/images"): + # print(file_name) + # img: Mat = cv2.imread(os.path.join(f"{test_path}/images", file_name)) # ground truth box = (68, 89) (257, 275) + # img_original_size = img.size + # img = cv2.resize(img, dsize=(640, 480)) + # img_msg: Image = cvbridge.cv2_to_imgmsg(img) + # + # # Mock the detections + # n.pub_detection = MagicMock() + # n.pub_boundingbox = MagicMock() + # n.pub_detection.get_num_connections = MagicMock(return_value=1) + # n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) + # n.pub_detection.publish = MagicMock() + # n.pub_boundingbox.publish = MagicMock() + # + # ci = CameraInfo() + # ci.height = img.shape[0] + # ci.width = img.shape[1] + # n.camera.camera_info = ci + # do.camera.camera_info = ci + # n.camera.pose = Transformation(position=[0, 0, 0.46], orientation_euler=[0, np.pi / 8, 0]) + # do.camera.pose = n.camera.pose + # + # # n.get_model_output(img_msg) + # detection_image = n.get_model_output(img) # send the image directly + # + # # Check assertion + # # if n.pub_boundingbox.publish.call_args is not None: + # # bounding_boxes = n.pub_boundingbox.publish.call_args[0][0] + # # do.objectDetectorCallback(bounding_boxes) + # + # if "DISPLAY" in os.environ: + # # mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) + # # cv2.imshow("Image", mat) + # # cv2.imshow("Image", img) + # cv2.imshow("Detections", detection_image) + # cv2.waitKey() + # + # cv2.destroyAllWindows() diff --git a/soccer_perception/soccer_object_localization/test/test_object_localization_ros.py b/soccer_perception/soccer_object_localization/test/test_object_localization_ros.py new file mode 100644 index 000000000..444b4a7cf --- /dev/null +++ b/soccer_perception/soccer_object_localization/test/test_object_localization_ros.py @@ -0,0 +1,63 @@ +import os +from os.path import expanduser +from unittest import TestCase +from unittest.mock import MagicMock + +import cv2 +import rospy +from cv2 import Mat +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from soccer_object_localization.detector_fieldline_ros import DetectorFieldlineRos + +from soccer_common.perception.camera_calculations_ros import CameraCalculationsRos +from soccer_common.utils import download_dataset + +os.environ["ROS_NAMESPACE"] = "/robot1" + + +# TODO fix unit test +class TestObjectLocalizationRos(TestCase): + # TODO Is this necessary or should we have tests like traj to bo on the real robot + + def test_fieldline_detection_ros(self): + rospy.init_node("test") + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + test_path = src_path + "data/images/fieldlines" + + download_dataset("https://drive.google.com/uc?id=1nJX6ySks_a7mESvCm3sNllmJTNpm-x2_", folder_path=test_path) + + # TODO should do on real world img set + # TODO we shouldnt need this + CameraCalculationsRos.reset_position = MagicMock() + d = DetectorFieldlineRos() + + # ROS + d.image_publisher.get_num_connections = MagicMock(return_value=1) + d.publish_point_cloud = True # TODO is this really needed + d.point_cloud_publisher.get_num_connections = MagicMock(return_value=1) + + cvbridge = CvBridge() + + for file_name in os.listdir(test_path): + # file_name = "img160_-1.452993567956688_-3.15_0.7763055830612666.png" + + print(file_name) + img: Mat = cv2.imread(os.path.join(test_path, file_name)) + + # ROS + img_msg: Image = cvbridge.cv2_to_imgmsg(img, encoding="rgb8") + d.image_publisher.publish = MagicMock() + d.image_callback(img_msg, debug=False) + + if "DISPLAY" in os.environ: + cv2.imshow("Before", img) + cv2.imwrite("/tmp/before.png", img) + + if d.image_publisher.publish.call_count != 0: + img_out = cvbridge.imgmsg_to_cv2(d.image_publisher.publish.call_args[0][0]) + cv2.imshow("After", img_out) + cv2.imwrite("/tmp/after.png", img_out) + + cv2.waitKey(0) + cv2.destroyAllWindows() diff --git a/soccer_strategy/src/soccer_strategy/communication/game_controller_receiver.py b/soccer_strategy/src/soccer_strategy/communication/game_controller_receiver.py index 85acc49ad..5f73530f7 100755 --- a/soccer_strategy/src/soccer_strategy/communication/game_controller_receiver.py +++ b/soccer_strategy/src/soccer_strategy/communication/game_controller_receiver.py @@ -26,7 +26,7 @@ class GameStateReceiver: GAME_CONTROLLER_ANSWER_PORT = 3939 def __init__(self): - self.team_id = int(os.getenv("ROBOCUP_TEAM_ID", 10)) + self.team_id = int(os.getenv("ROBOCUP_TEAM_ID", 16)) self.robot_id = rospy.get_param("robot_id", 1) rospy.loginfo("Listening to " + str(self.GAME_CONTROLLER_LISTEN_PORT) + " " + str(self.GAME_CONTROLLER_ANSWER_PORT)) diff --git a/soccer_strategy/src/soccer_strategy/old/robot_controlled_2d.py b/soccer_strategy/src/soccer_strategy/old/robot_controlled_2d.py index 67f82b973..e7443cd01 100644 --- a/soccer_strategy/src/soccer_strategy/old/robot_controlled_2d.py +++ b/soccer_strategy/src/soccer_strategy/old/robot_controlled_2d.py @@ -5,7 +5,7 @@ import rospy from soccer_pycontrol import path -from soccer_common.camera import Camera +from soccer_common.perception.camera import Camera from soccer_common.transformation import Transformation from soccer_common.utils import wrapToPi from soccer_strategy.old.ball import Ball diff --git a/soccerbot/launch/modules/localization.launch b/soccerbot/launch/modules/localization.launch index 6a5ae8e8e..d06ea7752 100644 --- a/soccerbot/launch/modules/localization.launch +++ b/soccerbot/launch/modules/localization.launch @@ -10,44 +10,44 @@ - - - + + + - - + + - - - - + + + + - - - - - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + - + diff --git a/soccerbot/launch/soccerbot.launch b/soccerbot/launch/soccerbot.launch index 43e15f09d..bf1f3c1ff 100644 --- a/soccerbot/launch/soccerbot.launch +++ b/soccerbot/launch/soccerbot.launch @@ -21,33 +21,33 @@ " /> - + + + + + + + + + + - + + + + + + + + + + - - - + + + @@ -61,69 +61,69 @@ - - - - + + + + - - - - + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + diff --git a/soccerbot/launch/soccerbot_multi.launch b/soccerbot/launch/soccerbot_multi.launch index 96103809a..44d1f45bc 100644 --- a/soccerbot/launch/soccerbot_multi.launch +++ b/soccerbot/launch/soccerbot_multi.launch @@ -13,7 +13,7 @@ - + @@ -21,23 +21,23 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + diff --git a/test_integration.py b/test_integration.py index c679756e3..622d3c435 100644 --- a/test_integration.py +++ b/test_integration.py @@ -15,7 +15,7 @@ from sensor_msgs.msg import JointState from timeout_decorator import timeout_decorator -from soccer_common.camera import Camera +from soccer_common.perception.camera import Camera from soccer_msgs.msg import BoundingBoxes, RobotState from soccer_strategy.old.team import Team @@ -316,7 +316,7 @@ def test_annotate_ball(self, num_samples=50, create_localization_labels=False): continue self.camera.reset_position(from_world_frame=True, camera_frame="/camera_gt", timestamp=image_msg.header.stamp) - label = self.camera.calculateBoundingBoxesFromBall(Transformation(ball_position), ball_radius) + label = self.camera.calculate_bounding_boxes_from_ball(Transformation(ball_position), ball_radius) # Draw the rectangle pt1 = (round(label[0][0]), round(label[0][1]))