diff --git a/GEMstack/knowledge/calibration/cameras.yaml b/GEMstack/knowledge/calibration/cameras.yaml index 4872db65d..cd5477a18 100644 --- a/GEMstack/knowledge/calibration/cameras.yaml +++ b/GEMstack/knowledge/calibration/cameras.yaml @@ -1,35 +1,35 @@ -cameras: - front: - K: - - [684.83331299, 0.0, 573.37109375] - - [0.0, 684.60968018, 363.70092773] - - [0.0, 0.0, 1.0] - D: [0.0, 0.0, 0.0, 0.0, 0.0] - T_l2c: - - [ 0.0289748006, -0.999580136, 0.0000368439, -0.0307300513] - - [-0.0094993062, -0.0003122155, -0.999954834, -0.386689354 ] - - [ 0.999534999, 0.0289731321, -0.0095043721, -0.671425124 ] - - [ 0.0, 0.0, 0.0, 1.0 ] - T_l2v: - - [ 0.99939639, 0.02547917, 0.023615, 1.1 ] - - [ -0.02530848, 0.99965156, -0.00749882, 0.03773583 ] - - [ -0.02379784, 0.00689664, 0.999693, 1.95320223 ] - - [ 0.0, 0.0, 0.0, 1.0 ] - - front_right: - K: - - [1176.25545, 0.0, 966.432645] - - [0.0, 1175.14569, 608.580326] - - [0.0, 0.0, 1.0 ] - D: [-0.270136325, 0.164393255, -0.00160720782, -0.0000741246708, -0.0619939758] - T_l2c: - - [-0.71836368, -0.69527204, -0.02346088, 0.05718003] - - [-0.09720448, 0.13371206, -0.98624154, -0.15983010] - - [ 0.68884317, -0.70619960, -0.16363744, -1.04767285] - - [ 0.0, 0.0, 0.0, 1.0 ] - T_l2v: - - [0.99939639, 0.02547917, 0.023615, 1.1] - - [-0.02530848, 0.99965156, -0.00749882, 0.03773583] - - [-0.02379784, 0.00689664, 0.999693, 1.95320223] - - [0.0, 0.0, 0.0, 1.0 ] - +cameras: + front: + K: + - [684.83331299, 0.0, 573.37109375] + - [0.0, 684.60968018, 363.70092773] + - [0.0, 0.0, 1.0] + D: [0.0, 0.0, 0.0, 0.0, 0.0] + T_l2c: + - [ 0.0289748006, -0.999580136, 0.0000368439, -0.0307300513] + - [-0.0094993062, -0.0003122155, -0.999954834, -0.386689354 ] + - [ 0.999534999, 0.0289731321, -0.0095043721, -0.671425124 ] + - [ 0.0, 0.0, 0.0, 1.0 ] + T_l2v: + - [ 0.99939639, 0.02547917, 0.023615, 1.1 ] + - [ -0.02530848, 0.99965156, -0.00749882, 0.03773583 ] + - [ -0.02379784, 0.00689664, 0.999693, 1.95320223 ] + - [ 0.0, 0.0, 0.0, 1.0 ] + + front_right: + K: + - [1176.25545, 0.0, 966.432645] + - [0.0, 1175.14569, 608.580326] + - [0.0, 0.0, 1.0 ] + D: [-0.270136325, 0.164393255, -0.00160720782, -0.0000741246708, -0.0619939758] + T_l2c: + - [-0.71836368, -0.69527204, -0.02346088, 0.05718003] + - [-0.09720448, 0.13371206, -0.98624154, -0.15983010] + - [ 0.68884317, -0.70619960, -0.16363744, -1.04767285] + - [ 0.0, 0.0, 0.0, 1.0 ] + T_l2v: + - [0.99939639, 0.02547917, 0.023615, 1.1] + - [-0.02530848, 0.99965156, -0.00749882, 0.03773583] + - [-0.02379784, 0.00689664, 0.999693, 1.95320223] + - [0.0, 0.0, 0.0, 1.0 ] + diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index b894a36b7..418b0b512 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -21,6 +21,9 @@ components: - lane_detection: inputs: [vehicle, roadgraph] outputs: vehicle_lane + - parking_detection: + inputs: obstacles + outputs: [goal, obstacles] - sign_detection: inputs: [vehicle, roadgraph] outputs: roadgraph.signs @@ -49,6 +52,12 @@ components: - driving_logic: inputs: outputs: intent + - _mission_planner: # one way + inputs: all + outputs: mission_plan + - _route_planner: # one way + inputs: all + outputs: route - motion_planning: inputs: all outputs: trajectory @@ -57,4 +66,4 @@ components: outputs: - signaling: inputs: [intent] - outputs: + outputs: \ No newline at end of file diff --git a/GEMstack/knowledge/detection/cone.pt b/GEMstack/knowledge/detection/cone.pt new file mode 100644 index 000000000..002a39ede Binary files /dev/null and b/GEMstack/knowledge/detection/cone.pt differ diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py new file mode 100644 index 000000000..d6e66aed8 --- /dev/null +++ b/GEMstack/onboard/perception/parking_detection.py @@ -0,0 +1,205 @@ +import rospy +import yaml +import numpy as np +from sensor_msgs.msg import PointCloud2 +from typing import Dict +from ..component import Component +from ...state import ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, ObstacleStateEnum +from ..interface.gem import GEMInterface +from .utils.constants import * +from .utils.detection_utils import * +from .utils.parking_utils import * +from .utils.visualization_utils import * + + +class ParkingSpotsDetector3D(Component): + def __init__( + self, + vehicle_interface: GEMInterface, + camera_name: str, + camera_calib_file: str, + visualize_3d: bool = True + ): + # Init Variables + self.vehicle_interface = vehicle_interface + self.cone_pts_3D = [] + self.ordered_cone_ground_centers_2D = [] + self.goal_parking_spot = None + self.parking_obstacles_pose = [] + self.parking_obstacles_dim = [] + self.ground_threshold = GROUND_THRESHOLD + self.visualization = visualize_3d + + # Load camera lidar to vehicle transform from the supplied YAML + with open(camera_calib_file, 'r') as f: + calib = yaml.safe_load(f) + cam_cfg = calib['cameras'][camera_name] + self.T_l2v = np.array(cam_cfg['T_l2v']) + + if self.T_l2v is None: + rospy.logerr("Camera calibration information missing. Please load the correct config.") + + # Subscribers (note: we need top lidar only for visualization) + self.lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, self.callback, queue_size=1) + # self.lidar_sub = self.vehicle_interface.subscribe_sensor("top_lidar", self.callback) + + # Publishers (all topics are for visualization purposes) + self.pub_lidar_top_vehicle_pc2 = rospy.Publisher("lidar_top_vehicle/point_cloud", PointCloud2, queue_size=10) + self.pub_vehicle_marker = rospy.Publisher("vehicle/marker", MarkerArray, queue_size=10) + self.pub_cones_centers_pc2 = rospy.Publisher("cones_detection/centers/point_cloud", PointCloud2, queue_size=10) + self.pub_parking_spot_marker = rospy.Publisher("parking_spot_detection/marker", MarkerArray, queue_size=10) + self.pub_polygon_marker = rospy.Publisher("polygon_detection/marker", MarkerArray, queue_size=10) + self.pub_obstacles_marker = rospy.Publisher("obstacle_detection/marker", MarkerArray, queue_size=10) + + def callback(self, top_lidar_msg): + # Top lidar points in ouster frame + lidar_ouster_frame = pc2_to_numpy(top_lidar_msg) + + # Visualize everything in vehicle frame + if self.visualization: + self.visualize( + self.cone_pts_3D, + self.ordered_cone_ground_centers_2D, + self.goal_parking_spot, + self.parking_obstacles_pose, + self.parking_obstacles_dim, + lidar_ouster_frame + ) + + def spin(self): + rospy.spin() + + def rate(self) -> float: + return 10.0 # Hz + + def state_inputs(self) -> list: + return ['obstacles'] + + def state_outputs(self) -> list: + return ['goal', 'obstacles'] + + + def visualize(self, + cone_pts_3D, + ordered_cone_ground_centers_2D, + goal_parking_spot, + parking_obstacles_pose, + parking_obstacles_dim, + lidar_ouster_frame): + # Transform top lidar pointclouds to vehicle frame for visualization + latest_lidar_vehicle = transform_points(lidar_ouster_frame, self.T_l2v) + latest_lidar_vehicle = filter_ground_points(latest_lidar_vehicle, self.ground_threshold) + ros_lidar_top_vehicle_pc2 = create_point_cloud(latest_lidar_vehicle, LIDAR_PC_COLOR, VEHICLE_FRAME) + self.pub_lidar_top_vehicle_pc2.publish(ros_lidar_top_vehicle_pc2) + + # Create vehicle marker + ros_vehicle_marker = create_markers([VEHICLE_FRAME_ORIGIN], + [VEHICLE_MARKER_DIM], + VEHICLE_MARKER_COLOR, + "markers", VEHICLE_FRAME) + self.pub_vehicle_marker.publish(ros_vehicle_marker) + + # Delete previous markers + ros_delete_polygon_marker = delete_markers("polygon", MAX_POLYGON_MARKERS) + self.pub_polygon_marker.publish(ros_delete_polygon_marker) + ros_delete_parking_spot_markers = delete_markers("parking_spot", MAX_PARKING_SPOT_MARKERS) + self.pub_parking_spot_marker.publish(ros_delete_parking_spot_markers) + ros_delete_obstacles_markers = delete_markers("obstacles", MAX_OBSTACLE_MARKERS) + self.pub_obstacles_marker.publish(ros_delete_obstacles_markers) + + # Draw polygon first + if len(ordered_cone_ground_centers_2D) > 0: + ros_polygon_marker = create_polygon_marker(ordered_cone_ground_centers_2D, ref_frame=VEHICLE_FRAME) + self.pub_polygon_marker.publish(ros_polygon_marker) + + # Create parking spot marker + if goal_parking_spot: + ros_parking_spot_marker = create_parking_spot_marker(goal_parking_spot, ref_frame=VEHICLE_FRAME) + self.pub_parking_spot_marker.publish(ros_parking_spot_marker) + + # Create parking obstacles marker + if parking_obstacles_pose and parking_obstacles_dim: + ros_obstacles_marker = create_markers(parking_obstacles_pose, + parking_obstacles_dim, + OBSTACLE_MARKER_COLOR, + "obstacles", VEHICLE_FRAME) + self.pub_obstacles_marker.publish(ros_obstacles_marker) + + # Draw 3D cone centers + if len(cone_pts_3D) > 0: + cone_ground_centers = np.array(cone_pts_3D) + cone_ground_centers[:, 2] = 0.0 + cone_ground_centers = [tuple(point) for point in cone_ground_centers] + ros_cones_centers_pc2 = create_point_cloud(cone_ground_centers, color=CONE_CENTER_PC_COLOR) + self.pub_cones_centers_pc2.publish(ros_cones_centers_pc2) + + + def update(self, cone_obstacles: Dict[str, Obstacle]): + # Initial variables + goal_parking_spot = None + parking_obstacles_pose = [] + parking_obstacles_dim = [] + ordered_cone_ground_centers_2D = [] + + # Populate cone points + cone_pts_3D = [] + for cone in cone_obstacles.values(): + cone_pt_3D = (cone.pose.x, cone.pose.y, 0.0) + cone_pts_3D.append(cone_pt_3D) + + # Detect parking spot + if len(cone_pts_3D) == NUM_CONES_PER_PARKING_SPOT: + goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(cone_pts_3D) + + # Update local variables for visualization + self.cone_pts_3D = cone_pts_3D + self.ordered_cone_ground_centers_2D = ordered_cone_ground_centers_2D + self.goal_parking_spot = goal_parking_spot + self.parking_obstacles_pose = parking_obstacles_pose + self.parking_obstacles_dim = parking_obstacles_dim + + # Return if no goal parking spot is found + if not goal_parking_spot: + return None + + # Constructing parking obstacles + current_time = self.vehicle_interface.time() + obstacle_id = 0 + parking_obstacles = {} + for o_pose, o_dim in zip(parking_obstacles_pose, parking_obstacles_dim): + x, y, z, yaw = o_pose + obstacle_pose = ObjectPose( + t=current_time, + x=x, + y=y, + z=z, + yaw=yaw, + pitch=0.0, + roll=0.0, + frame=ObjectFrameEnum.CURRENT + ) + new_obstacle = Obstacle( + pose=obstacle_pose, + dimensions=o_dim, + outline=None, + material=ObstacleMaterialEnum.BARRIER, + collidable=True, state=ObstacleStateEnum.STANDING + ) + parking_obstacles[f"parking_obstacle_{obstacle_id}"] = new_obstacle + obstacle_id += 1 + + # Constructing goal pose + x, y, yaw = goal_parking_spot + goal_pose = ObjectPose( + t=current_time, + x=x, + y=y, + z=0.0, + yaw=yaw, + pitch=0.0, + roll=0.0, + frame=ObjectFrameEnum.CURRENT + ) + + new_state = [goal_pose, parking_obstacles] + return new_state \ No newline at end of file diff --git a/GEMstack/onboard/perception/utils/constants.py b/GEMstack/onboard/perception/utils/constants.py new file mode 100644 index 000000000..759dbcf74 --- /dev/null +++ b/GEMstack/onboard/perception/utils/constants.py @@ -0,0 +1,34 @@ +import yaml + +# Ignore certain yaml tag +def ignore_relative_path(loader, node): + return loader.construct_scalar(node) +yaml.SafeLoader.add_constructor('!relative_path', ignore_relative_path) + +# Load vehicle geometry +vehicle_geometry = None +with open('./GEMstack/knowledge/vehicle/gem_e4_geometry.yaml', 'r') as f: + vehicle_geometry = yaml.safe_load(f) + +# Vehicle geometry constants +GEM_E4_LENGTH = 3.2 +GEM_E4_WIDTH = 1.7 +if vehicle_geometry: + GEM_E4_LENGTH = vehicle_geometry['length'] + GEM_E4_WIDTH = vehicle_geometry['width'] + +# Parking constants +GROUND_THRESHOLD = -0.15 +NUM_CONES_PER_PARKING_SPOT = 4 +VEHICLE_FRAME = "vehicle" + +# Visualization constants +VEHICLE_FRAME_ORIGIN = [0.0, 0.0, 0.0, 0.0] +VEHICLE_MARKER_DIM = [0.8, 0.5, 0.3] +VEHICLE_MARKER_COLOR = (0.0, 0.0, 1.0, 1) +OBSTACLE_MARKER_COLOR = (1.0, 0.0, 0.0, 0.4) +LIDAR_PC_COLOR = (255, 0, 0) +CONE_CENTER_PC_COLOR = (255, 0, 255) +MAX_POLYGON_MARKERS = 1 +MAX_PARKING_SPOT_MARKERS = 1 +MAX_OBSTACLE_MARKERS = 5 \ No newline at end of file diff --git a/GEMstack/onboard/perception/utils/detection_utils.py b/GEMstack/onboard/perception/utils/detection_utils.py new file mode 100644 index 000000000..bdb7361e0 --- /dev/null +++ b/GEMstack/onboard/perception/utils/detection_utils.py @@ -0,0 +1,25 @@ +import ros_numpy +import numpy as np + +def transform_points(points, transform): + ones_column = np.ones((points.shape[0], 1)) + points_extended = np.hstack((points, ones_column)) + points_transformed = ((transform @ (points_extended.T)).T) + return points_transformed[:, :3] + + +def filter_ground_points(lidar_points, ground_threshold = 0): + filtered_array = lidar_points[lidar_points[:, 2] > ground_threshold] + return filtered_array + + +def pc2_to_numpy(pc2_msg, want_rgb=False): + # Convert the ROS message to a numpy structured array + pc = ros_numpy.point_cloud2.pointcloud2_to_array(pc2_msg) + # Stack x,y,z fields to a (N,3) array + pts = np.stack((np.array(pc['x']).ravel(), + np.array(pc['y']).ravel(), + np.array(pc['z']).ravel()), axis=1) + # Apply filtering (for example, x > 0 and z in a specified range) + mask = (pts[:, 0] > 0) & (pts[:, 2] < -1.5) & (pts[:, 2] > -2.7) + return pts[mask] \ No newline at end of file diff --git a/GEMstack/onboard/perception/utils/parking_utils.py b/GEMstack/onboard/perception/utils/parking_utils.py new file mode 100644 index 000000000..21fb368d6 --- /dev/null +++ b/GEMstack/onboard/perception/utils/parking_utils.py @@ -0,0 +1,187 @@ +import cv2 +import math +import numpy as np +from scipy.spatial import ConvexHull +from .constants import * + + +def distPoint2LineAB(p, a, b): + pa = p - a + pb = p - b + ba = b - a + dist = np.abs(np.cross(pa, pb)) / np.linalg.norm(ba) + return dist + + +def calculateCandidateScore(pose, cornerPoints): + position = pose[0:2] + score = np.min([ + distPoint2LineAB(position, cornerPoints[0], cornerPoints[1]), + distPoint2LineAB(position, cornerPoints[1], cornerPoints[2]), + distPoint2LineAB(position, cornerPoints[2], cornerPoints[3]), + distPoint2LineAB(position, cornerPoints[3], cornerPoints[0]), + ]) + return score + + +def judgeRectInPolygon(rectPts:np.ndarray, polygonPts:np.ndarray): + polygon = polygonPts.reshape((-1, 1, 2)).astype(np.float32) + for pt in rectPts: + inside = cv2.pointPolygonTest(polygon, tuple(pt), measureDist=False) + if inside < 0: + return False + return True + + +def cvtPose2CarBox(carPose): + angleDegree = carPose[-1] + cx, cy = carPose[0:2] + rect = ((0, 0), (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree)) + carBox = cv2.boxPoints(rect) + carBox = carBox + np.array([cx, cy]) + return carBox + + +def find_all_candidate_parking_spots(cornerPts, angleStepDegree=10, positionStrideMeter=0.5): + cornerPts = np.array(cornerPts, dtype=np.float32) + min_x = np.min(cornerPts[:, 0]) + 0.5 + max_x = np.max(cornerPts[:, 0]) - 0.5 + min_y = np.min(cornerPts[:, 1]) + 0.5 + max_y = np.max(cornerPts[:, 1]) - 0.5 + + candidates = [] + + refAngleDegree = getMaxLenEdgeAngleDegree(cornerPts) + + for angleDegree in np.arange(-90, 90, angleStepDegree): + rect = ((0, 0), (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree)) + + for angleDegree in np.arange(refAngleDegree, refAngleDegree+1): + rect = ((0, 0), (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree)) + carBox = cv2.boxPoints(rect) + + for cx in np.arange(min_x, max_x, positionStrideMeter): + for cy in np.arange(min_y, max_y, positionStrideMeter): + car_box_shifted = carBox + np.array([cx, cy]) + + if judgeRectInPolygon(car_box_shifted, cornerPts): + candidates.append((cx, cy, angleDegree)) + return candidates + + +def findMaxLenEdgePoints(cornerPts): + maxLen = 0 + maxPt1, maxPt2 = None, None + for idx in range(-1, 3): + pt1, pt2 = cornerPts[idx], cornerPts[idx+1] + tempLen = np.linalg.norm(pt1 - pt2) + + if tempLen > maxLen: + maxPt1, maxPt2 = pt1, pt2 + maxLen = tempLen + return maxPt1, maxPt2 + + +def getMaxLenEdgeAngleDegree(cornerPts): + pt1, pt2 = findMaxLenEdgePoints(cornerPts) + connectVec = pt1 - pt2 + return - np.arctan(connectVec[0]/connectVec[1]) / np.pi * 180 + 90 + + +def drawCarPose(img, center, angleDegree, color=(0, 0, 255), scale=100): + rect = (center, (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree)) + box = cv2.boxPoints(rect) + box_scaled = np.int32(box * scale) + cv2.polylines(img, [box_scaled], isClosed=True, color=color, thickness=2) + + +def visualizeCandidateCarPoses(cornerPts, candidates, bestPose=None, scale=100): + img = np.zeros((1000, 1000, 3), dtype=np.uint8) + + parking_polygon = np.int32(cornerPts * scale) + cv2.polylines(img, [parking_polygon], isClosed=True, color=(0, 255, 0), thickness=2) + + for (x, y, angle) in candidates: + drawCarPose(img, (x, y), angle, color=(100, 100, 255), scale=scale) + + if bestPose is not None: + drawCarPose(img, bestPose[:2], bestPose[2], color=(0, 255, 0), scale=scale) + + cv2.imshow("Parking Candidates", img) + cv2.waitKey(0) + cv2.destroyAllWindows() + + +def select_best_candidate(candidates, cornerPts): + best_candidate = candidates[0] + max_score = float('-inf') + for pose in candidates: + score = calculateCandidateScore(pose, cornerPts) + if score > max_score: + best_candidate = pose + max_score = score + return best_candidate + + +def normalize_yaw(yaw): + """Normalize yaw to [0, π) for orientation equivalence.""" + return yaw % math.pi + + +def get_parking_obstacles(vertices): + vertices = np.array(vertices) + n = len(vertices) + segment_info = [] + + for i in range(n): + p1 = vertices[i] + p2 = vertices[(i + 1) % n] + center = (p1 + p2) / 2 + delta = p2 - p1 + length = np.linalg.norm(delta) + yaw = math.atan2(delta[1], delta[0]) + yaw = normalize_yaw(yaw) + segment_info.append(((center[0], center[1], 0.0, yaw), (length, 0.05, 1.0))) + + # Sort by length and remove the two shortest segments + sorted_indices = sorted(range(len(segment_info)), key=lambda i: segment_info[i][1][0]) + indices_to_remove = set(sorted_indices[:2]) + + filtered_positions = [segment_info[i][0] for i in range(len(segment_info)) if i not in indices_to_remove] + filtered_dimensions = [segment_info[i][1] for i in range(len(segment_info)) if i not in indices_to_remove] + + return filtered_positions, filtered_dimensions + +def order_points_all(points_2d): + points_np = np.array(points_2d) + hull = ConvexHull(points_np) + ordered = [points_np[i] for i in hull.vertices] + return ordered + +def detect_parking_spot(cone_3d_centers): + # Initial variables + goal_parking_spot = None + parking_obstacles_pose = [] + parking_obstacles_dim = [] + + # Get 2D cone centers + cone_ground_centers = np.array(cone_3d_centers) + cone_ground_centers_2D = cone_ground_centers[:, :2] + ordered_cone_ground_centers_2D = order_points_all(cone_ground_centers_2D) + # print(f"-----cone_ground_centers_2D: {len(ordered_cone_ground_centers_2D)}") + + # Check if points can form a polygon, if not then there is no goal parking spot + if len(cone_3d_centers) != len(ordered_cone_ground_centers_2D): + return None, [], [], [] + + # Find the valid candidates + candidates = find_all_candidate_parking_spots(ordered_cone_ground_centers_2D) + # print(f"-----candidates: {candidates}") + + # Select the best candidate parking spot + if len(candidates) > 0: + parking_obstacles_pose, parking_obstacles_dim = get_parking_obstacles(ordered_cone_ground_centers_2D) + # print(f"-----parking_obstacles: {self.parking_obstacles_pose}") + goal_parking_spot = select_best_candidate(candidates, ordered_cone_ground_centers_2D) + # print(f"-----goal_parking_spot: {self.goal_parking_spot}") + return goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D diff --git a/GEMstack/onboard/perception/utils/visualization_utils.py b/GEMstack/onboard/perception/utils/visualization_utils.py new file mode 100644 index 000000000..c11c63503 --- /dev/null +++ b/GEMstack/onboard/perception/utils/visualization_utils.py @@ -0,0 +1,233 @@ +from sensor_msgs.msg import PointField +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point +from .constants import * +import sensor_msgs.point_cloud2 as pc2 +import cv2 +import rospy +import struct +import math +import tf.transformations as tf_trans + + +def vis_2d_bbox(image, xywh, box): + # Setup + label_text = "Cone " + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.5 + font_color = (255, 255, 255) + line_type = 1 + text_thickness = 2 + + x, y, w, h = xywh + + if box.id is not None: + id = box.id.item() + else: + id = 0 + + # Draw bounding box + cv2.rectangle(image, (int(x - w / 2), int(y - h / 2)), (int(x + w / 2), int(y + h / 2)), (255, 0, 255), 3) + + # Define text label + x = int(x - w / 2) + y = int(y - h / 2) + label = label_text + str(id) + " : " + str(round(box.conf.item(), 2)) + + # Get text size + text_size, baseline = cv2.getTextSize(label, font, font_scale, line_type) + text_w, text_h = text_size + + # Position text above the bounding box + text_x = x + text_y = y - 10 if y - 10 > 10 else y + h + text_h + + # Draw main text on top of the outline + cv2.putText(image, label, (text_x, text_y - baseline), font, font_scale, font_color, text_thickness) + return image + +def create_point_cloud(points, color=(255, 0, 0), ref_frame="map"): + """ + Converts a list of (x, y, z) points into a PointCloud2 message. + """ + header = rospy.Header() + header.stamp = rospy.Time.now() + header.frame_id = ref_frame # Change to your TF frame + + fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1), + ] + + # Convert RGB color to packed float32 + r, g, b = color + packed_color = struct.unpack('f', struct.pack('I', (r << 16) | (g << 8) | b))[0] + + point_cloud_data = [(x, y, z, packed_color) for x, y, z in points] + + return pc2.create_cloud(header, fields, point_cloud_data) + +def yaw_to_quaternion(yaw): + """Convert yaw (in radians) to a normalized quaternion (x, y, z, w).""" + return tf_trans.quaternion_from_euler(0, 0, yaw) + +def create_markers(poses, dimensions, color = (0.0, 1.0, 1.5, 0.2), ns="markers", ref_frame="map"): + """ + Create 3D bbox markers from centroids and dimensions + """ + marker_array = MarkerArray() + + for i, (pose, dimension) in enumerate(zip(poses, dimensions)): + # Skip if no centroid or dimension + if (pose == None) or (dimension == None): + continue + + marker = Marker() + marker.header.frame_id = ref_frame # Reference frame + marker.header.stamp = rospy.Time.now() + marker.ns = ns + marker.id = i # Unique ID for each marker + marker.type = Marker.CUBE # Cube for bounding box + marker.action = Marker.ADD + + # Position (center of the bounding box) + c_x, c_y, c_z, yaw = pose + if (not isinstance(c_x, float)) or (not isinstance(c_y, float)) or (not isinstance(c_z, float)): + continue + + marker.pose.position.x = c_x + marker.pose.position.y = c_y + marker.pose.position.z = c_z + + # Orientation (default, no rotation) + q = yaw_to_quaternion(yaw) + marker.pose.orientation.x = q[0] + marker.pose.orientation.y = q[1] + marker.pose.orientation.z = q[2] + marker.pose.orientation.w = q[3] + # Bounding box dimensions + d_x, d_y, d_z = dimension + if (not isinstance(d_x, float)) or (not isinstance(d_y, float)) or (not isinstance(d_z, float)): + continue + + marker.scale.x = d_x + marker.scale.y = d_y + marker.scale.z = d_z + + # Random colors for each bounding box + r, g, b, a = color + marker.color.r = r # Varying colors + marker.color.g = g + marker.color.b = b + marker.color.a = a # Transparency + + marker.lifetime = rospy.Duration() # Persistent + marker_array.markers.append(marker) + return marker_array + + +def create_polygon_marker(vertices_2d, ref_frame="map"): + marker_array = MarkerArray() + + marker = Marker() + marker.header.frame_id = ref_frame + marker.header.stamp = rospy.Time.now() + marker.ns = "polygon" + marker.id = 0 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + + # Style + marker.scale.x = 0.1 # Line width + + # Color (blue) + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + + marker.pose.orientation.w = 1.0 + + # Convert 2D vertices into geometry_msgs/Point with z=0.0 + for vx, vy in vertices_2d: + marker.points.append(Point(x=vx, y=vy, z=0.0)) + + # Close the loop by repeating the first point + marker.points.append(Point(x=vertices_2d[0][0], y=vertices_2d[0][1], z=0.0)) + + marker_array.markers.append(marker) + return marker_array + + +def create_parking_spot_marker(closest_spot, length=GEM_E4_LENGTH, width=GEM_E4_WIDTH, ref_frame="map"): + marker_array = MarkerArray() + + marker = Marker() + marker.header.frame_id = ref_frame + marker.header.stamp = rospy.Time.now() + marker.ns = "parking_spot" + marker.id = 0 + marker.type = Marker.TRIANGLE_LIST + marker.action = Marker.ADD + + # Transparency and color (green) + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.5 + + marker.pose.orientation.w = 1.0 + + # Not used in TRIANGLE_LIST, but required + marker.scale.x = 1.0 + marker.scale.y = 1.0 + marker.scale.z = 1.0 + + x, y, theta_deg = closest_spot + + # Convert orientation to radians + theta = math.radians(theta_deg) + + # Half dimensions + dx = length / 2.0 + dy = width / 2.0 + + # Define rectangle corners (local frame, clockwise) + local_corners = [ + (-dx, -dy), + (-dx, dy), + (dx, dy), + (dx, -dy) + ] + + # Transform to global frame + global_corners = [] + for lx, ly in local_corners: + gx = x + lx * math.cos(theta) - ly * math.sin(theta) + gy = y + lx * math.sin(theta) + ly * math.cos(theta) + global_corners.append(Point(x=gx, y=gy, z=0.0)) + + # Define two triangles to fill the rectangle + marker.points.append(global_corners[0]) + marker.points.append(global_corners[2]) + marker.points.append(global_corners[1]) + + marker.points.append(global_corners[0]) + marker.points.append(global_corners[3]) + marker.points.append(global_corners[2]) + + marker_array.markers.append(marker) + return marker_array + + +def delete_markers(ns="markers", max_markers=15): + marker_array = MarkerArray() + for i in range(max_markers): + marker = Marker() + marker.ns = ns + marker.id = i + marker.action = Marker.DELETE + marker_array.markers.append(marker) + return marker_array \ No newline at end of file diff --git a/GEMstack/onboard/planning/parking_component.py b/GEMstack/onboard/planning/parking_component.py new file mode 100644 index 000000000..6860fd357 --- /dev/null +++ b/GEMstack/onboard/planning/parking_component.py @@ -0,0 +1,40 @@ +from typing import List +from ..component import Component +from ...utils import serialization +from ...state import Route, ObjectFrameEnum, AllState, PlannerEnum, MissionObjective +import os +import numpy as np +import time + +class ParkingSim(Component): + def __init__(self): + self.start_time = time.time() + pass + + def state_inputs(self): + return ["all"] + + def rate(self): + return 10.0 + + def state_outputs(self)-> List[str]: + return ["mission_plan"] + + def update(self, state: AllState): + # Calculate elapsed time since initialization. + elapsed_time = time.time() - self.start_time + + # Reading goal from state + # print(f"\n AllState (parking goal): {state.goal} \n") + # print(f"AllState (parking obstacles): {state.obstacles} \n") + + # After a goal is detected, change the mission plan to use PARKING. + if state.goal: + print("\n Parking goal available. Entering PARKING mode......") + mission_plan = MissionObjective(PlannerEnum.PARKING, state.goal) + else: + print("\n Entering SCANNING mode......") + mission_plan = MissionObjective(PlannerEnum.SCANNING) + + print(f"\n ParkingSim update with state: {mission_plan} \n") + return mission_plan \ No newline at end of file diff --git a/GEMstack/onboard/planning/route_planning_component.py b/GEMstack/onboard/planning/route_planning_component.py new file mode 100644 index 000000000..3eb85d4e0 --- /dev/null +++ b/GEMstack/onboard/planning/route_planning_component.py @@ -0,0 +1,47 @@ +import os +from typing import Dict, List + +import numpy as np +from GEMstack.onboard.component import Component +from GEMstack.state.agent import AgentState +from GEMstack.state.all import AllState +from GEMstack.state.physical_object import ObjectFrameEnum +from GEMstack.state.route import PlannerEnum, Route + + + +class RoutePlanningComponent(Component): + """Reads a route from disk and returns it as the desired route.""" + def __init__(self): + print("Route Planning Component init") + self.planner = None + self.route = None + + def state_inputs(self): + return ["all"] + + def state_outputs(self) -> List[str]: + return ['route'] + + def rate(self): + return 10.0 + + def update(self, state: AllState): + # print("Route Planner's mission:", state.mission_plan.planner_type.value) + # print("type of mission plan:", type(PlannerEnum.RRT_STAR)) + # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value) + # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.PARKING.value) + # print("Mission plan:", state.mission_plan) + # print("Vehicle x:", state.vehicle.pose.x) + # print("Vehicle y:", state.vehicle.pose.y) + # print("Vehicle yaw:", state.vehicle.pose.yaw) + if state.mission_plan.type.name == "PARKING": + print("I am in PARKING mode") + # Return a route after doing some processing based on mission plan REMOVE ONCE OTHER PLANNERS ARE IMPLEMENTED + + elif state.mission_plan.type.name == "SCANNING": + print("I am in SCANNING mode") + else: + print("Unknown mode") + + return self.route \ No newline at end of file diff --git a/GEMstack/state/__init__.py b/GEMstack/state/__init__.py index 5ec5f71c2..84efba805 100644 --- a/GEMstack/state/__init__.py +++ b/GEMstack/state/__init__.py @@ -13,10 +13,12 @@ 'AgentState','AgentEnum','AgentActivityEnum', 'ObstacleMaterialEnum','ObstacleStateEnum', 'SceneState', + 'ObstacleMaterialEnum','ObstacleStateEnum', 'VehicleIntent','VehicleIntentEnum', 'AgentIntent', 'EntityRelationEnum','EntityRelation','EntityRelationGraph', - 'MissionEnum','MissionObjective', + 'MissionEnum','MissionObjective', 'MissionPlan', + 'PlannerEnum', 'Route', 'PredicateValues', 'AllState'] @@ -33,6 +35,7 @@ from .agent_intent import AgentIntent from .relations import EntityRelation, EntityRelationEnum, EntityRelationGraph from .mission import MissionEnum,MissionObjective -from .route import Route +from .route import Route, PlannerEnum +from .mission import MissionObjective from .predicates import PredicateValues from .all import AllState diff --git a/GEMstack/state/all.py b/GEMstack/state/all.py index fc976fe24..4fb863ed5 100644 --- a/GEMstack/state/all.py +++ b/GEMstack/state/all.py @@ -7,6 +7,7 @@ from .agent_intent import AgentIntent,AgentIntentMixture from .relations import EntityRelation from .mission import MissionObjective +from .mission import MissionObjective from .route import Route from .trajectory import Trajectory from .predicates import PredicateValues @@ -23,10 +24,13 @@ class AllState(SceneState): agent_intents : Dict[str,AgentIntentMixture] = field(default_factory=dict) relations : List[EntityRelation] = field(default_factory=list) predicates : PredicateValues = field(default_factory=PredicateValues) + goal: ObjectPose = None # planner-output state mission : MissionObjective = field(default_factory=MissionObjective) + mission_plan: MissionObjective = None intent : VehicleIntent = field(default_factory=VehicleIntent) + # planner_type : Optional[PlannerEnum] = None route : Optional[Route] = None trajectory : Optional[Trajectory] = None diff --git a/GEMstack/state/mission.py b/GEMstack/state/mission.py index 72954e574..dd120abc5 100644 --- a/GEMstack/state/mission.py +++ b/GEMstack/state/mission.py @@ -3,6 +3,8 @@ from . import ObjectPose from ..utils.serialization import register from enum import Enum +from .route import PlannerEnum + class MissionEnum(Enum): IDLE = 0 # not driving, no mission diff --git a/GEMstack/state/obstacle.py b/GEMstack/state/obstacle.py index 251fb30db..d7e5db881 100644 --- a/GEMstack/state/obstacle.py +++ b/GEMstack/state/obstacle.py @@ -31,5 +31,4 @@ class ObstacleStateEnum(Enum): class Obstacle(PhysicalObject): material : ObstacleMaterialEnum collidable : bool - state: ObstacleStateEnum - + state: ObstacleStateEnum = ObstacleStateEnum.UNKNOWN diff --git a/GEMstack/state/route.py b/GEMstack/state/route.py index e8da72d4a..8993f7810 100644 --- a/GEMstack/state/route.py +++ b/GEMstack/state/route.py @@ -6,7 +6,6 @@ from enum import Enum - class PlannerEnum(Enum): RRT_STAR = 0 #position / yaw in m / radians relative to starting pose of vehicle HYBRID_A_STAR = 1 #position / yaw in m / radians relative to current pose of vehicle @@ -16,6 +15,7 @@ class PlannerEnum(Enum): IDLE = 4 # no mission, no driving SUMMON_DRIVING = 5 # route planning with lanes PARALLEL_PARKING = 6 # route planning for parallel parking + SCANNING = 7 @dataclass @register @@ -28,4 +28,3 @@ class Route(Path): """ lanes : List[str] = field(default_factory=list) wait_lines : List[str] = field(default_factory=list) - diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml new file mode 100644 index 000000000..4745bdc12 --- /dev/null +++ b/launch/parking_detection.yaml @@ -0,0 +1,108 @@ +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# require_engaged: False +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + # agent_detection : cone_detection.ConeDetector3D + obstacle_detection : + type: cone_detection.ConeDetector3D + args: + camera_name: front_right #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + + # optional overrides + enable_tracking: False + visualize_2d: False + use_cyl_roi: False + save_data: False + orientation: False + use_start_frame: False + parking_detection: + type: parking_detection.ParkingSpotsDetector3D + args: + camera_name: front_right #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + visualize_3d: True + perception_normalization : StandardPerceptionNormalizer + planning: + _mission_planner: + type: parking_component.ParkingSim +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settingsNone structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + detector_only: + run: + description: "Run the parking spot detection code" + drive: + planning: + trajectory_tracking: + test: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + obstacle_detection : + type: cone_detection.ConeDetector3D + args: + camera_name: front_right #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + + # optional overrides + enable_tracking: False + visualize_2d: False + use_cyl_roi: False + save_data: False + orientation: False + use_start_frame: False + # agent_detection : cone_detection_simple.ConeDetectorSimple3D + parking_detection: + type: parking_detection.ParkingSpotsDetector3D + args: + camera_name: front_right #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + visualize_3d: True + planning: + _mission_planner: + type: parking_component.ParkingSim + _route_planner: + type: route_planning_component.RoutePlanningComponent + + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: [!include "klampt_visualization.yaml", !include "mpl_visualization.yaml"] + log_ros: + log: + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file