From 7fb60367299efa62934fe1b4218d6b1f29547d15 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Thu, 19 Jan 2023 17:54:10 +1100 Subject: [PATCH] renamed use of goal to node and stopped using translation3d in immutable format --- goal_map.py | 51 +++++++++++++++++++++----------------- helper_types.py | 10 ++++---- magic_numbers.py | 2 +- test_images.py | 34 ++++++++++++------------- vision.py | 64 ++++++++++++++++++++++++------------------------ 5 files changed, 83 insertions(+), 78 deletions(-) diff --git a/goal_map.py b/goal_map.py index 995b9e1..13f29ca 100644 --- a/goal_map.py +++ b/goal_map.py @@ -1,17 +1,17 @@ from helper_types import ( - GoalRegionState, - GoalRegionObservation, - GoalRegion, + NodeRegionState, + NodeRegionObservation, + NodeRegion, ExpectedGamePiece, ) from wpimath.geometry import Translation3d -class GoalRegionMap: +class NodeRegionMap: X_OFFSET_TO_GRID: float = 0.0 Y_OFFSET_TO_GRID: float = 516.763 INTAKE_ZONE_OFFSET: float = 0.0 - Y_DISTANCE_BETWEEN_GOALS: float = 558.0 + Y_DISTANCE_BETWEEN_NODES: float = 558.0 Z_DISTANCE_CUBE_LOOK_UP: list[float] = [1169.988, 865.188, 0.0] Z_DISTANCE_CONE_LOOK_UP: list[float] = [ @@ -24,40 +24,45 @@ class GoalRegionMap: X_DISTANCE_CONE_LOOK_UP: list[float] = [364.231, 795.231, 1167.655] def __init__(self, on_blue_alliance: bool): - self.map: list[GoalRegionState] = [] + self.map: list[NodeRegionState] = [] - # This will start from the top row of goals + # This will start from the top row of nodes for row in range(3): cone_height = self.Z_DISTANCE_CONE_LOOK_UP[row] cube_height = self.Z_DISTANCE_CUBE_LOOK_UP[row] # this will start from the grid closest to the field origin for grid in range(3): - # this will start from the goal closest to the field origin in each grid - for goal in range(3): - id = row * 9 + grid * 3 + goal + # this will start from the node closest to the field origin in each grid + for node in range(3): + id = row * 9 + grid * 3 + node expected_game_piece = ExpectedGamePiece.CONE - position = Translation3d() - position.y = self.Y_DISTANCE_BETWEEN_GOALS * (grid + goal) + pos_x = 0.0 + pos_y = 0.0 + pos_z = 0.0 + + pos_y = self.Y_DISTANCE_BETWEEN_NODES * (grid + node) if not on_blue_alliance: - position.y += self.INTAKE_ZONE_OFFSET - position.z = cone_height - position.x = self.X_DISTANCE_CONE_LOOK_UP[grid] + pos_y += self.INTAKE_ZONE_OFFSET + pos_z = cone_height + pos_x = self.X_DISTANCE_CONE_LOOK_UP[grid] if row == 2: expected_game_piece = ExpectedGamePiece.BOTH - position.z = cube_height - position.x = self.X_DISTANCE_CUBE_LOOK_UP[grid] - elif goal == 1: + pos_z = cube_height + pos_x = self.X_DISTANCE_CUBE_LOOK_UP[grid] + elif node == 1: expected_game_piece = ExpectedGamePiece.CUBE - position.z = cube_height - position.x = self.X_DISTANCE_CUBE_LOOK_UP[grid] + pos_z = cube_height + pos_x = self.X_DISTANCE_CUBE_LOOK_UP[grid] + + position = Translation3d(pos_x, pos_y, pos_z) self.map.append[ - (GoalRegionState(GoalRegion(id, expected_game_piece, position))) + (NodeRegionState(NodeRegion(id, expected_game_piece, position))) ] - def update(self, goal_observations: list[GoalRegionObservation]): + def update(self, node_observations: list[NodeRegionObservation]): pass - def get_state(self) -> list[GoalRegionState]: + def get_state(self) -> list[NodeRegionState]: return self.map diff --git a/helper_types.py b/helper_types.py index 32b69c6..0cd6b28 100644 --- a/helper_types.py +++ b/helper_types.py @@ -21,19 +21,19 @@ def area(self) -> int: @dataclass -class GoalRegion: +class NodeRegion: id: int expected_game_piece: ExpectedGamePiece position: Translation3d @dataclass -class GoalRegionObservation: +class NodeRegionObservation: bounding_box: BoundingBox - goal_region: GoalRegion + node_region: NodeRegion @dataclass -class GoalRegionState: - goal_region: GoalRegion +class NodeRegionState: + node_region: NodeRegion occupied: bool = False diff --git a/magic_numbers.py b/magic_numbers.py index 2084882..275a097 100644 --- a/magic_numbers.py +++ b/magic_numbers.py @@ -15,7 +15,7 @@ CONTOUR_TO_BOUNDING_BOX_AREA_RATIO_THRESHOLD = 0.1 -# Gate angle to determine if robot can even see the goal or not to avoid projecting backwards +# Gate angle to determine if robot can even see the node or not to avoid projecting backwards DIRECTION_GATE_ANGLE = pi ROBOT_BASE_TO_CAMERA_TRANSLATION = Translation3d(0.0, 0.0, 0.0) diff --git a/test_images.py b/test_images.py index 665df5d..0325a4a 100644 --- a/test_images.py +++ b/test_images.py @@ -3,7 +3,7 @@ import cv2 import vision from helper_types import BoundingBox, ExpectedGamePiece -from goal_map import GoalRegionMap +from node_map import NodeRegionMap from wpimath.geometry import Pose2d import numpy as np from camera_config import CameraParams @@ -28,33 +28,33 @@ def read_test_data_csv(fname: str): return result -def read_goal_region_in_frame_csv(fname: str): +def read_node_region_in_frame_csv(fname: str): with open(fname) as f: result = [] for ( image, - goal_region_visible, + node_region_visible, robot_x, robot_y, heading, - goal_region_id, + node_region_id, ) in csv.reader(f): result.append( ( image, - goal_region_visible == "True", + node_region_visible == "True", float(robot_x), float(robot_y), float(heading), - int(goal_region_id), + int(node_region_id), ) ) return result images = read_test_data_csv("test/expected.csv") -goal_region_in_frame_images = read_goal_region_in_frame_csv( - "test/goal_region_in_frame.csv" +node_region_in_frame_images = read_node_region_in_frame_csv( + "test/node_region_in_frame.csv" ) @@ -116,30 +116,30 @@ def test_sample_images( @pytest.mark.parametrize( - "filename,goal_region_visible,robot_x,robot_y,heading, goal_region_id", - goal_region_in_frame_images, + "filename,node_region_visible,robot_x,robot_y,heading, node_region_id", + node_region_in_frame_images, ) -def test_goal_region_in_frame( +def test_node_region_in_frame( filename: str, - goal_region_visible: bool, + node_region_visible: bool, robot_x: float, robot_y: float, heading: float, - goal_region_id: int, + node_region_id: int, ): image = cv2.imread(f"./test/{filename}") assert image is not None - goal_region_map = GoalRegionMap() + node_region_map = NodeRegionMap() - goal_region = goal_region_map.get_state()[goal_region_id].goal_region + node_region = node_region_map.get_state()[node_region_id].node_region robot_pose = Pose2d(robot_x, robot_y, heading) camera_matrix = np.array([1, 0, 0], [0, 1, 0], [0, 0, 1]) assert ( - vision.is_goal_region_in_image(image, robot_pose, camera_matrix, goal_region) - == goal_region_visible + vision.is_node_region_in_image(image, robot_pose, camera_matrix, node_region) + == node_region_visible ) diff --git a/vision.py b/vision.py index 0bee0c2..cab8d53 100644 --- a/vision.py +++ b/vision.py @@ -14,14 +14,14 @@ import cv2 import numpy as np from helper_types import ( - GoalRegionObservation, - GoalRegion, - GoalRegionState, + NodeRegionObservation, + NodeRegion, + NodeRegionState, ExpectedGamePiece, BoundingBox, ) from camera_config import CameraParams -from goal_map import GoalRegionMap +from node_map import NodeRegionMap from wpimath.geometry import Pose2d, Pose3d, Translation3d, Transform3d, Rotation3d @@ -29,7 +29,7 @@ class Vision: def __init__(self, camera_manager: CameraManager, connection: NTConnection) -> None: self.camera_manager = camera_manager self.connection = connection - self.map = GoalRegionMap() + self.map = NodeRegionMap() def run(self) -> None: """Main process function. @@ -124,84 +124,84 @@ def is_game_piece_present( def process_image(frame: np.ndarray, pose: Pose2d): - # visible_goals = self.find_visible_goals(frame, pose) + # visible_nodes = self.find_visible_nodes(frame, pose) - # goal_states = self.detect_goal_state(frame, visible_goals) + # node_states = self.detect_node_state(frame, visible_nodes) # whatever the update step is - # self.map.update(goal_states) + # self.map.update(node_states) # map_state = self.map.get_state() # annotate frame - # annotated_frame = annotate_image(frame, map_state, goal_states) + # annotated_frame = annotate_image(frame, map_state, node_states) - # return state of map (state of all goal regions) and annotated camera stream + # return state of map (state of all node regions) and annotated camera stream return -def find_visible_goals(frame: np.ndarray, pose: Pose2d) -> list[GoalRegionObservation]: - """Segment image to find visible goals in a frame +def find_visible_nodes(frame: np.ndarray, pose: Pose2d) -> list[NodeRegionObservation]: + """Segment image to find visible nodes in a frame Args: - frame (np.ndarray): New camera frame containing goals + frame (np.ndarray): New camera frame containing nodes pose (Pose2d): Current robot pose in the world frame Returns: - List[GoalRegionObservation]: List of goal region observations with no information about occupancy + List[NodeRegionObservation]: List of node region observations with no information about occupancy """ pass -def detect_goal_state( - frame: np.ndarray, regions_of_interest: list[GoalRegionObservation] -) -> list[GoalRegionObservation]: - """Detect goal occupancy in a set of observed goal regions +def detect_node_state( + frame: np.ndarray, regions_of_interest: list[NodeRegionObservation] +) -> list[NodeRegionObservation]: + """Detect node occupancy in a set of observed node regions Args: - frame (np.ndarray): New camera frame containing goals - regions_of_interest (List[GoalRegionObservation]): List of goal region observations with no information about occupancy + frame (np.ndarray): New camera frame containing nodes + regions_of_interest (List[NodeRegionObservation]): List of node region observations with no information about occupancy Returns: - List[GoalRegionObservation]: List of goal region observations + List[NodeRegionObservation]: List of node region observations """ pass def annotate_image( frame: np.ndarray, - map: list[GoalRegionState], - goal_observations: list[GoalRegionObservation], + map: list[NodeRegionState], + node_observations: list[NodeRegionObservation], ) -> np.ndarray: - """annotate a frame with projected goal points + """annotate a frame with projected node points Args: frame (np.ndarray): raw image frame without annotation - map (List[GoalState]): current map state with information on occupancy state_ - goal_observations (List[GoalRegionObservation]): goal observations in the current time step + map (List[NodeState]): current map state with information on occupancy state_ + node_observations (List[NodeRegionObservation]): node observations in the current time step Returns: - np.ndarray: frame annotated with observed goal regions + np.ndarray: frame annotated with observed node regions """ pass -def is_goal_region_in_image( +def is_node_region_in_image( robot_pose: Pose2d, camera_params: CameraParams, - goal_region: GoalRegion, + node_region: NodeRegion, ) -> bool: # create transform to make camera origin world_to_robot = Transform3d(Pose3d(), Pose3d(robot_pose)) world_to_camera = world_to_robot + camera_params.transform - goal_region_camera_frame = world_to_camera.inverse() + Transform3d( - goal_region.position, Rotation3d() + node_region_camera_frame = world_to_camera.inverse() + Transform3d( + node_region.position, Rotation3d() ) # Check the robot is facing the right direction for the point by checking it is inside the FOV return point3d_in_field_of_view( - goal_region_camera_frame.translation(), camera_params + node_region_camera_frame.translation(), camera_params )