diff --git a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml index 273c9c99d..1840f9108 100644 --- a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml +++ b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml @@ -2,7 +2,7 @@ global_frame: enu initial_pois: circle_totems: [26.09821319580078, 59.91523361206055, 0.0] - dock: [108.05, -1.07, 0.0] + dock: [28, 22, 0.0] entrance_gate: [51.28, -48.69, 0.0] obstacle_course: [-32.803, -83.41, 0.0] ring_challenge: [61.271873474121094, 15.894840240478516, 0.0] diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch index 64d2e88bf..604f7ab1c 100644 --- a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch +++ b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_controller.launch @@ -10,4 +10,4 @@ izz: 500.58 - + \ No newline at end of file diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py b/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py index 1f9b561d3..5b0340c56 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/docking.py @@ -1,150 +1,910 @@ #!/usr/bin/env python3 +import copy +import math +import random +from typing import Optional + +import cv2 import numpy as np -from geometry_msgs.msg import Point -from mil_misc_tools import ThrowingArgumentParser -from mil_msgs.msg import ObjectsInImage -from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest -from mil_tools import rosmsg_to_numpy +import rospy +from cv_bridge import CvBridge +from dynamic_reconfigure.msg import DoubleParameter +from geometry_msgs.msg import Pose +from image_geometry import PinholeCameraModel +from mil_tools import pose_to_numpy, rosmsg_to_numpy +from nav_msgs.msg import OccupancyGrid +from navigator_vision import GripPipeline +from sensor_msgs.msg import CameraInfo, Image +from std_srvs.srv import SetBool, SetBoolRequest +from tf.transformations import quaternion_matrix from .navigator import NaviGatorMission +PANNEL_MAX = 0 +PANNEL_MIN = 2 + +CAMERA_LINK_OPTICAL = "wamv/front_left_camera_link_optical" + +COLOR_SEQUENCE_SERVICE = "/vrx/scan_dock/color_sequence" + +TIMEOUT_SECONDS = 30 + class Docking(NaviGatorMission): - @classmethod - def decode_parameters(cls, parameters): - argv = parameters.split() - return cls.parser.parse_args(argv) + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.ogrid = None + self.ogrid_origin = None + self.ogrid_cpm = None + self.ogrid_sub = self.nh.subscribe( + "/ogrid", + OccupancyGrid, + callback=self.ogrid_cb, + ) - @classmethod - async def setup(cls): - parser = ThrowingArgumentParser( - description="Dock", - usage="""Default parameters: \'runtask Docking - \'""", + self.camera_topic = rospy.get_param( + "/classifier/image_topic", + "/camera/front/left/image_color", + ) + split = self.camera_topic.split("/") + self.camera_info_topic = "" + for i in range(1, len(split) - 1): + self.camera_info_topic += "/" + split[i] + rospy.logerr(self.camera_info_topic) + + self.camera_info_topic += "/camera_info" + + # Service to save and restore the settings of PCODAR + self.pcodar_save = self.nh.get_service_client("/pcodar/save", SetBool) + self.image_sub = self.nh.subscribe( + self.camera_topic, + Image, ) - parser.add_argument("-t", "--time", type=int, default=-1) - cls.parser = parser - - cls.bboxsub = cls.nh.subscribe("/bbox_pub", ObjectsInImage) - await cls.bboxsub.setup() - cls.camera_lidar_tf = cls.nh.get_service_client( - "/camera_to_lidar/front_right_cam", - CameraToLidarTransform, + self.cam_frame = None + self.image_info_sub = self.nh.subscribe( + self.camera_info_topic, + CameraInfo, ) + self.model = PinholeCameraModel() + self.center = None + self.intup = lambda arr: tuple(np.array(arr, dtype=np.int64)) + self.last_image = None + + self.contour_pub = self.nh.advertise("/contour_pub", Image) + + @classmethod + async def init(cls): + pass @classmethod async def shutdown(cls): - await cls.bboxsub.shutdown() + await cls.contour_pub.shutdown() + await cls.ogrid_sub.shutdown() + await cls.image_sub.shutdown() + await cls.image_info_sub.shutdown() + await cls.pcodar_save.shutdown() async def run(self, args): - # Parse Arguments - wait_time = args.time - - # Find Dock - dock_position = None - largest_size = 0 - boat_pos = (self.tx_pose)[0] - # Get 10 closest unclassified objects - unclass = await self.get_sorted_objects(name="UNKNOWN", n=10, throw=False) - for obj in unclass[0]: - point = rosmsg_to_numpy(obj.pose.position) - scale = rosmsg_to_numpy(obj.scale) - - # Filter such that we know the dock is closer than 20 meters - if np.linalg.norm(point - boat_pos) > 20: - break - - size = scale[0] * scale[1] - - if size > largest_size: - largest_size = size - dock_position = point - - if dock_position is None: - self.send_feedback("Cancelling, failed to find dock position") + await self.contour_pub.setup() + rospy.logerr("RUN START") + await self.ogrid_sub.setup() + rospy.logerr("OGRID DONE") + await self.image_sub.setup() + rospy.logerr("IMAGE DONE") + await self.image_info_sub.setup() + rospy.logerr("INFO DONE") + await self.pcodar_save.wait_for_service() + rospy.logerr("PCODAR DONE") + + self.grip = GripPipeline() + await self.change_wrench("autonomous") + rospy.logerr("WRENCH SET") + + self.bridge = CvBridge() + msg = await self.image_info_sub.get_next_message() + self.model.fromCameraInfo(msg) + rospy.logerr("HERE") + + self.cam_frame = (await self.image_sub.get_next_message()).header.frame_id + rospy.logerr("HERE2") + + # Save PCODAR settings + await self.pcodar_save(SetBoolRequest(True)) + + # Change cluster tolerance to make it easier to find dock + pcodar_cluster_tol = DoubleParameter() + pcodar_cluster_tol.name = "cluster_tolerance_m" + pcodar_cluster_tol.value = 10 + await self.pcodar_set_params(doubles=[pcodar_cluster_tol]) + rospy.logerr("HERE3") + + # Get the POI (point of interest) from the config file and move to it + # This is a predetermined position of the general location for the dock + pos = await self.poi.get("dock") + rospy.logerr("HERE4") + pos = pos[0] + await self.move.look_at(pos).go() # Face the dock + await self.move.set_position(pos).look_at(pos).go() + + # Decrease cluster tolerance as we approach dock since lidar points are more dense + # This helps scenario where stc buoy is really close to dock + pcodar_cluster_tol = DoubleParameter() + pcodar_cluster_tol.name = "cluster_tolerance_m" + pcodar_cluster_tol.value = 4 + await self.pcodar_set_params(doubles=[pcodar_cluster_tol]) + await self.nh.sleep(5) + + # move to the open side of the dock + await self.move_to_correct_side() + + # retry calculation to make sure we really found the open side + await self.move_to_correct_side() + + # TODO: IMPORTANT MESSAGE FROM WILLIAM - Change color to correct one when announced on each day. Correct strings are {"Blue", "Red", "Green"} + goal_color = "Blue" + correct_dock_number = ( + 0 # Correct dock number will be set to 0 1 or 2 after the CV code runs + ) + + # The LIDAR to camera mapping is very unreliable, so we loop until it is correct + for _ in range(0, 10): + # get the dock object from the database + dock, pos = await self.get_sorted_objects(name="dock", n=1) + + # dock is PerceptionObject + position, rotation, dock = self.get_dock_data(dock) + + # get LIDAR points + points = rosmsg_to_numpy(dock.points) + + # get transform from enu to boat space + enu_to_boat = await self.tf_listener.get_transform("wamv/base_link", "enu") + corrected = np.empty(points.shape) + + # convert LIDAR points from enu to boat space + for i in range(points.shape[0]): + corrected[i] = enu_to_boat.transform_point(points[i]) + + # get the centers and clusters that represent the backside of the dock + centers, clusters = self.get_cluster_centers(corrected) + + # Sort both the centers and the clusters by y coordinate + sorted_indices = centers[:, 1].argsort()[::-1] + centers = centers[sorted_indices] + clusters = [clusters[i] for i in sorted_indices] + + # crop the images to get bbox and find color + images = await self.crop_images(clusters, centers) + + # TODO: IMPORTANT MESSAGE FROM WILL - These are the preferred sizes in the sim, but the docks will be farther apart in real life, so adjust tolerances during practice + # preferred height (x) for cropped image: 170-180 + # preferred width (y) for cropped image: 130-150 + + # If the images are correct, then break. Check that the height and width are in reasonable ranges + if ( + images[0].shape[0] in range(70, 171) + and images[0].shape[1] in range(101, 180) + and images[1].shape[0] in range(70, 171) + and images[1].shape[1] in range(101, 180) + and images[2].shape[0] in range(70, 171) + and images[2].shape[1] in range(101, 180) + ): + correct_dock_number = self.find_color(images, goal_color) + if correct_dock_number != -1: + break + + rospy.logerr(f"Here is the correct dock number: {correct_dock_number}") + + # temporary code that just moves boat to center of cluster with whatever color was specified + correct_dock = copy.deepcopy(centers[correct_dock_number]) + + # calculate center of cluster and move towards it but at an offset distance + correct_dock[0] = 0 + forward = copy.deepcopy(centers[correct_dock_number]) + # This is what calculates how far from the dock the boat docks + forward[0] = forward[0] - 3.25 + boat_to_enu = await self.tf_listener.get_transform("enu", "wamv/base_link") + correct_center = boat_to_enu.transform_point(correct_dock) + nextPt = boat_to_enu.transform_point(forward) + await self.move.set_position(correct_center).go(blind=True, move_type="skid") + await self.move.set_position(nextPt).go( + blind=True, + move_type="skid", + speed_factor=[0.25, 0.25, 0.25], + ) + + # Align with hole -> work in progress, see navigator_vision/dockdeliver_pipeline.py in navigator vision + # This part finds the holes in the docks for a future aligning algorithm that has not been implemented yet + image = await self.image_sub.get_next_message() + image = self.bridge.imgmsg_to_cv2(image) + self.grip.process(image) + + img = self.grip.hsv_threshold_output + img2 = self.grip.blur_output + contours = self.grip.filter_contours_output + + # print_image_values(img) + print(contours[0]) + centroid = np.mean(contours[0], axis=0) + print(centroid[0]) + print((centroid[0][0], centroid[0][1])) + + cv2.imshow("blur", img2) + cv2.imshow("threshold", img) + img3 = cv2.drawContours(img2, contours, -1, (0, 255, 0), 3) + img3 = cv2.circle( + img3, + ((math.ceil(centroid[0][0]), math.ceil(centroid[0][1]))), + radius=2, + color=(0, 0, 255), + thickness=1, + ) + cv2.imshow("contours", img3) + + # cv2.waitKey(0) + cv2.destroyAllWindows() + + # Shoot racquet ball projectile + rospy.logerr("- BEFORE SHOOT PROJ -") + if correct_dock_number != -1 and correct_dock_number is not None: + # TODO: Aim the raqcetball launcher into the holes. The racquetball launcher is fixed, so the boat is what needs to be aimed. + # When you get to the practice course, this can theoretically be done with, for example, + # await self.move.forward(1.3).go() + # await self.move.left(0.3).go() + # Since the boat should be in the same place in the dock whenever it docks + + await self.start_launcher() + await self.nh.sleep(2) + + for i in range(0, 4): + await self.fire_launcher() + await self.nh.sleep(2) + + await self.stop_launcher() + + await self.move.backward(10).go(blind=True) + await self.pcodar_save(SetBoolRequest(False)) + + def get_dock_data(self, dock): + dock = dock[0] + position, quat = pose_to_numpy(dock.pose) + rotation = quaternion_matrix(quat) + return position, rotation, dock + + async def move_to_correct_side(self): + await self.find_dock() + + # get a vector to the longer side of the dock + dock, pos = None, None + while dock is None or pos is None: + try: + # looks the the LIDAR cluster database and finds the object with name "dock" + dock, pos = await self.get_sorted_objects(name="dock", n=1) + except Exception as _: + # retries if an exception occurs (cannot find dock) + await self.find_dock() + await self.move.yaw_right( + 20, + unit="deg", + ).go() # Likely the boat is not facing the right direction and can't see the dock, this line fixes that + dock, pos = await self.get_sorted_objects(name="dock", n=1) + + # find the open side of the dock + side, position = self.get_correct_side(dock) + + # move the boat to the middle of the correct side offset by 2 meters back and face the center of the dock + await self.move.set_position(side).look_at(position).backward(2).go() + + def get_correct_side(self, dock): + # dock is PerceptionObject + position, rotation, dock = self.get_dock_data(dock) + + # get bounding box of the dock + bbox_large = rosmsg_to_numpy(dock.scale) + bbox_copy = copy.deepcopy(bbox_large) + + bbox_large[2] = 0 + bbox_small = copy.deepcopy(bbox_large) + + max_dim = np.argmax(bbox_large[:2]) + bbox_large[max_dim] = 0 + bbox_enu = np.dot(rotation[:3, :3], bbox_large) + + min_dim = np.argmin(bbox_small[:2]) + bbox_small[min_dim] = 0 + bbox_enu_small = np.dot(rotation[:3, :3], bbox_small) + # this black magic uses the property that a rotation matrix is just a + # rotated cartesian frame and only gets the vector that points towards + # the longest side since the vector pointing that way will be at the + # same index as the scale for the smaller side. This is genius! + # - Andrew Knee + + # move to first attempt + print("moving in front of dock") + # curr_pose = await self.tx_pose() + side_a = bbox_enu + position + side_b = -bbox_enu + position + side_c = bbox_enu_small + position + side_d = -bbox_enu_small + position + + return ( + self.calculate_correct_side( + copy.deepcopy(side_a), + copy.deepcopy(side_b), + copy.deepcopy(side_c), + copy.deepcopy(side_d), + position, + rotation[:3, :3], + bbox_copy, + ), + position, + ) + + # separates the clusters using k means clustering + def get_cluster_centers(self, data): + # fig = plt.figure() + # ax = fig.add_subplot(111, projection='3d') + # ax.plot(data[:,0],data[:,1],data[:,2]) + # plt.draw() + # plt.pause(0.1) + # input("") + # plt.close(fig) + + # cut off all points below the mean z value + mean = np.mean(data, axis=0)[2] + data = data[data[:, 2] > mean] + + # Additionally, cut of points below lower quartile x value + xq1 = np.quantile(data[:, 0], 0.25) + data = data[data[:, 0] > xq1] + centroids = [] + + # fig = plt.figure() + # ax = fig.add_subplot(111, projection='3d') + # ax.plot(data[:,0],data[:,1],data[:,2]) + # plt.draw() + # plt.pause(0.1) + # input("") + # plt.close(fig) + + # Sample initial centroids + random_indices = random.sample(range(data.shape[0]), 3) + for i in random_indices: + centroids.append(data[i]) + + # Create a list to store which centroid is assigned to each dataset + assigned_centroids = [0] * len(data) + + def compute_l2_distance(x, centroid): + # Initialise the distance to 0 + dist = 0 + + # Loop over the dimensions. Take squared difference and add to dist + + for i in range(len(x)): + dist += (centroid[i] - x[i]) ** 2 + + return dist + + def get_closest_centroid(x, centroids): + # Initialise the list to keep distances from each centroid + centroid_distances = [] + + # Loop over each centroid and compute the distance from data point. + for centroid in centroids: + dist = compute_l2_distance(x, centroid) + centroid_distances.append(dist) + + # Get the index of the centroid with the smallest distance to the data point + closest_centroid_index = min( + range(len(centroid_distances)), + key=lambda x: centroid_distances[x], + ) + + return closest_centroid_index + + def compute_sse(data, centroids, assigned_centroids): + # Initialise SSE + sse = 0 + + # Compute the squared distance for each data point and add. + for i, x in enumerate(data): + # Get the associated centroid for data point + centroid = centroids[assigned_centroids[i]] + + # Compute the Distance to the centroid + dist = compute_l2_distance(x, centroid) + + # Add to the total distance + sse += dist + + sse /= len(data) + return sse + + # Number of dimensions in centroid + num_centroid_dims = data.shape[1] + + # List to store SSE for each iteration + sse_list = [] + + # Loop over iterations + for n in range(10): + + # Loop over each data point + for i in range(len(data)): + x = data[i] + + # Get the closest centroid + closest_centroid = get_closest_centroid(x, centroids) + + # Assign the centroid to the data point. + assigned_centroids[i] = closest_centroid + + # Loop over centroids and compute the new ones. + for c in range(len(centroids)): + # Get all the data points belonging to a particular cluster + cluster_data = [ + data[i] for i in range(len(data)) if assigned_centroids[i] == c + ] + + # Initialise the list to hold the new centroid + new_centroid = [0] * len(centroids[0]) + + # Compute the average of cluster members to compute new centroid + # Loop over dimensions of data + for dim in range(num_centroid_dims): + dim_sum = [x[dim] for x in cluster_data] + dim_sum = sum(dim_sum) / len(dim_sum) + new_centroid[dim] = dim_sum + + # assign the new centroid + centroids[c] = new_centroid + + # Compute the SSE for the iteration + sse = compute_sse(data, centroids, assigned_centroids) + sse_list.append(sse) + + cluster_members = [] + + for c in range(len(centroids)): + cluster_member = [ + data[i] for i in range(len(data)) if assigned_centroids[i] == c + ] + cluster_members.append(np.array(cluster_member)) + + means = [] + + for c in range(len(cluster_members)): + means.append(np.mean(cluster_members[c], axis=0)) + + print(means) + return np.asarray(means), cluster_members + + def crop_image(self, pts, transform, img): + # rospy.logerr("The points for cropping before are", pts) + pts = [self.model.project3dToPixel(transform.transform_point(a)) for a in pts] + pts = np.array([[int(a[0]), int(a[1])] for a in pts], dtype=np.int32) + pts = np.int32([pts]) + rospy.logerr("The points for cropping are", pts) + mask = np.zeros((img.shape[0], img.shape[1]), dtype=np.uint8) + cv2.fillPoly(mask, pts, (255)) + res = cv2.bitwise_and(img, img, mask=mask) + rect = cv2.boundingRect(pts) + cropped = res[rect[1] : rect[1] + rect[3], rect[0] : rect[0] + rect[2]] + return cropped + + def get_cluster_corners(self, cluster): + avg_x = np.mean(cluster[:, 0]) + min_y = np.amin(cluster[:, 1]) + max_y = np.amax(cluster[:, 1]) + min_z = np.amin(cluster[:, 2]) + max_z = np.amax(cluster[:, 2]) + return np.asarray( + [ + [avg_x, min_y, min_z], + [avg_x, min_y, max_z], + [avg_x, max_y, max_z], + [avg_x, max_y, min_z], + ], + ) + + async def crop_images(self, clusters, centers): + image = await self.image_sub.get_next_message() + image = self.bridge.imgmsg_to_cv2(image) + # cv2.imshow("Initial image", image) Not needed + boat_to_cam = await self.tf_listener.get_transform( + self.cam_frame, + "wamv/base_link", + ) + rospy.logerr("Cluster 0: ", clusters[0]) + rospy.logerr("Cluster 1: ", clusters[1]) + rospy.logerr("Cluster 2: ", clusters[2]) + + left = self.crop_image( + self.get_cluster_corners(clusters[0]), + boat_to_cam, + image, + ) + middle = self.crop_image( + self.get_cluster_corners(clusters[1]), + boat_to_cam, + image, + ) + right = self.crop_image( + self.get_cluster_corners(clusters[2]), + boat_to_cam, + image, + ) + list = [left, middle, right] + + rospy.logerr(f"Left image shape: {left.shape}") + rospy.logerr(f"Middle image shape: {middle.shape}") + rospy.logerr(f"Right image shape: {right.shape}") + + # Skip resize, its broken + # resized = [] + # for im in list: + # if im.size > 0: # Occasionally size is 0, causes errors + # resized = [ + # cv2.resize( + # im, + # (int(im.shape[1] * h_min / im.shape[0]), h_min), + # interpolation=cv2.INTER_CUBIC, + # ) + # ] + # else: + # print(f"Error: Image {counter} has size 0") + # counter += 1 + # concat = cv2.hconcat(resized) + # msg = self.bridge.cv2_to_imgmsg(concat, encoding="rgb8") + # self.contour_pub.publish(msg) + + return list + + # I don't think this function is needed anymore because anthony put his CV box detection code in main + async def shoot_projectile(self): + # Gets the image from the boat camera at this point in time + img = await self.image_sub.get_next_message() + img = self.bridge.imgmsg_to_cv2(img) + + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + # Apply Gaussian blur to the image + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + + # Use Canny edge detection + edges = cv2.Canny(blurred, 50, 150) + + # Find contours in the edged image + contours, _ = cv2.findContours(edges, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) + + # Count to keep track of number of squares + square_count = 0 + + cv2.imshow("Squares Detected", img) + cv2.waitKey(0) + cv2.destroyAllWindows() + + # Loop over the contours + for contour in contours: + + # Approximate the contour to a polygon + epsilon = 0.02 * cv2.arcLength(contour, True) + approx = cv2.approxPolyDP(contour, epsilon, True) + + # If the approximated contour has 4 vertices, it's a square (or rectangle) + if len(approx) == 4: + x, y, w, h = cv2.boundingRect(approx) + + # Calculate the aspect ratio + aspect_ratio = float(w) / h + + # Check if the aspect ratio is close to 1 (square) + if 0.95 <= aspect_ratio <= 1.05: + rospy.logerr("- SHOOT PROJ IS A SQUARE -") + # cv2.drawContours(img, [approx], -1, (0, 255, 0), 2) + square_count += 1 + # Display the result + cv2.imshow("Squares Detected", img) + cv2.waitKey(0) + cv2.destroyAllWindows() + + # If three squares are not found (color and two holes) then bad + if square_count != 3: + rospy.logerr("Error: Incorrect number of squares detected") return - self.send_feedback("Found dock, looking for image") - - # Find the camera input - center_frame = await self.get_center_frame() - symbol = center_frame[2].lower() - - self.send_feedback(f"Identified {symbol}") - - # Find the target point - target_pt = await self.get_target_pt(center_frame) - - self.send_feedback("Identified target") - - # Identify the time to wait in the dock - if wait_time == -1: - if "triangle" in symbol: - wait_time = 7 - elif "circle" in symbol: - wait_time = 17 - else: # Cruciform - wait_time = 27 - - # Go to pose - self.send_feedback("Moving into dock") - await self.move.set_position(target_pt).look_at(dock_position).go(blind=True) - - # Sleep the appropriate amount of time - self.send_feedback("------------------------------------------------") - self.send_feedback("!!!!!!!!!!!!! STATION KEEPING !!!!!!!!!!!!!!!!!!") - await self.nh.sleep(wait_time) - self.send_feedback("!!!!!!!!!!!!!!! EXITING DOCK !!!!!!!!!!!!!!!!!!!") - self.send_feedback("------------------------------------------------") - - # Back out of the dock - await self.move.backward(5).go(blind=True) - await self.move.backward(5).go(blind=True) - - self.send_feedback("Done with docking!") - - async def get_center_frame(self): - msgf = await self.bboxsub.get_next_message() - msg = msgf.objects[0] - # print msg - c1 = rosmsg_to_numpy(msg.points[0]) - c2 = rosmsg_to_numpy(msg.points[1]) - tmp = (((c1 + c2) / 2.0), msgf, msg.name) - return tmp - - async def get_target_pt(self, center_frame): - msg = CameraToLidarTransformRequest() - msg.header.stamp = center_frame[1].header.stamp - msg.header.frame_id = center_frame[1].header.frame_id - msg.point = Point(x=center_frame[0][0], y=center_frame[0][1], z=0.0) - msg.tolerance = 500 - - pose_offset = await self.camera_lidar_tf(msg) - - cam_to_enu = await self.tf_listener.get_transform( - "enu", - center_frame[1].header.frame_id, + rospy.logerr("- SHOOT PROJ WINDOWS DESTROYED -") + + # ADD SHOOTING MECHANICS HERE + + def find_color(self, images, goal_color): + # NOTE: An OpenCV window will open, close it to progress + # Current iteration of find color works by looking + # at a vertical line at the center of the image and averaging + # non-gray values, returning Red Green Blue or Other. + + count = 0 + + # set a tolerance value for if we find two colors in the same image + color_tolerance = 0.1 + + for img in images: + # Check if the image is empty before processing + if img is None or img.size == 0: + rospy.logerr("Error: Find color received image with 0 size, skipping") + continue + + # Convert from BGR to RGB + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + + # Now we find the color of the dock using the vertical centerline + # Iterate over the vertical centerline from top to bottom + cropped_img = img + center_x = cropped_img.shape[1] // 2 + total_red = 0 + total_green = 0 + total_blue = 0 + num_pixels = 0 # Number of nongray pixels at center + + # Also get the number of pixels for each color + green_pixels = 0 + red_pixels = 0 + blue_pixels = 0 + + for center_y in range(cropped_img.shape[0]): + current_color = cropped_img[ + center_y, + center_x, + ] # Get RGB value at center + current_color = current_color.astype( + np.int32, + ) # Turns int8 colors into int32 to prevent overflow + # Check if the color is weighted towards R G or B + if ( + current_color[2] > current_color[0] + current_color[1] + or current_color[1] > current_color[0] + current_color[2] + or current_color[0] > current_color[1] + current_color[2] + ): + # If the color is not gray, add it to the total color + total_red += current_color[0] + total_green += current_color[1] + total_blue += current_color[2] + num_pixels += 1 + + # Count the number of pixels that are weighted towards R G or B + if current_color[0] > current_color[1] + current_color[2]: + red_pixels += 1 + elif current_color[1] > current_color[0] + current_color[2]: + green_pixels += 1 + elif current_color[2] > current_color[0] + current_color[1]: + blue_pixels += 1 + + # Color at center is average of non gray pixels + if num_pixels == 0: # Check if any colors found + dock_color = (0, 0, 0) + else: + dock_color = ( + total_red / num_pixels, + total_green / num_pixels, + total_blue / num_pixels, + ) + + # Get the ratios of each color + red_ratio = red_pixels / num_pixels if num_pixels > 0 else 0 + green_ratio = green_pixels / num_pixels if num_pixels > 0 else 0 + blue_ratio = blue_pixels / num_pixels if num_pixels > 0 else 0 + + # Now check if we find two colors outside of the tolerance by checking if there is an outright majority of one color + if not ( + ( + red_ratio - green_ratio > color_tolerance + and red_ratio - blue_ratio > color_tolerance + ) + or ( + green_ratio - blue_ratio > color_tolerance + and green_ratio - red_ratio > color_tolerance + ) + or ( + blue_ratio - green_ratio > color_tolerance + and blue_ratio - red_ratio > color_tolerance + ) + ): + rospy.logerr( + f"Error: Found two colors in image {count} with ratios: R: {red_ratio}, G: {green_ratio}, B: {blue_ratio}", + ) + # We return -1 signaling that this failed + return -1 + + # Max ratio allowed between main color and other 2 values + color_ratio = 0.9 + # Log the color (even after converting to RGB they still need to be BGR for this somehow) + if ( + dock_color[0] > color_ratio * (dock_color[1] + dock_color[2]) + and goal_color == "Blue" + ): + # cv2.imshow('Blue Dock Detected', cropped_img) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + rospy.logerr("Detected color: Blue") + return count + elif ( + dock_color[1] > color_ratio * (dock_color[0] + dock_color[2]) + and goal_color == "Green" + ): + rospy.logerr("Detected color: Green") + return count + elif ( + dock_color[2] > color_ratio * (dock_color[0] + dock_color[1]) + and goal_color == "Red" + ): + # cv2.imshow('Red Dock Detected', cropped_img) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + rospy.logerr("Detected color: Red") + return count + else: + rospy.logerr(f"Detected color: RGB{dock_color}") + + # Publish the image + msg = self.bridge.cv2_to_imgmsg(img, encoding="rgb8") + self.contour_pub.publish(msg) + + # Press key to destroy OpenCV window + # cv2.waitKey(0) + # cv2.destroyAllWindows() + + count += 1 + + return -1 + + def get_ogrid_coords(self, arr): + return self.intup(self.ogrid_cpm * (np.asarray(arr) - self.ogrid_origin))[:2] + + def bbox_to_point(self, scale, rot, pos, xmul, ymul): + return self.intup( + self.ogrid_cpm + * ( + self.get_point( + [(xmul * scale[0]) / 2, (ymul * scale[1]) / 2, scale[2]], + rot, + pos, + ) + - self.ogrid_origin + )[:2], ) - normal = rosmsg_to_numpy(pose_offset.normal) - normal = cam_to_enu.transform_vector(normal) - normal = normal[0:2] / np.linalg.norm(normal[0:2]) - normal = np.append(normal, [0]) - found_pt = rosmsg_to_numpy(pose_offset.closest) - found_pt = cam_to_enu.transform_point(found_pt) - found_pt[2] = 0 - - # Extend out by normal multiplier - normal *= 3 - found_pt_1 = found_pt + normal - found_pt_2 = found_pt + -1 * normal - - # Which is closer - boat_pos = (self.tx_pose)[0] - if np.linalg.norm(found_pt_1 - boat_pos) > np.linalg.norm( - found_pt_2 - boat_pos, - ): - found_pt = found_pt_2 + + # returns True if side a is closest, False is side b is closest + # This function works by drawing lines on the OGrid image. The OGrid is a 2D top down + # image that takes the LIDAR points and colors a pixel in this grid is a point occupies it. + # Using the bounding box of the LIDAR object, we create a bounding box around the 2D image of the dock. + # Then, the center is calculated and lines are drawn to the middle point of each side of the bounding box. + # The number of occupies squares are counted on the line from the object center to line middle. The line + # which has the least amount of occupied space points to the open side of the dock. + def calculate_correct_side( + self, + side_a: [float], + side_b: [float], + side_c: [float], + side_d: [float], + position: [float], + rotation: [[float]], + scale: [float], + ) -> [float]: + print("Finding ogrid center of mass") + + point1 = self.bbox_to_point(scale, rotation, position, -1, 1) + point2 = self.bbox_to_point(scale, rotation, position, 1, 1) + point3 = self.bbox_to_point(scale, rotation, position, 1, -1) + point4 = self.bbox_to_point(scale, rotation, position, -1, -1) + contours = np.array([point1, point2, point3, point4]) + + center = self.calculate_center_of_mass(contours) + mask = np.zeros(self.last_image.shape, np.uint8) + + bounding_rect = cv2.boundingRect(contours) + x, y, w, h = bounding_rect + + image_or = np.zeros(self.last_image.shape, np.uint8) + + cv2.line(mask, self.get_ogrid_coords(side_a), center, (255, 255, 255), 2) + side_a_and = cv2.bitwise_and(copy.deepcopy(self.last_image), mask) + image_or = cv2.bitwise_or(image_or, side_a_and) + mask = np.zeros(self.last_image.shape, np.uint8) + + cv2.line(mask, self.get_ogrid_coords(side_b), center, (255, 255, 255), 2) + side_b_and = cv2.bitwise_and(copy.deepcopy(self.last_image), mask) + image_or = cv2.bitwise_or(image_or, side_b_and) + mask = np.zeros(self.last_image.shape, np.uint8) + + cv2.line(mask, self.get_ogrid_coords(side_c), center, (255, 255, 255), 2) + side_c_and = cv2.bitwise_and(copy.deepcopy(self.last_image), mask) + image_or = cv2.bitwise_or(image_or, side_c_and) + mask = np.zeros(self.last_image.shape, np.uint8) + + cv2.line(mask, self.get_ogrid_coords(side_d), center, (255, 255, 255), 2) + side_d_and = cv2.bitwise_and(copy.deepcopy(self.last_image), mask) + image_or = cv2.bitwise_or(image_or, side_d_and) + + msg = self.bridge.cv2_to_imgmsg(image_or, encoding="rgb8") + self.contour_pub.publish(msg) + + side_a_count = np.count_nonzero(side_a_and == np.asarray([255, 255, 255])) + print(side_a_count) + side_b_count = np.count_nonzero(side_b_and == np.asarray([255, 255, 255])) + print(side_b_count) + side_c_count = np.count_nonzero(side_c_and == np.asarray([255, 255, 255])) + print(side_c_count) + side_d_count = np.count_nonzero(side_d_and == np.asarray([255, 255, 255])) + print(side_d_count) + + lowest = min([side_a_count, side_b_count, side_c_count, side_d_count]) + print("Center of mass found") + if lowest == side_a_count: + return side_a + elif lowest == side_b_count: + return side_b + elif lowest == side_c_count: + return side_c else: - found_pt = found_pt_1 + return side_d + + def ogrid_to_position(self, ogrid): + ogrid = np.array(ogrid, dtype=np.float64) + ogrid = ogrid / self.ogrid_cpm + ogrid = ogrid + self.ogrid_origin[:2] + return tuple(ogrid) + + def calculate_center_of_mass(self, points): + bounding_rect = cv2.boundingRect(points) + x, y, w, h = bounding_rect + mask = np.zeros(self.last_image.shape, np.uint8) + cv2.drawContours(mask, [points], -1, (255, 255, 255), -1, cv2.LINE_AA) + masked = cv2.bitwise_and(self.last_image, mask) + count = 0 + x_sum = 0 + y_sum = 0 + for i in range(x, x + w): + for j in range(y, y + h): + if (masked[j, i] == [255, 255, 255]).all(): + x_sum = x_sum + i + y_sum = y_sum + j + count = count + 1 + x_sum = int(x_sum / count) + y_sum = int(y_sum / count) + return (x_sum, y_sum) + + def get_point(self, corner, rotation_matrix, center): + return np.matmul(rotation_matrix, np.asarray(corner)) + np.asarray(center) + + async def find_dock_poi(self, hint: Optional[Pose] = None): + print("Finding poi") + + # This function is used to find the position of the dock after the boat is near a POI + async def find_dock(self): + print("Searching for dock") + msg = None + while msg is None: + try: + # gets the largest object in the clusters database + msg = await self.get_largest_object() + except Exception: + await self.move.forward(10).go() + # label the largest LIDAR object in the PCODAR database "dock" + await self.pcodar_label(msg.id, "dock") + # if no pcodar objects, throw error, exit mission + pose = pose_to_numpy(msg.pose) + + # return position of dock + return pose + + def ogrid_cb(self, msg): + self.ogrid = np.array(msg.data).reshape((msg.info.height, msg.info.width)) + self.ogrid_origin = rosmsg_to_numpy(msg.info.origin.position) + self.ogrid_cpm = 1 / msg.info.resolution + + image = 255 * np.greater(self.ogrid, 90).astype(np.uint8) + grayImage = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) - return found_pt + self.last_image = grayImage + return diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py index 36c78fcbc..5ceab9f60 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py @@ -29,7 +29,9 @@ from ros_alarms import TxAlarmListener from sensor_msgs.msg import CameraInfo, Image from std_msgs.msg import Bool +from std_srvs.srv import Empty as Emptysrv from std_srvs.srv import ( + EmptyRequest, SetBool, SetBoolRequest, SetBoolResponse, @@ -211,6 +213,22 @@ def enu_odom_set(odom): cls._grind_motor_pub.setup(), ) + try: + cls._ball_spin_srv = cls.nh.get_service_client( + "/ball_launcher/spin", + SetBool, + ) + cls._ball_launch_srv = cls.nh.get_service_client( + "/ball_launcher/drop_ball", + Emptysrv, + ) + except AttributeError as err: + fprint( + f"Error getting ball launcher service clients: {err}", + title="NAVIGATOR", + msg_color="red", + ) + try: cls._actuator_client = cls.nh.get_service_client( "/actuator_driver/actuate", @@ -534,11 +552,17 @@ async def reload_launcher(self): await self.set_valve("LAUNCHER_RELOAD_RETRACT", False) self.launcher_state = "inactive" + async def start_launcher(self): + await self._ball_spin_srv(SetBoolRequest(data=True)) + + async def stop_launcher(self): + await self._ball_spin_srv(SetBoolRequest(data=False)) + async def fire_launcher(self): if self.launcher_state != "inactive": raise Exception(f"Launcher is {self.launcher_state}") self.launcher_state = "firing" - await self.set_valve("LAUNCHER_FIRE", True) + await self._ball_launch_srv(EmptyRequest()) await self.nh.sleep(0.5) self.launcher_state = "inactive" @@ -546,6 +570,11 @@ def set_valve(self, name, state): req = SetValveRequest(actuator=name, opened=state) return self._actuator_client(req) + async def get_largest_object(self, name: str = "UNKNOWN", **kwargs): + objects = (await self.database_query(object_name=name, **kwargs)).objects + largest_object = max(objects, key=lambda x: x.scale.x * x.scale.y) + return largest_object + async def get_sorted_objects( self, name: str, diff --git a/NaviGator/navigator.rviz b/NaviGator/navigator.rviz index 401204deb..bd2713369 100644 --- a/NaviGator/navigator.rviz +++ b/NaviGator/navigator.rviz @@ -26,7 +26,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: Lidar @@ -61,61 +60,71 @@ Visualization Manager: Enabled: true Links: All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - velodyne_link: + BL_engine_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - velodyne_post_arm_link: + BL_propeller_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - velodyne_post_link: + BR_engine_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/BL_engine_link: + BR_propeller_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/BL_propeller_link: + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_engine_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/BR_engine_link: + FL_propeller_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/BR_propeller_link: + FR_engine_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/FL_engine_link: + FR_propeller_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/FL_propeller_link: + Link Tree Style: Links in Alphabetic Order + velodyne: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/FR_engine_link: + velodyne_post_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/FR_propeller_link: + velodyne_post_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wamv/ball_shooter_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wamv/ball_shooter_launcher_link: Alpha: 1 Show Axes: false Show Trail: false @@ -167,24 +176,21 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - wamv/gps_wamv_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true wamv/hydrophones: Alpha: 1 Show Axes: false Show Trail: false + Value: true wamv/imu_wamv_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - wamv/measurement: + wamv/ins_link: Alpha: 1 Show Axes: false Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -208,34 +214,40 @@ Visualization Manager: Value: false - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false - enu: + BL_engine_link: Value: true - utm: + BL_propeller_link: Value: true - velodyne_link: + BR_engine_link: Value: true - velodyne_post_arm_link: + BR_propeller_link: Value: true - velodyne_post_link: + FL_engine_link: + Value: true + FL_propeller_link: Value: true - wamv/BL_engine_link: + FR_engine_link: Value: true - wamv/BL_propeller_link: + FR_propeller_link: Value: true - wamv/BR_engine_link: + enu: + Value: true + utm: Value: true - wamv/BR_propeller_link: + velodyne: Value: true - wamv/FL_engine_link: + velodyne_post_arm_link: Value: true - wamv/FL_propeller_link: + velodyne_post_link: Value: true - wamv/FR_engine_link: + wamv/ball_shooter_base_link: Value: true - wamv/FR_propeller_link: + wamv/ball_shooter_launcher_link: Value: true wamv/base_link: Value: true @@ -257,13 +269,11 @@ Visualization Manager: Value: true wamv/front_right_cam_post_link: Value: true - wamv/gps_wamv_link: - Value: true wamv/hydrophones: Value: true wamv/imu_wamv_link: Value: true - wamv/measurement: + wamv/ins_link: Value: true Marker Alpha: 1 Marker Scale: 1 @@ -276,21 +286,24 @@ Visualization Manager: utm: {} wamv/base_link: - velodyne_post_link: - velodyne_post_arm_link: - velodyne_link: - {} - wamv/BL_engine_link: - wamv/BL_propeller_link: + BL_engine_link: + BL_propeller_link: {} - wamv/BR_engine_link: - wamv/BR_propeller_link: + BR_engine_link: + BR_propeller_link: {} - wamv/FL_engine_link: - wamv/FL_propeller_link: + FL_engine_link: + FL_propeller_link: {} - wamv/FR_engine_link: - wamv/FR_propeller_link: + FR_engine_link: + FR_propeller_link: + {} + velodyne_post_link: + velodyne_post_arm_link: + velodyne: + {} + wamv/ball_shooter_base_link: + wamv/ball_shooter_launcher_link: {} wamv/dummy_link: {} @@ -304,13 +317,11 @@ Visualization Manager: wamv/front_right_cam_link: wamv/front_right_cam_link_optical: {} - wamv/gps_wamv_link: - {} wamv/hydrophones: {} wamv/imu_wamv_link: {} - wamv/measurement: + wamv/ins_link: {} Update Interval: 0 Value: true @@ -529,9 +540,9 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -8.08499813079834 - Y: -3.4566752910614014 - Z: 11.338399887084961 + X: -11.992816925048828 + Y: 2.1578445434570312 + Z: 17.758596420288086 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -549,7 +560,7 @@ Window Geometry: Height: 948 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001fe0000031afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000209000001b20000000000000000fb0000002200460072006f006e00740020004c006500660074002000430061006d00650072006101000001ec000001690000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000057c0000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001fe0000031afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000209000001b20000000000000000fb0000002200460072006f006e00740020004c006500660074002000430061006d00650072006101000001ec000001690000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000057c0000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -559,5 +570,5 @@ Window Geometry: Views: collapsed: true Width: 1920 - X: 0 + X: 72 Y: 27 diff --git a/NaviGator/perception/navigator_vision/navigator_vision/__init__.py b/NaviGator/perception/navigator_vision/navigator_vision/__init__.py index 7496dabf3..4e70a368d 100644 --- a/NaviGator/perception/navigator_vision/navigator_vision/__init__.py +++ b/NaviGator/perception/navigator_vision/navigator_vision/__init__.py @@ -1,3 +1,4 @@ from .scan_the_code_classifier import ScanTheCodeClassifier from .vrx_color_classifier import VrxColorClassifier from .vrx_stc_color_classifier import VrxStcColorClassifier +from .dockdeliver_pipeline import GripPipeline diff --git a/NaviGator/perception/navigator_vision/navigator_vision/dock1-green.png b/NaviGator/perception/navigator_vision/navigator_vision/dock1-green.png new file mode 100644 index 000000000..d76d9be73 Binary files /dev/null and b/NaviGator/perception/navigator_vision/navigator_vision/dock1-green.png differ diff --git a/NaviGator/perception/navigator_vision/navigator_vision/dock1-red.png b/NaviGator/perception/navigator_vision/navigator_vision/dock1-red.png new file mode 100644 index 000000000..e39596498 Binary files /dev/null and b/NaviGator/perception/navigator_vision/navigator_vision/dock1-red.png differ diff --git a/NaviGator/perception/navigator_vision/navigator_vision/dock2-blue.png b/NaviGator/perception/navigator_vision/navigator_vision/dock2-blue.png new file mode 100644 index 000000000..ced3c858d Binary files /dev/null and b/NaviGator/perception/navigator_vision/navigator_vision/dock2-blue.png differ diff --git a/NaviGator/perception/navigator_vision/navigator_vision/dockdeliver_pipeline.py b/NaviGator/perception/navigator_vision/navigator_vision/dockdeliver_pipeline.py new file mode 100644 index 000000000..23037b187 --- /dev/null +++ b/NaviGator/perception/navigator_vision/navigator_vision/dockdeliver_pipeline.py @@ -0,0 +1,168 @@ +import cv2 +import numpy +import math +from enum import Enum + +class GripPipeline: + """ + An OpenCV pipeline generated by GRIP. + """ + + def __init__(self): + """initializes all values to presets or None if need to be set + """ + + self.__blur_type = BlurType.Box_Blur + self.__blur_radius = 6 + self.blur_output = None + + self.__hsv_threshold_input = self.blur_output + self.__hsv_threshold_hue = [0.0, 180.0] + self.__hsv_threshold_saturation = [0.0, 160.55555555555554] + self.__hsv_threshold_value = [64.20863309352518, 255.0] + + self.hsv_threshold_output = None + + self.__find_contours_input = self.hsv_threshold_output + self.__find_contours_external_only = False + + self.find_contours_output = None + + self.__filter_contours_contours = self.find_contours_output + self.__filter_contours_min_area = 1000.0 + self.__filter_contours_min_perimeter = 0.0 + self.__filter_contours_min_width = 0.0 + self.__filter_contours_max_width = 1000.0 + self.__filter_contours_min_height = 0.0 + self.__filter_contours_max_height = 1000.0 + self.__filter_contours_solidity = [90.8273381294964, 100.0] + self.__filter_contours_max_vertices = 50.0 + self.__filter_contours_min_vertices = 0.0 + self.__filter_contours_min_ratio = 0.0 + self.__filter_contours_max_ratio = 2.0 + + self.filter_contours_output = None + + + def process(self, source0): + """ + Runs the pipeline and sets all outputs to new values. + """ + # Step Blur0: + self.__blur_input = source0 + (self.blur_output) = self.__blur(self.__blur_input, self.__blur_type, self.__blur_radius) + + # Step HSV_Threshold0: + self.__hsv_threshold_input = self.blur_output + (self.hsv_threshold_output) = self.__hsv_threshold(self.__hsv_threshold_input, self.__hsv_threshold_hue, self.__hsv_threshold_saturation, self.__hsv_threshold_value) + + # Step Find_Contours0: + self.__find_contours_input = self.hsv_threshold_output + (self.find_contours_output) = self.__find_contours(self.__find_contours_input, self.__find_contours_external_only) + + # Step Filter_Contours0: + self.__filter_contours_contours = self.find_contours_output + (self.filter_contours_output) = self.__filter_contours(self.__filter_contours_contours, self.__filter_contours_min_area, self.__filter_contours_min_perimeter, self.__filter_contours_min_width, self.__filter_contours_max_width, self.__filter_contours_min_height, self.__filter_contours_max_height, self.__filter_contours_solidity, self.__filter_contours_max_vertices, self.__filter_contours_min_vertices, self.__filter_contours_min_ratio, self.__filter_contours_max_ratio) + + + @staticmethod + def __blur(src, type, radius): + """Softens an image using one of several filters. + Args: + src: The source mat (numpy.ndarray). + type: The blurType to perform represented as an int. + radius: The radius for the blur as a float. + Returns: + A numpy.ndarray that has been blurred. + """ + if(type is BlurType.Box_Blur): + ksize = int(2 * round(radius) + 1) + return cv2.blur(src, (ksize, ksize)) + elif(type is BlurType.Gaussian_Blur): + ksize = int(6 * round(radius) + 1) + return cv2.GaussianBlur(src, (ksize, ksize), round(radius)) + elif(type is BlurType.Median_Filter): + ksize = int(2 * round(radius) + 1) + return cv2.medianBlur(src, ksize) + else: + return cv2.bilateralFilter(src, -1, round(radius), round(radius)) + + @staticmethod + def __hsv_threshold(input, hue, sat, val): + """Segment an image based on hue, saturation, and value ranges. + Args: + input: A BGR numpy.ndarray. + hue: A list of two numbers the are the min and max hue. + sat: A list of two numbers the are the min and max saturation. + lum: A list of two numbers the are the min and max value. + Returns: + A black and white numpy.ndarray. + """ + out = cv2.cvtColor(input, cv2.COLOR_BGR2HSV) + return cv2.inRange(out, (hue[0], sat[0], val[0]), (hue[1], sat[1], val[1])) + + @staticmethod + def __find_contours(input, external_only): + """Sets the values of pixels in a binary image to their distance to the nearest black pixel. + Args: + input: A numpy.ndarray. + external_only: A boolean. If true only external contours are found. + Return: + A list of numpy.ndarray where each one represents a contour. + """ + if(external_only): + mode = cv2.RETR_EXTERNAL + else: + mode = cv2.RETR_LIST + method = cv2.CHAIN_APPROX_SIMPLE + contours, hierarchy =cv2.findContours(input, mode=mode, method=method) + return contours + + @staticmethod + def __filter_contours(input_contours, min_area, min_perimeter, min_width, max_width, + min_height, max_height, solidity, max_vertex_count, min_vertex_count, + min_ratio, max_ratio): + """Filters out contours that do not meet certain criteria. + Args: + input_contours: Contours as a list of numpy.ndarray. + min_area: The minimum area of a contour that will be kept. + min_perimeter: The minimum perimeter of a contour that will be kept. + min_width: Minimum width of a contour. + max_width: MaxWidth maximum width. + min_height: Minimum height. + max_height: Maximimum height. + solidity: The minimum and maximum solidity of a contour. + min_vertex_count: Minimum vertex Count of the contours. + max_vertex_count: Maximum vertex Count. + min_ratio: Minimum ratio of width to height. + max_ratio: Maximum ratio of width to height. + Returns: + Contours as a list of numpy.ndarray. + """ + output = [] + for contour in input_contours: + x,y,w,h = cv2.boundingRect(contour) + if (w < min_width or w > max_width): + continue + if (h < min_height or h > max_height): + continue + area = cv2.contourArea(contour) + if (area < min_area): + continue + if (cv2.arcLength(contour, True) < min_perimeter): + continue + hull = cv2.convexHull(contour) + solid = 100 * area / cv2.contourArea(hull) + if (solid < solidity[0] or solid > solidity[1]): + continue + if (len(contour) < min_vertex_count or len(contour) > max_vertex_count): + continue + ratio = (float)(w) / h + if (ratio < min_ratio or ratio > max_ratio): + continue + output.append(contour) + return output + + +BlurType = Enum('BlurType', 'Box_Blur Gaussian_Blur Median_Filter Bilateral_Filter') + diff --git a/NaviGator/perception/navigator_vision/navigator_vision/test_pipeline.py b/NaviGator/perception/navigator_vision/navigator_vision/test_pipeline.py new file mode 100644 index 000000000..2dc256b24 --- /dev/null +++ b/NaviGator/perception/navigator_vision/navigator_vision/test_pipeline.py @@ -0,0 +1,33 @@ +from dockdeliver_pipeline import GripPipeline +import cv2 as cv +import numpy as np +import math + + +img_path = "dock2-blue.png" +image = cv.imread(img_path) + +pipeline = GripPipeline() + +pipeline.process(image) +img = pipeline.hsv_threshold_output +img2 = pipeline.blur_output +contours = pipeline.filter_contours_output + + +#print_image_values(img) +print(contours[0]) +centroid = np.mean(contours[0], axis=0) +print(centroid[0]) +print((centroid[0][0],centroid[0][1])) + +cv.imshow("blur", img2) +cv.imshow("threshold", img) +img3 = cv.drawContours(img2, contours, -1, (0,255,0), 3) +img3 = cv.circle(img3, ((math.ceil(centroid[0][0]),math.ceil(centroid[0][1]))), radius=2, color=(0, 0, 255), thickness=1) +cv.imshow("contours", img3) + + + +cv.waitKey(0) +cv.destroyAllWindows() \ No newline at end of file diff --git a/NaviGator/perception/navigator_vision/nodes/dock_color_detection/dock_color_detection.py b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/dock_color_detection.py new file mode 100644 index 000000000..dc0dec4bc --- /dev/null +++ b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/dock_color_detection.py @@ -0,0 +1,42 @@ +import cv2 as cv +import numpy as np + + +def find_center_pixel(image_path): + image = cv.imread(image_path) + + gray_image = cv.cvtColor(image, cv.COLOR_RGB2GRAY) + + median = cv.medianBlur(gray_image, 5) + + kernel = np.array([[0, -1, 0], [-1, 5, -1], [0, -1, 0]]) + + sharpened_image = cv.filter2D(median, -1, kernel) + + _, binary = cv.threshold(sharpened_image, 0, 255, cv.THRESH_BINARY + cv.THRESH_OTSU) + + kernel = cv.getStructuringElement(cv.MORPH_RECT, (5, 5)) + morphed = cv.morphologyEx(binary, cv.MORPH_CLOSE, kernel) + morphed = cv.morphologyEx(morphed, cv.MORPH_OPEN, kernel) + + cv.imshow("Morphed", morphed) + cv.waitKey(0) + cv.destroyAllWindows() + + +def red_blue_or_green(pixel): + red, green, blue = pixel + + max_color = max(red, green, blue) + + if max_color == red: + return "red" + elif max_color == green: + return "green" + elif max_color == blue: + return "blue" + else: + return "unknown" + + +find_center_pixel("./test_images/dock_red1.jpg") diff --git a/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_blue1.jpg b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_blue1.jpg new file mode 100644 index 000000000..bd824eb54 Binary files /dev/null and b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_blue1.jpg differ diff --git a/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_red1.jpg b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_red1.jpg new file mode 100644 index 000000000..0088abec2 Binary files /dev/null and b/NaviGator/perception/navigator_vision/nodes/dock_color_detection/test_images/dock_red1.jpg differ diff --git a/NaviGator/perception/navigator_vision/nodes/dockdeliver_pipeline.py b/NaviGator/perception/navigator_vision/nodes/dockdeliver_pipeline.py new file mode 100644 index 000000000..97dccb6b1 --- /dev/null +++ b/NaviGator/perception/navigator_vision/nodes/dockdeliver_pipeline.py @@ -0,0 +1,105 @@ +import cv2 +import numpy +import math +from enum import Enum + +class GripPipeline: + """ + An OpenCV pipeline generated by GRIP. + """ + + def __init__(self): + """initializes all values to presets or None if need to be set + """ + + self.__blur_type = BlurType.Box_Blur + self.__blur_radius = 26.126126126126124 + + self.blur_output = None + + self.__rgb_threshold_input = self.blur_output + self.__rgb_threshold_red = [0.0, 109.04040404040403] + self.__rgb_threshold_green = [64.20863309352518, 148.10606060606062] + self.__rgb_threshold_blue = [119.24460431654676, 255.0] + + self.rgb_threshold_output = None + + self.__find_contours_input = self.rgb_threshold_output + self.__find_contours_external_only = False + + self.find_contours_output = None + + + def process(self, source0): + """ + Runs the pipeline and sets all outputs to new values. + """ + # Step Blur0: + self.__blur_input = source0 + (self.blur_output) = self.__blur(self.__blur_input, self.__blur_type, self.__blur_radius) + + # Step RGB_Threshold0: + self.__rgb_threshold_input = self.blur_output + (self.rgb_threshold_output) = self.__rgb_threshold(self.__rgb_threshold_input, self.__rgb_threshold_red, self.__rgb_threshold_green, self.__rgb_threshold_blue) + + # Step Find_Contours0: + self.__find_contours_input = self.rgb_threshold_output + (self.find_contours_output) = self.__find_contours(self.__find_contours_input, self.__find_contours_external_only) + + + @staticmethod + def __blur(src, type, radius): + """Softens an image using one of several filters. + Args: + src: The source mat (numpy.ndarray). + type: The blurType to perform represented as an int. + radius: The radius for the blur as a float. + Returns: + A numpy.ndarray that has been blurred. + """ + if(type is BlurType.Box_Blur): + ksize = int(2 * round(radius) + 1) + return cv2.blur(src, (ksize, ksize)) + elif(type is BlurType.Gaussian_Blur): + ksize = int(6 * round(radius) + 1) + return cv2.GaussianBlur(src, (ksize, ksize), round(radius)) + elif(type is BlurType.Median_Filter): + ksize = int(2 * round(radius) + 1) + return cv2.medianBlur(src, ksize) + else: + return cv2.bilateralFilter(src, -1, round(radius), round(radius)) + + @staticmethod + def __rgb_threshold(input, red, green, blue): + """Segment an image based on color ranges. + Args: + input: A BGR numpy.ndarray. + red: A list of two numbers the are the min and max red. + green: A list of two numbers the are the min and max green. + blue: A list of two numbers the are the min and max blue. + Returns: + A black and white numpy.ndarray. + """ + out = cv2.cvtColor(input, cv2.COLOR_BGR2RGB) + return cv2.inRange(out, (red[0], green[0], blue[0]), (red[1], green[1], blue[1])) + + @staticmethod + def __find_contours(input, external_only): + """Sets the values of pixels in a binary image to their distance to the nearest black pixel. + Args: + input: A numpy.ndarray. + external_only: A boolean. If true only external contours are found. + Return: + A list of numpy.ndarray where each one represents a contour. + """ + if(external_only): + mode = cv2.RETR_EXTERNAL + else: + mode = cv2.RETR_LIST + method = cv2.CHAIN_APPROX_SIMPLE + im2, contours, hierarchy =cv2.findContours(input, mode=mode, method=method) + return contours + + +BlurType = Enum('BlurType', 'Box_Blur Gaussian_Blur Median_Filter Bilateral_Filter') + diff --git a/NaviGator/simulation/VRX/vrx b/NaviGator/simulation/VRX/vrx index e2dc3de4f..edecd95e3 160000 --- a/NaviGator/simulation/VRX/vrx +++ b/NaviGator/simulation/VRX/vrx @@ -1 +1 @@ -Subproject commit e2dc3de4f369a5c88747508da2c7f85b58ff0cbe +Subproject commit edecd95e38ff68e59d66f647448415222a4529ce diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config new file mode 100644 index 000000000..2bf440cd0 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config @@ -0,0 +1,20 @@ + + + robotx_dock_2024 + 1.0 + model.sdf + + + Max Vu + m.vu@ufl.edu + + + + Carlos Agüero + caguero@openrobotics.org + + + + The dock used in RobotX 2024. + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf new file mode 100644 index 000000000..0c354b8ed --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf @@ -0,0 +1,349 @@ + + + + + true + + + + model://dock_2024_base + + + + + + 5.75 3 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard1::link_symbols::visual_square + + true + vrx/dock_2024_placard1 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + + 5.75 9 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard2::link_symbols::visual_square + + true + vrx/dock_2024_placard2 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + + 5.75 15 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard3::link_symbols::visual_square + + + true + vrx/dock_2024_placard3 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb new file mode 100644 index 000000000..49b850bfe --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb @@ -0,0 +1,215 @@ + + + + + true + + + + model://dock_2024_base + + <% + # Constants + dock_x = 5.75 + dock_y = 3 + dock_z = 1.5 + dock_roll = 0 + dock_pitch = 0 + dock_yaw = Math::PI / 2.0 + circle_offset_x = -0.06 + circle_offset_y = 0 + circle_offset_z = 0.6 + circle_offset_roll = 0 + circle_offset_pitch = Math::PI / 2.0 + circle_offset_yaw = -Math::PI / 2.0 + cross_offset_x = -0.06 + cross_offset_y = 0 + cross_offset_z = 0.6 + cross_offset_roll = 0 + cross_offset_pitch = Math::PI / 2.0 + cross_offset_yaw = -Math::PI / 2.0 + triangle_offset_x = -0.06 + triangle_offset_y = -0.5 + triangle_offset_z = 0.2 + triangle_offset_roll = Math::PI / 2.0 + triangle_offset_pitch = 0 + triangle_offset_yaw = 0 + placard_offset_x = 0 + placard_offset_y = 6 + placard_offset_z = 0 + placard_offset_roll = 0 + placard_offset_pitch = 0 + placard_offset_yaw = 0 + + # Variables + placard_x = dock_x + placard_y = dock_y + placard_z = dock_z + placard_roll = dock_roll + placard_pitch = dock_pitch + placard_yaw = dock_yaw + + circle_x = dock_x + circle_offset_x + circle_y = dock_y + circle_offset_y + circle_z = dock_z + circle_offset_z + circle_roll = dock_roll + circle_offset_roll + circle_pitch = dock_pitch + circle_offset_pitch + circle_yaw = dock_yaw + circle_offset_yaw + + cross_x = dock_x + cross_offset_x + cross_y = dock_y + cross_offset_y + cross_z = dock_z + cross_offset_z + cross_roll = dock_roll + cross_offset_roll + cross_pitch = dock_pitch + cross_offset_pitch + cross_yaw = dock_yaw + cross_offset_yaw + + triangle_x = dock_x + triangle_offset_x + triangle_y = dock_y + triangle_offset_y + triangle_z = dock_z + triangle_offset_z + triangle_roll = dock_roll + triangle_offset_roll + triangle_pitch = dock_pitch + triangle_offset_pitch + triangle_yaw = dock_yaw + triangle_offset_yaw + %> + <% for i in 1..3 %> + + + <%=placard_x%> <%=placard_y%> <%=placard_z%> <%=placard_roll%> <%=placard_pitch%> <%=placard_yaw%> + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + + robotx_dock_2024::dock_2024_placard<%= i %>::link_symbols::visual_square + + true + vrx/dock_2024_placard<%= i %> + shuffle + + + + 0 0 0 1.571 0 0 + + + 1.1176 1.1176 0.001 + + + 1 + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + <% + placard_x += placard_offset_x + placard_y += placard_offset_y + placard_z += placard_offset_z + placard_roll += placard_offset_roll + placard_pitch += placard_offset_pitch + placard_yaw += placard_offset_yaw + + circle_x += placard_offset_x + circle_y += placard_offset_y + circle_z += placard_offset_z + circle_roll += placard_offset_roll + circle_pitch += placard_offset_pitch + circle_yaw += placard_offset_yaw + + cross_x += placard_offset_x + cross_y += placard_offset_y + cross_z += placard_offset_z + cross_roll += placard_offset_roll + cross_pitch += placard_offset_pitch + cross_yaw += placard_offset_yaw + + triangle_x += placard_offset_x + triangle_y += placard_offset_y + triangle_z += placard_offset_z + triangle_roll += placard_offset_roll + triangle_pitch += placard_offset_pitch + triangle_yaw += placard_offset_yaw + %> + <% end %> + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config new file mode 100644 index 000000000..d8d58d747 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config @@ -0,0 +1,20 @@ + + + robotx_dock_2024_base + 1.0 + model.sdf + + + Max Vu + m.vu@ufl.edu + + + + Carlos Agüero + caguero@openrobotics.org + + + + The dock base used in RobotX 2024. + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf new file mode 100644 index 000000000..bd7aa2e41 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf @@ -0,0 +1,142 @@ + + + + + + + false + + + + + true + + + + + dock_block_0_0 + 0 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_3 + 0 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_6 + 0 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_9 + 0 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_1_0 + 2 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_3 + 2 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_6 + 2 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_9 + 2 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_2_0 + 4 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_3 + 4 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_6 + 4 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_9 + 4 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_3_0 + 6 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_1 + 6 2 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_2 + 6 4 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_3 + 6 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_4 + 6 8 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_5 + 6 10 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_6 + 6 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_7 + 6 14 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_8 + 6 16 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_9 + 6 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + + + + diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png b/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png new file mode 100755 index 000000000..8664df705 Binary files /dev/null and b/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png differ diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae b/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae new file mode 100644 index 000000000..77861b7dc --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae @@ -0,0 +1,607 @@ + + + + + + FBX COLLADA exporter + + + 2021-08-31T22:23:17Z + + 2021-08-31T22:23:17Z + + + + <unit meter="0.010000" name="centimeter"/> + <up_axis>Z_UP</up_axis> + </asset> + <library_images> + <image id="MapFBXASC032FBXASC0351-image" name="MapFBXASC032FBXASC0351"> + <init_from>Placard.png</init_from> + </image> + </library_images> + <library_materials> + <material id="Placard" name="Placard"> + <instance_effect url="#Placard-fx"/> + </material> + </library_materials> + <library_effects> + <effect id="Placard-fx" name="Placard"> + <profile_COMMON> + <technique sid="standard"> + <phong> + <emission> + <color sid="emission">0.000000 0.000000 0.000000 1.000000</color> + </emission> + <ambient> + <color sid="ambient">0.588000 0.588000 0.588000 1.000000</color> + </ambient> + <diffuse> + <texture texture="MapFBXASC032FBXASC0351-image" texcoord="CHANNEL0"> + <extra> + <technique profile="MAYA"> + <wrapU sid="wrapU0">TRUE</wrapU> + <wrapV sid="wrapV0">TRUE</wrapV> + <blend_mode>ADD</blend_mode> + </technique> + </extra> + </texture> + </diffuse> + <specular> + <color sid="specular">0.000000 0.000000 0.000000 1.000000</color> + </specular> + <shininess> + <float sid="shininess">2.000000</float> + </shininess> + <reflective> + <color sid="reflective">0.000000 0.000000 0.000000 1.000000</color> + </reflective> + <reflectivity> + <float sid="reflectivity">1.000000</float> + </reflectivity> + <transparent opaque="RGB_ZERO"> + <color sid="transparent">1.000000 1.000000 1.000000 1.000000</color> + </transparent> + <transparency> + <float sid="transparency">0.000000</float> + </transparency> + </phong> + </technique> + </profile_COMMON> + </effect> + </library_effects> + <library_geometries> + <geometry id="Box001-lib" name="Box001Mesh"> + <mesh> + <source id="Box001-POSITION"> + <float_array id="Box001-POSITION-array" count="192"> +-100.000000 -25.000000 -150.000000 +-100.000000 25.000000 -150.000000 +100.000000 25.000000 -150.000000 +100.000000 -25.000000 -150.000000 +-100.000000 -25.000000 150.000000 +100.000000 -25.000000 150.000000 +100.000000 25.000000 150.000000 +-100.000000 25.000000 150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +-84.680214 14.826656 132.722687 +-84.680214 14.826664 82.722687 +-34.680214 14.826664 82.722687 +-34.680214 14.826656 132.722687 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +48.787140 14.826658 120.222687 +48.787140 14.826662 95.222687 +73.787140 14.826662 95.222687 +73.787140 14.826658 120.222687 +100.000000 -25.000000 66.811462 +100.000000 25.000000 66.811462 +-100.000000 25.000000 66.811462 +-100.000000 -25.000000 66.811462 +-19.386715 -25.000000 66.811462 +25.193655 -25.000000 66.811462 +18.392944 -25.000000 150.000000 +-20.149960 -25.000000 150.000000 +-20.406273 25.000000 150.000000 +20.272606 25.000000 150.000000 +18.256310 25.000000 66.811462 +13.001312 25.000000 -150.000000 +-25.653687 25.000000 66.811462 +-39.329834 25.000000 -150.000000 +27.587723 -25.000000 -150.000000 +-12.759123 -25.000000 -150.000000 +0.000000 0.000000 -150.000000 +-84.680206 -24.999998 132.722687 +-87.685837 -25.000000 135.728302 +0.000000 0.000000 -150.000000 +-31.674593 -25.000000 135.728302 +-34.680214 -24.999998 132.722687 +0.000000 0.000000 -150.000000 +73.787140 -24.999998 120.222687 +76.792763 -25.000000 123.228302 +0.000000 0.000000 -150.000000 +76.792763 -25.000000 92.217072 +73.787140 -24.999998 95.222687 +0.000000 0.000000 -150.000000 +-84.680206 -24.999998 82.722687 +-87.685837 -25.000000 79.717072 +0.000000 0.000000 -150.000000 +48.787140 -24.999998 120.222687 +45.781517 -25.000000 123.228302 +0.000000 0.000000 -150.000000 +45.781517 -25.000000 92.217072 +48.787140 -24.999998 95.222687 +0.000000 0.000000 -150.000000 +-34.680214 -24.999998 82.722687 +-31.674593 -25.000000 79.717072 +</float_array> + <technique_common> + <accessor source="#Box001-POSITION-array" count="64" stride="3"> + <param name="X" type="float"/> + <param name="Y" type="float"/> + <param name="Z" type="float"/> + </accessor> + </technique_common> + </source> + <source id="Box001-Normal0"> + <float_array id="Box001-Normal0-array" count="828"> +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 -1.000000 -0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +</float_array> + <technique_common> + <accessor source="#Box001-Normal0-array" count="276" stride="3"> + <param name="X" type="float"/> + <param name="Y" type="float"/> + <param name="Z" type="float"/> + </accessor> + </technique_common> + </source> + <source id="Box001-UV0"> + <float_array id="Box001-UV0-array" count="232"> +0.546320 0.682655 +0.546320 0.723931 +0.522597 0.723931 +0.570855 0.629042 +0.570855 0.666927 +0.547133 0.666805 +0.406890 0.833552 +0.406890 0.818839 +0.418944 0.809071 +0.352896 0.630064 +0.376618 0.630064 +0.376618 0.732930 +0.521810 0.719682 +0.521810 0.748467 +0.418944 0.748467 +0.519765 0.629042 +0.519765 0.652764 +0.416899 0.652764 +0.352896 0.732930 +0.409559 0.180505 +0.409559 0.156782 +0.621670 0.104854 +0.376618 0.772398 +0.602774 0.104854 +0.409559 0.130203 +0.352896 0.772398 +0.428455 0.130203 +0.601961 0.081132 +0.601961 0.104854 +0.578239 0.104854 +0.416899 0.629042 +0.578239 0.131434 +0.578239 0.107712 +0.377431 0.652764 +0.609930 0.107712 +0.621791 0.107712 +0.597663 0.126608 +0.377431 0.629042 +0.597663 0.107712 +0.409559 0.183362 +0.418944 0.713193 +0.428455 0.183362 +0.578239 0.155760 +0.590100 0.155760 +0.590100 0.167621 +0.590100 0.134292 +0.379475 0.748467 +0.590100 0.153187 +0.522597 0.689576 +0.547133 0.629042 +0.418944 0.844563 +0.521810 0.810207 +0.521810 0.844563 +0.392177 0.833552 +0.379475 0.844563 +0.392177 0.818839 +0.379475 0.805845 +0.386247 0.782090 +0.412821 0.782090 +0.379475 0.710704 +0.379475 0.691404 +0.379475 0.653577 +0.418944 0.653577 +0.418944 0.692360 +0.521810 0.653577 +0.578239 0.081132 +0.521810 0.694853 +0.570855 0.685213 +0.547133 0.686105 +0.570855 0.723931 +0.578239 0.167621 +0.547133 0.723931 +0.522597 0.670433 +0.418944 0.787920 +0.546320 0.657827 +0.522597 0.629042 +0.546320 0.629042 +0.428455 0.156782 +0.379475 0.787558 +0.428455 0.180505 +0.379475 0.749673 +0.418944 0.749673 +0.428455 0.153925 +0.412821 0.755516 +0.409559 0.153925 +0.386247 0.755516 +0.602774 0.081132 +0.621670 0.081132 +0.597135 0.107712 +0.597135 0.131434 +0.621791 0.126608 +0.609930 0.126608 +0.428455 0.195223 +0.409559 0.195223 +0.521810 0.791065 +0.521810 0.749673 +0.461106 0.187098 +0.461106 0.175237 +0.449245 0.187098 +0.449245 0.175237 +0.443314 0.135636 +0.467036 0.135636 +0.467036 0.111913 +0.443314 0.111913 +0.609524 0.107712 +0.609524 0.126608 +0.578239 0.153187 +0.578239 0.134292 +0.462532 0.188524 +0.462532 0.173811 +0.447819 0.188524 +0.447819 0.173811 +0.441888 0.137062 +0.468462 0.137062 +0.468462 0.110487 +0.441888 0.110487 +</float_array> + <technique_common> + <accessor source="#Box001-UV0-array" count="116" stride="2"> + <param name="S" type="float"/> + <param name="T" type="float"/> + </accessor> + </technique_common> + </source> + <vertices id="Box001-VERTEX"> + <input semantic="POSITION" source="#Box001-POSITION"/> + </vertices> + <triangles count="92" material="Placard"> + <input semantic="VERTEX" offset="0" source="#Box001-VERTEX"/> + <input semantic="NORMAL" offset="1" source="#Box001-Normal0"/> + <input semantic="TEXCOORD" offset="2" set="0" source="#Box001-UV0"/> + <p> 38 0 48 2 1 1 3 2 2 2 3 1 38 4 48 35 5 0 32 6 5 4 7 3 31 8 4 4 9 3 32 10 5 7 11 49 24 12 18 2 13 10 25 14 11 2 15 10 24 16 18 3 17 9 36 18 40 37 19 12 26 20 14 26 21 14 37 22 12 1 23 13 26 24 30 0 25 16 27 26 17 0 27 16 26 28 30 1 29 15 12 30 79 41 31 19 53 32 20 12 33 79 53 34 20 13 35 77 14 36 87 13 37 21 53 38 23 14 39 87 53 40 23 62 41 86 15 42 84 62 43 26 45 44 82 62 45 26 15 46 84 14 47 24 12 48 29 14 49 27 15 50 28 14 51 27 12 52 29 13 53 65 45 54 31 41 55 32 15 56 89 41 57 32 12 58 88 15 59 89 20 60 91 56 61 34 60 62 35 20 63 91 60 64 35 21 65 90 22 66 105 21 67 36 60 68 38 22 69 105 60 70 38 51 71 104 23 72 93 51 73 41 47 74 92 51 75 41 23 76 93 22 77 39 23 78 44 20 79 70 21 80 42 23 81 44 21 82 42 22 83 43 20 84 107 47 85 47 56 86 106 47 87 47 20 88 107 23 89 45 50 90 6 59 91 7 29 92 8 50 93 6 29 94 8 24 95 50 24 96 18 6 97 22 5 98 25 6 99 22 24 100 18 25 101 11 32 102 59 36 103 40 7 104 46 7 105 46 36 106 40 26 107 14 26 108 30 4 109 33 7 110 37 4 111 33 26 112 30 27 113 17 24 114 50 38 115 51 3 116 52 38 117 51 24 118 50 29 119 8 24 120 50 5 121 54 48 122 53 24 123 50 48 124 53 50 125 6 5 126 54 30 127 56 57 128 55 5 129 54 57 130 55 48 131 53 28 132 73 31 133 78 44 134 57 28 135 73 44 136 57 63 137 58 4 138 80 27 139 81 42 140 85 42 141 85 27 142 81 54 143 83 31 144 78 4 145 80 42 146 85 31 147 78 42 148 85 44 149 57 32 150 5 30 151 67 33 152 68 30 153 67 32 154 5 31 155 4 33 156 68 5 157 69 6 158 71 5 159 69 33 160 68 30 161 67 6 162 61 25 163 62 33 164 60 33 165 60 25 166 62 34 167 63 25 168 62 2 169 64 34 170 63 34 171 63 2 172 64 35 173 66 33 174 60 34 175 63 32 176 59 32 177 59 34 178 63 36 179 40 34 180 63 35 181 66 36 182 40 36 183 40 35 184 66 37 185 12 28 186 73 39 187 94 29 188 8 29 189 8 39 190 94 38 191 51 35 192 0 38 193 48 39 194 72 39 195 72 37 196 74 35 197 0 27 198 81 0 199 95 28 200 73 28 201 73 0 202 95 39 203 94 39 204 72 0 205 75 37 206 74 0 207 75 1 208 76 37 209 74 54 210 83 28 211 73 63 212 58 28 213 73 54 214 83 27 215 81 31 216 78 28 217 73 30 218 56 30 219 56 28 220 73 29 221 8 30 222 56 59 223 7 57 224 55 59 225 7 30 226 56 29 227 8 59 228 109 50 229 108 51 230 96 59 231 109 51 232 96 60 233 97 50 234 108 48 235 110 47 236 98 50 237 108 47 238 98 51 239 96 48 240 110 57 241 111 56 242 99 48 243 110 56 244 99 47 245 98 63 246 113 44 247 112 45 248 100 63 249 113 45 250 100 62 251 101 42 252 115 54 253 114 53 254 102 42 255 115 53 256 102 41 257 103 44 258 112 42 259 115 41 260 103 44 261 112 41 262 103 45 263 100 63 264 113 53 265 102 54 266 114 53 267 102 63 268 113 62 269 101 57 270 111 59 271 109 60 272 97 57 273 111 60 274 97 56 275 99</p> + </triangles> + </mesh> + </geometry> + </library_geometries> + <library_visual_scenes> + <visual_scene id="placard_2022" name="placard_2022"> + <node name="Box001" id="Box001" sid="Box001"> + <matrix sid="matrix">1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix> + <instance_geometry url="#Box001-lib"> + <bind_material> + <technique_common> + <instance_material symbol="Placard" target="#Placard"/> + </technique_common> + </bind_material> + </instance_geometry> + <extra> + <technique profile="FCOLLADA"> + <visibility>1.000000</visibility> + </technique> + </extra> + </node> + <extra> + <technique profile="MAX3D"> + <frame_rate>30.000000</frame_rate> + </technique> + <technique profile="FCOLLADA"> + <start_time>0.000000</start_time> + <end_time>3.333333</end_time> + </technique> + </extra> + </visual_scene> + </library_visual_scenes> + <scene> + <instance_visual_scene url="#placard_2022"/> + </scene> +</COLLADA> diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config new file mode 100644 index 000000000..5ca0548f5 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config @@ -0,0 +1,20 @@ +<?xml version="1.0"?> +<model> + <name>placard_2022</name> + <version>1.0</version> + <sdf version="1.6">model.sdf</sdf> + + <author> + <name>Carlos Agüero</name> + <email>caguero@openrobotics.org</email> + </author> + + <author> + <name>Cole Biesemeyer</name> + <email>cole@openrobotics.org</email> + </author> + + <description> + A white placard with a symbol used in the RobotX competition. + </description> +</model> diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf new file mode 100644 index 000000000..95411d3c7 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf @@ -0,0 +1,72 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <model name="placard"> + <pose>0 0 1 0 0 0</pose> + <!-- The white placard --> + <link name="link"> + <collision name="collision_1"> + <pose>-0.9234 0.0 1.164 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>0.153199 0.500 0.672</size> + </box> + </geometry> + </collision> + <collision name="collision_2"> + <pose>-0.596803 0.0 1.41357 -3.14159 -1.5708 0.0</pose> + <geometry> + <box> + <size>0.17285 0.5 0.5</size> + </box> + </geometry> + </collision> + <collision name="collision_3"> + <pose>0.326598 0.0 1.35111 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>1.3468 0.500 0.29777</size> + </box> + </geometry> + </collision> + <collision name="collision_4"> + <pose>0.070527 0.0 1.07723 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>0.834664 0.500 0.25</size> + </box> + </geometry> + </collision> + <collision name="collision_6"> + <pose>0.326598 0.0 0.890117 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>1.3468 0.500 0.124233</size> + </box> + </geometry> + </collision> + <collision name="collision_5"> + <pose>0.868936 0.0 1.07723 1.5708 0.0 1.5708</pose> + <geometry> + <box> + <size>0.500 0.25 0.262128</size> + </box> + </geometry> + </collision> + <collision name="collision_7"> + <pose>0.0 0.0 -0.336 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>2.000 0.500 2.328</size> + </box> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <mesh> + <uri>file://placard_2024/meshes/placard_2024.dae</uri> + </mesh> + </geometry> + </visual> + </link> + </model> +</sdf> diff --git a/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config new file mode 100644 index 000000000..741ef5967 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config @@ -0,0 +1,20 @@ +<?xml version="1.0"?> +<model> + <name>symbol_rectangle</name> + <version>1.0</version> + <sdf version="1.6">model.sdf</sdf> + + <author> + <name>Max Vu</name> + <email>m.vu@ufl.edu</email> + </author> + <author> + <name>Carlos Agüero</name> + <email>caguero@openrobotics.org</email> + </author> + + <description> + A square symbol used in some elements of the RobotX competition. + edited for robotx 2024 + </description> +</model> diff --git a/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf new file mode 100644 index 000000000..560189836 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf @@ -0,0 +1,17 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <model name="symbol_square"> + <static>true</static> + <pose>0 0 0 0 1.5708 0</pose> + <link name="link"> + <visual name="visual"> + <geometry> + <box> + <size>0.6096 0.6096 0.001</size> + </box> + </geometry> + <transparency>1</transparency> + </visual> + </link> + </model> +</sdf> diff --git a/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc b/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc new file mode 100644 index 000000000..1b4ab604f --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc @@ -0,0 +1,260 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include <gazebo/rendering/Scene.hh> + +#include "vrx_gazebo/placard_plugin.hh" + +// Static initialization. +std::map<std::string, std_msgs::ColorRGBA> PlacardPlugin::kColors = { + { "red", CreateColor(1.0, 0.0, 0.0, 1.0) }, + { "green", CreateColor(0.0, 1.0, 0.0, 1.0) }, + { "blue", CreateColor(0.0, 0.0, 1.0, 1.0) }, + // {"yellow", CreateColor(1.0, 1.0, 0.0, 1.0)}, +}; + +std::vector<std::string> PlacardPlugin::kShapes = { + // "circle", "cross", "triangle", "rectangle", + "square" +}; + +///////////////////////////////////////////////// +PlacardPlugin::PlacardPlugin() : gzNode(new gazebo::transport::Node()) +{ +} + +////////////////////////////////////////////////// +std_msgs::ColorRGBA PlacardPlugin::CreateColor(const double _r, const double _g, const double _b, const double _a) +{ + static std_msgs::ColorRGBA color; + color.r = _r; + color.g = _g; + color.b = _b; + color.a = _a; + return color; +} + +////////////////////////////////////////////////// +void PlacardPlugin::InitializeAllPatterns() +{ + for (auto const &colorPair : this->kColors) + for (auto const &shape : this->kShapes) + this->allPatterns.push_back({ colorPair.first, shape }); +} + +////////////////////////////////////////////////// +void PlacardPlugin::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf) +{ + GZ_ASSERT(_parent != nullptr, "Received NULL model pointer"); + + this->scene = _parent->GetScene(); + GZ_ASSERT(this->scene != nullptr, "NULL scene"); + + this->InitializeAllPatterns(); + + if (!this->ParseSDF(_sdf)) + return; + + // Quit if ros plugin was not loaded + if (!ros::isInitialized()) + { + ROS_ERROR("ROS was not initialized."); + return; + } + + if (this->shuffleEnabled) + { + this->nh = ros::NodeHandle(this->ns); + this->changeSymbolSub = this->nh.subscribe(this->rosShuffleTopic, 1, &PlacardPlugin::ChangeSymbol, this); + } + + this->nextUpdateTime = this->scene->SimTime(); + + this->updateConnection = gazebo::event::Events::ConnectPreRender(std::bind(&PlacardPlugin::Update, this)); + + this->gzNode->Init(); + this->symbolSub = gzNode->Subscribe(this->symbolSubTopic, &PlacardPlugin::ChangeSymbolTo, this, true); +} + +////////////////////////////////////////////////// +void PlacardPlugin::ChangeSymbolTo(gazebo::ConstDockPlacardPtr &_msg) +{ + std::lock_guard<std::mutex> lock(this->mutex); + this->shape = _msg->shape(); + this->color = _msg->color(); +} + +////////////////////////////////////////////////// +bool PlacardPlugin::ParseSDF(sdf::ElementPtr _sdf) +{ + // We initialize it with a random shape and color. + this->ChangeSymbol(std_msgs::Empty::ConstPtr()); + + // Parse the shape. + if (_sdf->HasElement("shape")) + { + std::string aShape = _sdf->GetElement("shape")->Get<std::string>(); + std::transform(aShape.begin(), aShape.end(), aShape.begin(), ::tolower); + // Sanity check: Make sure the shape is allowed. + if (std::find(this->kShapes.begin(), this->kShapes.end(), aShape) != this->kShapes.end()) + { + this->shape = aShape; + } + else + { + ROS_INFO_NAMED("PlacardPlugin", "incorrect [%s] <shape>, using random shape", aShape.c_str()); + } + } + + // Parse the color. We initialize it with a random color. + if (_sdf->HasElement("colors")) + { + } + else if (_sdf->HasElement("color")) + { + std::string aColor = _sdf->GetElement("color")->Get<std::string>(); + std::transform(aColor.begin(), aColor.end(), aColor.begin(), ::tolower); + // Sanity check: Make sure the color is allowed. + if (this->kColors.find(aColor) != this->kColors.end()) + { + this->color = aColor; + } + else + { + ROS_INFO_NAMED("PlacardPlugin", "incorrect [%s] <color>, using random color", aColor.c_str()); + } + } + + // Required: visuals. + if (!_sdf->HasElement("visuals")) + { + ROS_ERROR("<visuals> missing"); + return false; + } + + auto visualsElem = _sdf->GetElement("visuals"); + if (!visualsElem->HasElement("visual")) + { + ROS_ERROR("<visual> missing"); + return false; + } + + auto visualElem = visualsElem->GetElement("visual"); + while (visualElem) + { + std::string visualName = visualElem->Get<std::string>(); + this->visualNames.push_back(visualName); + visualElem = visualElem->GetNextElement(); + } + + // Optional: Is shuffle enabled? + if (_sdf->HasElement("shuffle")) + { + this->shuffleEnabled = _sdf->GetElement("shuffle")->Get<bool>(); + + // Required if shuffle enabled: ROS topic. + if (!_sdf->HasElement("ros_shuffle_topic")) + { + ROS_ERROR("<ros_shuffle_topic> missing"); + } + this->rosShuffleTopic = _sdf->GetElement("ros_shuffle_topic")->Get<std::string>(); + } + + // Required: namespace. + if (!_sdf->HasElement("robot_namespace")) + { + ROS_ERROR("<robot_namespace> missing"); + } + this->ns = _sdf->GetElement("robot_namespace")->Get<std::string>(); + if (!_sdf->HasElement("gz_symbol_topic")) + { + this->symbolSubTopic = "/" + this->ns + "/symbol"; + } + else + { + this->symbolSubTopic = _sdf->GetElement("gz_symbol_topic")->Get<std::string>(); + } + return true; +} + +////////////////////////////////////////////////// +void PlacardPlugin::Update() +{ + // Get the visuals if needed. + if (this->visuals.empty()) + { + for (auto visualName : this->visualNames) + { + auto visualPtr = this->scene->GetVisual(visualName); + if (visualPtr) + this->visuals.push_back(visualPtr); + else + ROS_ERROR("Unable to find [%s] visual", visualName.c_str()); + } + } + + // Only update the plugin at 1Hz. + if (this->scene->SimTime() < this->nextUpdateTime) + return; + + this->nextUpdateTime = this->nextUpdateTime + gazebo::common::Time(1.0); + + std::lock_guard<std::mutex> lock(this->mutex); + + // Update the visuals. + for (auto visual : this->visuals) + { + std_msgs::ColorRGBA color; + color.a = 0.0; + +#if GAZEBO_MAJOR_VERSION >= 8 + auto name = visual->Name(); +#else + auto name = visual->GetName(); +#endif + auto delim = name.rfind("/"); + auto shortName = name.substr(delim + 1); + if (shortName.find(this->shape) != std::string::npos) + { + color = this->kColors[this->color]; + } +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Color gazeboColor(color.r, color.g, color.b, color.a); +#else + gazebo::common::Color gazeboColor(color.r, color.g, color.b, color.a); +#endif + + visual->SetAmbient(gazeboColor); + visual->SetDiffuse(gazeboColor); + } +} + +////////////////////////////////////////////////// +void PlacardPlugin::ChangeSymbol(const std_msgs::Empty::ConstPtr &_msg) +{ + { + std::lock_guard<std::mutex> lock(this->mutex); + this->color = this->allPatterns[this->allPatternsIdx].at(0); + this->shape = this->allPatterns[this->allPatternsIdx].at(1); + this->allPatternsIdx = (this->allPatternsIdx + 1) % this->allPatterns.size(); + } + + ROS_INFO_NAMED("PlacardPlugin", "New symbol is %s %s", this->color.c_str(), this->shape.c_str()); +} + +// Register plugin with gazebo +GZ_REGISTER_VISUAL_PLUGIN(PlacardPlugin) diff --git a/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world new file mode 100644 index 000000000..d4fbde58e --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world @@ -0,0 +1,413 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <!-- COORDINATE: {'constant': 0, 'environment': 0} --> + <world name="default_scan_dock_fling_2024"> + <!-- Estimated latitude/longitude of sydneyregatta + from satellite imagery --> + <spherical_coordinates> + <surface_model>EARTH_WGS84</surface_model> + <world_frame_orientation>ENU</world_frame_orientation> + <latitude_deg>-33.724223</latitude_deg> + <longitude_deg>150.679736</longitude_deg> + <elevation>0.0</elevation> + <heading_deg>0.0</heading_deg> + </spherical_coordinates> + <!-- A global light source --> + <include> + <uri>model://sun</uri> + </include> + <gui fullscreen="0"> + <camera name="user_camera"> + <pose>-478.101746 148.200836 13.203143 0.0 0.248344 2.936862</pose> + <view_controller>orbit</view_controller> + </camera> + <!--<plugin name="GUITaskWidget" filename="libgui_task_widget.so"/>--> + </gui> + <include> + <!-- Note - the pose tag doesn't seem to work for heightmaps, so you need + to go into the model file to change the altitutde/height!--> + <pose> 0 0 0.2 0 0 0 </pose> + <uri>model://sydney_regatta</uri> + </include> + <!-- The posts for securing the WAM-V --> + <include> + <name>post_0</name> + <pose>-535.916809 154.362869 0.675884 -0.17182 0.030464 -0.005286</pose> + <uri>model://post</uri> + </include> + <include> + <name>post_1</name> + <pose>-527.48999 153.854782 0.425844 -0.1365 0 0</pose> + <uri>model://post</uri> + </include> + <include> + <name>post_2</name> + <pose>-544.832825 156.671951 0.499025 -0.162625 0 0 </pose> + <uri>model://post</uri> + </include> + <!-- Antenna for communication with the WAM-V --> + <include> + <pose>-531.063721 147.668579 1.59471 -0.068142 0 -0.1</pose> + <uri>model://antenna</uri> + </include> + <!-- ground station tents --> + <include> + <name>ground_station_0</name> + <pose>-540.796448 146.493744 1.671421 -0.00834 0.01824 1.301726</pose> + <uri>model://ground_station</uri> + </include> + <include> + <name>ground_station_1</name> + <pose>-537.622681 145.827896 1.681931 -0.00603 0.018667 1.301571</pose> + <uri>model://ground_station</uri> + </include> + <include> + <name>ground_station_2</name> + <pose>-534.550537 144.910400 1.720474 -0.004994 0.020798 1.301492</pose> + <uri>model://ground_station</uri> + </include> + <!-- The projectile for the ball shooter --> + <include> + <name>blue_projectile</name> + <pose>-545 60 0.03 0 0 0</pose> + <uri>model://blue_projectile</uri> + </include> + <model name="ocean_waves"> + <static>true</static> + <plugin filename="libWavefieldModelPlugin.so" name="wavefield_plugin"> + <static>false</static> + <update_rate>30</update_rate> + <size>1000 1000</size> + <cell_count>50 50</cell_count> + <wave> + <model>PMS</model> + <period>5.0</period> + <number>3</number> + <scale>2.5</scale> + <gain>0.0</gain> + <direction>1.0 0.0</direction> + <angle>0.4</angle> + <tau>2.0</tau> + <amplitude>0.0</amplitude> + <!-- No effect for the PMS model --> + <steepness>0.0</steepness> + </wave> + </plugin> + <link name="ocean_waves_link"> + <visual name="ocean_waves_visual"> + <plugin filename="libWavefieldVisualPlugin.so" name="ocean_waves_visual_plugin"> + <enableRtt>true</enableRtt> + <rttNoise>0.1</rttNoise> + <refractOpacity>0.2</refractOpacity> + <reflectOpacity>0.2</reflectOpacity> + <wave> + <model>PMS</model> + <period>5.0</period> + <number>3</number> + <scale>2.5</scale> + <gain>0.0</gain> + <direction>1.0 0.0</direction> + <angle>0.4</angle> + <tau>2.0</tau> + <amplitude>0.0</amplitude> + <!-- No effect for the PMS model --> + <steepness>0.0</steepness> + </wave> + </plugin> + <geometry> + <mesh> + <scale>2.5 2.5 1</scale> + <uri>model://ocean_waves/meshes/mesh.dae</uri> + </mesh> + </geometry> + <material> + <script> + <uri>model://ocean_waves/materials/scripts/waves.material</uri> + <name>WaveSim/GerstnerWaves</name> + </script> + </material> + <laser_retro>-1</laser_retro> + </visual> + <visual name="ocean_waves_below_visual"> + <pose>0 0 -0.05 0 0 0</pose> + <!-- Offset to prevent rendering conflict --> + <geometry> + <mesh> + <scale>2.5 2.5 1</scale> + <uri>model://ocean_waves/meshes/mesh_below.dae</uri> + </mesh> + </geometry> + <material> + <script> + <uri>model://ocean_waves/materials/scripts/waves.material</uri> + <name>WaveSim/GerstnerWaves</name> + </script> + </material> + <laser_retro>-1</laser_retro> + </visual> + </link> + </model> + <!--Gazebo Plugin for simulating WAM-V dynamics--> + <plugin filename="libusv_gazebo_wind_plugin.so" name="wind"> + <!-- models to be effected by the wind --> + <wind_obj> + <name>wamv</name> + <link_name>wamv/base_link</link_name> + <coeff_vector>.5 .5 .33</coeff_vector> + </wind_obj> + <!-- Wind --> + <wind_direction>240</wind_direction> + <!-- in degrees --> + <wind_mean_velocity>0.0</wind_mean_velocity> + <var_wind_gain_constants>0</var_wind_gain_constants> + <var_wind_time_constants>2</var_wind_time_constants> + <random_seed>10</random_seed> + <!-- set to zero/empty to randomize --> + <update_rate>10</update_rate> + <topic_wind_speed>/vrx/debug/wind/speed</topic_wind_speed> + <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> + </plugin> + <scene> + <sky> + <clouds> + <speed>12</speed> + </clouds> + </sky> + <grid>0</grid> + <origin_visual>0</origin_visual> + <ambient> + +1 1 1 1 + </ambient> + </scene> + <include> + <uri>model://dock_2024</uri> + <pose>-480 190 0 0 0 1.0</pose> + </include> + <include> + <uri>model://robotx_light_buoy</uri> + <pose>-532 175 0.25 0 0 0</pose> + </include> + + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard1_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard1_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard1_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard1_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard2_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard2_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard2_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard2_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard3_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard3_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard3_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard3_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- The scoring plugin --> + <plugin filename="libscan_dock_scoring_plugin.so" name="scan_dock__deliver_scoring_plugin"> + <!-- Parameters for scoring_plugin --> + <vehicle>wamv</vehicle> + <task_name>scan_dock_deliver</task_name> + <initial_state_duration>3</initial_state_duration> + <ready_state_duration>3</ready_state_duration> + <running_state_duration>300</running_state_duration> + <release_joints> + <joint> + <name>wamv_external_pivot_joint</name> + </joint> + <joint> + <name>wamv_external_riser</name> + </joint> + </release_joints> + <!-- Color sequence checker --> + <robot_namespace>vrx</robot_namespace> + <color_sequence_service>scan_dock_deliver/color_sequence</color_sequence_service> + <color_1>yellow</color_1> + <color_2>blue</color_2> + <color_3>green</color_3> + <!-- Dock checkers --> + <dock_bonus_points>15</dock_bonus_points> + <correct_dock_bonus_points>5</correct_dock_bonus_points> + <bays> + <bay> + <name>bay1</name> + <internal_activation_topic>/vrx/dock_2024/bay_1/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_1_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard1/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>True</dock_allowed> + <symbol>red_square</symbol> + </bay> + <bay> + <name>bay2</name> + <internal_activation_topic>/vrx/dock_2024/bay_2/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_2_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard2/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>False</dock_allowed> + <symbol>blue_square</symbol> + </bay> + <bay> + <name>bay3</name> + <internal_activation_topic>/vrx/dock_2024/bay_3/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_3_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard3/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>False</dock_allowed> + <symbol>green_square</symbol> + </bay> + </bays> + <!-- Shooting targets --> + <targets> + <!-- Placard #1 --> + <target> + <topic>vrx/dock_2024_placard1_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard1_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + <!-- Placard #2 --> + <target> + <topic>vrx/dock_2024_placard2_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard2_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + <!-- Placard #3 --> + <target> + <topic>vrx/dock_2024_placard3_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard3_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + </targets> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #1 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay1"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_1</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay1_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_1_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #2 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay2"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_2</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay2_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_2_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #3 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay3"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_3</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay3_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_3_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + </world> +</sdf> diff --git a/SubjuGator/drivers/vectornav b/SubjuGator/drivers/vectornav index 1bf953def..b907c18b2 160000 --- a/SubjuGator/drivers/vectornav +++ b/SubjuGator/drivers/vectornav @@ -1 +1 @@ -Subproject commit 1bf953defad87b7373e318b3858614f5a8bdaf70 +Subproject commit b907c18b2c43da64ad8fb089e3001b25640b7a39 diff --git a/mil_common/axros b/mil_common/axros index d096c6746..edf86ec28 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit d096c6746d597c6964e69933ddab0156096bfa78 +Subproject commit edf86ec28f8be8f0659827fc924ef3fb46884331 diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/CMakeLists.txt b/mil_common/perception/point_cloud_object_detection_and_recognition/CMakeLists.txt index fc2306573..3bae3e2e2 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/CMakeLists.txt +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/CMakeLists.txt @@ -1,6 +1,6 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 2.8.3) project(point_cloud_object_detection_and_recognition) find_package(catkin REQUIRED COMPONENTS diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_associator.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_associator.hpp index 4d009602d..238b73544 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_associator.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_associator.hpp @@ -19,6 +19,7 @@ class Associator public: /// Update the dynamic reconfigure parameters associated with this class void update_config(Config const& config); + void get_config(Config& config); /// Associate old objects with newly identified clusters. @prev_objects is updated + appended in place for new /// associations void associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters); diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_detector.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_detector.hpp index 3a9c127b2..29f94dd15 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_detector.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/object_detector.hpp @@ -18,6 +18,7 @@ class ObjectDetector clusters_t get_clusters(point_cloud_const_ptr pc); /// Update the dynamic reconfigure parameters associated with this class void update_config(Config const& config); + void get_config(Config& config); private: pcl::EuclideanClusterExtraction<point_t> cluster_extractor_; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/ogrid_manager.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/ogrid_manager.hpp index 1a4ef4a3f..35d93126f 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/ogrid_manager.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/ogrid_manager.hpp @@ -23,6 +23,7 @@ class OgridManager void update_ogrid(ObjectMap const& objects); void draw_boundary(); void update_config(Config const& config); + void get_config(Config& config); void set_bounds(point_cloud_ptr pc); private: diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp index 1db53eaa9..a4817b249 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp @@ -27,6 +27,7 @@ #include "pcodar_types.hpp" #include "persistent_cloud_filter.hpp" #include "point_cloud_builder.hpp" +#include "std_srvs/SetBool.h" namespace pcodar { @@ -50,6 +51,7 @@ class NodeBase bool DBQuery_cb(mil_msgs::ObjectDBQuery::Request& req, mil_msgs::ObjectDBQuery::Response& res); /// Reset PCODAR virtual bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + /// Transform bool transform_to_global(std::string const& frame, ros::Time const& time, Eigen::Affine3d& out, ros::Duration timeout = ros::Duration(1, 0)); @@ -57,6 +59,8 @@ class NodeBase bool transform_point_cloud(const sensor_msgs::PointCloud2& pcloud2, point_cloud_i& out); virtual bool bounds_update_cb(const mil_bounds::BoundsConfig& config); virtual void ConfigCallback(Config const& config, uint32_t level); + virtual void save_config(); + virtual void restore_config(); public: std::shared_ptr<ObjectMap> objects_; @@ -83,6 +87,7 @@ class NodeBase MarkerManager marker_manager_; OgridManager ogrid_manager_; + Config saved_config_; // Intensity filter double intensity_filter_min_intensity; double intensity_filter_max_intensity; @@ -98,17 +103,21 @@ class Node : public NodeBase void initialize() override; private: + void save_config() override; + void restore_config() override; bool bounds_update_cb(const mil_bounds::BoundsConfig& config) override; void ConfigCallback(Config const& config, uint32_t level) override; void update_config(Config const& config); /// Reset PCODAR bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) override; + bool StoreParameters(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); private: ros::Publisher pub_pcl_; // Subscriber ros::Subscriber pc_sub; + ros::ServiceServer store_parameters_service_; // Model (It eventually will be object tracker, but for now just detections) InputCloudFilter input_cloud_filter_; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/persistent_cloud_filter.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/persistent_cloud_filter.hpp index bb4425510..6bf1a30c5 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/persistent_cloud_filter.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/persistent_cloud_filter.hpp @@ -13,6 +13,7 @@ class PersistentCloudFilter PersistentCloudFilter(); void filter(point_cloud_const_ptr in, point_cloud& pc); void update_config(Config const& config); + void get_config(Config& config); private: pcl::RadiusOutlierRemoval<point_t> outlier_filter_; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/point_cloud_builder.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/point_cloud_builder.hpp index e3cb97ee5..190cf62d9 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/point_cloud_builder.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/point_cloud_builder.hpp @@ -26,6 +26,8 @@ class PointCloudCircularBuffer /// Clear buffer void clear(); + void get_config(Config& config); + private: /// Accumulated pointcloud kept up to date with each call to add_point_cloud point_cloud_ptr mega_cloud_; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp index d506d6553..d30c512cd 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp @@ -80,4 +80,10 @@ void Associator::update_config(Config const& config) forget_unseen_ = config.associator_forget_unseen; } +void Associator::get_config(Config& config) +{ + config.associator_max_distance = max_distance_; + config.associator_forget_unseen = forget_unseen_; +} + } // namespace pcodar diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_detector.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_detector.cpp index 86832c064..169ec50f5 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_detector.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_detector.cpp @@ -27,4 +27,10 @@ void ObjectDetector::update_config(Config const& config) cluster_extractor_.setMinClusterSize(config.cluster_min_points); } +void ObjectDetector::get_config(Config& config) +{ + config.cluster_tolerance_m = cluster_extractor_.getClusterTolerance(); + config.cluster_min_points = cluster_extractor_.getMinClusterSize(); +} + } // namespace pcodar diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp index 3a3bc1dc9..c7639187b 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp @@ -16,21 +16,21 @@ void OgridManager::initialize(ros::NodeHandle& nh) void OgridManager::draw_boundary() { - std::vector<cv::Point> bounds(bounds_->size()); - for (int i = 0; i < bounds.size(); ++i) - { - bounds[i] = point_in_ogrid(bounds_->points[i]); - } + /* + std::vector<cv::Point>bounds(pcodar::boundary.size()); + for(int i = 0; i < bounds.size(); ++i) { + bounds[i] = point_in_ogrid(bounds[i]); + } - for (int i = 0; i < bounds.size(); ++i) - { - // std::cout << bounds[i] << std::endl; - cv::circle(ogrid_mat_, bounds[i], 15, cv::Scalar(99), -1); - } - const cv::Point* pts = (const cv::Point*)cv::Mat(bounds).data; - int npts = cv::Mat(bounds).rows; + for(int i = 0; i < bounds.size(); ++i) { + // std::cout << bounds[i] << std::endl; + cv::circle(ogrid_mat_, bounds[i], 15, cv::Scalar(99), -1); + } + const cv::Point *pts = (const cv::Point*) cv::Mat(bounds).data; + int npts = cv::Mat(bounds).rows; - cv::polylines(ogrid_mat_, &pts, &npts, 1, true, cv::Scalar(99), 5); + cv::polylines(ogrid_mat_, &pts, &npts, 1, true, cv::Scalar(99), 5); + */ } void OgridManager::set_bounds(point_cloud_ptr pc) @@ -115,4 +115,12 @@ void OgridManager::update_config(Config const& config) ogrid_mat_ = cv::Mat(cv::Size(ogrid_.info.width, ogrid_.info.height), CV_8UC1, ogrid_.data.data()); } +void OgridManager::get_config(Config& config) +{ + config.ogrid_width_meters = width_meters_; + config.ogrid_height_meters = height_meters_; + config.ogrid_resolution_meters_per_cell = resolution_meters_per_cell_; + config.ogrid_inflation_meters = inflation_cells_ * resolution_meters_per_cell_; +} + } // namespace pcodar diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp index 5a0ccf137..9b32ab50a 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp @@ -95,6 +95,16 @@ bool NodeBase::Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Respons return true; } +void NodeBase::save_config() +{ + ogrid_manager_.get_config(saved_config_); +} + +void NodeBase::restore_config() +{ + ogrid_manager_.update_config(saved_config_); +} + bool NodeBase::transform_point_cloud(const sensor_msgs::PointCloud2& pc_msg, point_cloud_i& out) { Eigen::Affine3d transform; @@ -127,6 +137,37 @@ Node::Node(ros::NodeHandle _nh) : NodeBase(_nh) input_cloud_filter_.set_robot_footprint(min, max); } +void Node::save_config() +{ + NodeBase::save_config(); + persistent_cloud_builder_.get_config(saved_config_); + persistent_cloud_filter_.get_config(saved_config_); + detector_.get_config(saved_config_); + ass.get_config(saved_config_); +} + +void Node::restore_config() +{ + NodeBase::restore_config(); + persistent_cloud_builder_.update_config(saved_config_); + persistent_cloud_filter_.update_config(saved_config_); + detector_.update_config(saved_config_); + ass.update_config(saved_config_); +} + +bool Node::StoreParameters(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) +{ + if (req.data) + { + save_config(); + } + else + { + restore_config(); + } + return true; +} + void Node::update_config(Config const& config) { this->intensity_filter_min_intensity = config.intensity_filter_min_intensity; @@ -157,6 +198,7 @@ void Node::initialize() // Publish occupancy grid and visualization markers pub_pcl_ = nh_.advertise<point_cloud>("persist_pcl", 1); + store_parameters_service_ = nh_.advertiseService("save", &Node::StoreParameters, this); } bool Node::Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/persistent_cloud_filter.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/persistent_cloud_filter.cpp index 0a75b0b2b..35a77fcb2 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/persistent_cloud_filter.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/persistent_cloud_filter.cpp @@ -23,4 +23,10 @@ void PersistentCloudFilter::update_config(Config const& config) outlier_filter_.setMinNeighborsInRadius(config.persistant_cloud_filter_min_neighbors); } +void PersistentCloudFilter::get_config(Config& config) +{ + config.persistant_cloud_filter_radius = outlier_filter_.getRadiusSearch(); + config.persistant_cloud_filter_min_neighbors = outlier_filter_.getMinNeighborsInRadius(); +} + } // namespace pcodar diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/point_cloud_builder.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/point_cloud_builder.cpp index d69caddc3..4513b3a39 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/point_cloud_builder.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/point_cloud_builder.cpp @@ -34,6 +34,11 @@ void PointCloudCircularBuffer::update_config(Config const& config) prev_clouds_.set_capacity(config.accumulator_number_persistant_clouds); } +void PointCloudCircularBuffer::get_config(Config& config) +{ + config.accumulator_number_persistant_clouds = prev_clouds_.capacity(); +} + point_cloud_ptr PointCloudCircularBuffer::get_point_cloud() { return mega_cloud_; diff --git a/mil_common/perception/vision_stack b/mil_common/perception/vision_stack index c0465411b..f0b44298b 160000 --- a/mil_common/perception/vision_stack +++ b/mil_common/perception/vision_stack @@ -1 +1 @@ -Subproject commit c0465411b257a947b06c7be07e8a36b270956d6d +Subproject commit f0b44298b6019978725dcb7e51ded59ecb81deea diff --git a/mil_common/perception/yolov7-ros b/mil_common/perception/yolov7-ros index 800224501..dba012183 160000 --- a/mil_common/perception/yolov7-ros +++ b/mil_common/perception/yolov7-ros @@ -1 +1 @@ -Subproject commit 800224501ed4a9b7aa4ba71a2dbd67d59a3373b8 +Subproject commit dba012183a5d2ab1035c232fa7174debdfdf37e9