diff --git a/core/src/custom_msgs/msg/SonarSweepResponse.msg b/core/src/custom_msgs/msg/SonarSweepResponse.msg index 2b4fad92..e5dab184 100644 --- a/core/src/custom_msgs/msg/SonarSweepResponse.msg +++ b/core/src/custom_msgs/msg/SonarSweepResponse.msg @@ -4,5 +4,8 @@ geometry_msgs/Pose pose # Normal angle float64 normal_angle +# Angle of wall +float64 angle_of_wall + # Object is there -bool is_object \ No newline at end of file +bool is_object diff --git a/core/src/custom_msgs/srv/SonarSweepRequest.srv b/core/src/custom_msgs/srv/SonarSweepRequest.srv index 730944b7..7ba05db4 100644 --- a/core/src/custom_msgs/srv/SonarSweepRequest.srv +++ b/core/src/custom_msgs/srv/SonarSweepRequest.srv @@ -10,3 +10,4 @@ bool is_object geometry_msgs/PoseStamped pose float64 normal_angle +float64 angle_of_wall diff --git a/onboard/src/sonar/README.md b/onboard/src/sonar/README.md new file mode 100644 index 00000000..5fc2b17b --- /dev/null +++ b/onboard/src/sonar/README.md @@ -0,0 +1,39 @@ +# Sonar + +The sonar package listens from sonar ping data from the Blue Robotics Ping360. We then run a denoising pipeline which primarily utilizes 2D FFTs to allow for object detection within the scan. The package publishes sonar scans as they are run. + +## Structure +The following are the folders and files in the sonar package: +`config`: Robot-specific config files for the sonar package. +`launch`: Contains the launch files for the Sonar package. +`resource`: Empty ROS2 resource directory. +`sonar`: +- `decode_ping_python_360.py`: Directly interfaces with the Ping360 to decode messages. +- `sonar_image_processing.py`: Contains utility methods to process Sonar images. +- `sonar_object_detection.py`: Contains pipeline for sonar denoising and object detection. +- `sonar_utils.py`: Contains utility sonar methods for sonar calculations. +- `sonar_test_client`: Creates client node to send sonar sweep requests. +- `sonar.py`: Contains logic to initialize and run the sonar node. +`sweep_data`: Sample raw sonar scan data. + +## Config +The `config` directory contains robot-specific `.yaml` files. The format is as follows: +```yaml +ftdi: FTDI device serial number of the USB-to-serial adapter used by the Ping360 +center_gradians: Referencing heading for center direction of the sonar +increase_ccw: Whether angle values increases counterclockwise or not +``` + +## Topics + +### Published +- `/sonar/image/raw` + - When the sonar pipeline runs, it publishes the raw sonar image to this topic + - Type: `sensor_msgs/CompressedImage` +- `/sonar/image/compressed` + - When the sonar pipeline runs, it publishes the denoised image to this topic + - Type: `sensor_msgs/CompressedImage` +- `/sonar/wall/angle` + - When the sonar pipeline runs, it publishes the relative angle of a wall (if found) to the robot + - When it faces directly at a wall: 0 radians, if it is parallel with the wall on the right side: pi/2 radians, if it is parallel with the wall on the left side: -pi/2 radians. + - Type: `std_msgs/msg/Float32` diff --git a/onboard/src/sonar/sonar/sonar.py b/onboard/src/sonar/sonar/sonar.py index 2c6cec37..1b18da25 100644 --- a/onboard/src/sonar/sonar/sonar.py +++ b/onboard/src/sonar/sonar/sonar.py @@ -1,9 +1,6 @@ -import io import os from pathlib import Path -import cv2 -import matplotlib.pyplot as plt import numpy as np import rclpy import resource_retriever as rr @@ -12,21 +9,19 @@ from brping import Ping360 from custom_msgs.srv import SonarSweepRequest from cv_bridge import CvBridge -from geometry_msgs.msg import PoseStamped -from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas +from geometry_msgs.msg import Pose, PoseStamped from rclpy.node import Node from sensor_msgs.msg import CompressedImage from serial.tools import list_ports -from sklearn.cluster import DBSCAN -from sklearn.decomposition import PCA -from std_msgs.msg import String +from std_msgs.msg import Float32, String -from sonar import sonar_image_processing, sonar_utils +from sonar import sonar_image_processing, sonar_object_detection, sonar_utils class Sonar(Node): """Class to interface with the Sonar device.""" - CONFIG_FILE_PATH = f'package://sonar/config/{os.getenv("ROBOT_NAME")}.yaml' + + CONFIG_FILE_PATH = f"package://sonar/config/{os.getenv('ROBOT_NAME')}.yaml" BAUD_RATE = 2000000 # hz SAMPLE_PERIOD_TICK_DURATION = 25e-9 # s @@ -40,16 +35,18 @@ class Sonar(Node): _serial_port = None CONNECTION_RETRY_PERIOD = 5 # s LOOP_RATE = 10 # Hz - STATUS_LOOP_RATE = 5 # Hz + STATUS_LOOP_RATE = 5 # Hz SONAR_STATUS_TOPIC = 'sonar/status' SONAR_REQUEST_TOPIC = 'sonar/request' SONAR_IMAGE_TOPIC = 'sonar/image/compressed' + SONAR_RAW_IMAGE_TOPIC = 'sonar/image/raw' + SONAR_WALL_ANGLE_PUBLISHER = 'sonar/wall/angle' NODE_NAME = 'sonar' - CONSTANT_SWEEP_START = 100 - CONSTANT_SWEEP_END = 300 + CONSTANT_SWEEP_START = 250 + CONSTANT_SWEEP_END = 150 VALUE_THRESHOLD = 95 # Sonar intensity threshold DBSCAN_EPS = 3 # DBSCAN epsilon @@ -57,6 +54,8 @@ class Sonar(Node): NUM_RETRIES = 10 + NEGATE_POSE = False + def __init__(self) -> None: super().__init__(self.NODE_NAME) self.get_logger().info('Sonar planning node initialized') @@ -80,6 +79,8 @@ def __init__(self) -> None: self.cv_bridge = CvBridge() if self.stream: self.sonar_image_publisher = self.create_publisher(CompressedImage, self.SONAR_IMAGE_TOPIC, 10) + self.raw_image_publisher = self.create_publisher(CompressedImage, self.SONAR_RAW_IMAGE_TOPIC, 10) + self.wall_angle_publisher = self.create_publisher(Float32, self.SONAR_WALL_ANGLE_PUBLISHER, 10) self.current_scan = (-1, -1, -1) # (start_angle, end_angle, distance_of_scan) @@ -106,18 +107,18 @@ def connect(self) -> None: self.init_sonar() self.run() except StopIteration: - self.get_logger().error(f'Error in connecting to sonar, trying again in ' - f'{self.CONNECTION_RETRY_PERIOD} seconds.') + self.get_logger().error( + f'Error in connecting to sonar, trying again in {self.CONNECTION_RETRY_PERIOD} seconds.', + ) def init_sonar(self) -> None: """Set up default parameters for the sonar device.""" - self.number_of_samples = self.DEFAULT_NUMER_OF_SAMPLES - self.ping360.set_number_of_samples(self.number_of_samples) + self.ping360.set_number_of_samples(self.DEFAULT_NUMER_OF_SAMPLES) - self.sample_period = self.range_to_period(self.DEFAULT_RANGE) + self.sample_period = sonar_utils.range_to_period(self.DEFAULT_RANGE, self.DEFAULT_NUMER_OF_SAMPLES) self.ping360.set_sample_period(self.sample_period) - self.transmit_duration = self.range_to_transmit(self.DEFAULT_RANGE) + self.transmit_duration = sonar_utils.range_to_transmit(self.DEFAULT_RANGE, self.DEFAULT_NUMER_OF_SAMPLES) self.ping360.set_transmit_duration(self.transmit_duration) def publish_status(self) -> None: @@ -125,52 +126,6 @@ def publish_status(self) -> None: # make string type std_msgs String self.status_publisher.publish(String(data='Sonar Running')) - def range_to_period(self, sonar_range: int) -> int: - """ - From a given range determines the sample_period. - - Sample_period is the time between each sample. Given a distance we can calculate the sample period using the - formula: 2 * range / (number_of_samples * speed_of_sound_in_water * 25e-9) where number of samples is the - number of samples taken between 0m and the set range, speed of sound in water is 1480m/s and 25e-9 is the - sample period tick duration. - - https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3 - - Args: - sonar_range (int): max range in meters of the sonar scan. - - Returns: - sample_period (int): sample period in ms. - """ - period = 2 * sonar_range / (self.number_of_samples * self.SPEED_OF_SOUND_IN_WATER - * self.SAMPLE_PERIOD_TICK_DURATION) - return round(period) - - def range_to_transmit(self, sonar_range: int) -> int: - """ - From a given range determines the transmit_duration. - - Per firmware engineer: - 1. Starting point is TxPulse in usec = ((one-way range in metres) * 8000) / - (Velocity of sound in metres per second) - 2. Then check that TxPulse is wide enough for currently selected sample interval in usec, - i.e., if TxPulse < (2.5 * sample interval) then TxPulse = (2.5 * sample interval) - 3. Perform limit checking - https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3 - - Args: - sonar_range (int): max range in meters of the sonar scan. - - Returns: - transmit_duration (int): max transmit duration in ms. - """ - # 1 - transmit_duration = 8000 * sonar_range / self.SPEED_OF_SOUND_IN_WATER - # 2 (transmit duration is microseconds, samplePeriod() is nanoseconds) - transmit_duration = max(self.range_to_period(sonar_range) / 40, transmit_duration) - # 3 min_transmit is 5 and max_transmit is 500 - return round(max(5, min(500, transmit_duration))) - def set_new_range(self, sonar_range: int) -> None: """ Set a new sample_period and transmit_duration. @@ -179,60 +134,12 @@ def set_new_range(self, sonar_range: int) -> None: sonar_range (int): max range in meters of the sonar scan. """ self.prev_range = sonar_range - self.sample_period = self.range_to_period(sonar_range) + self.sample_period = sonar_utils.range_to_period(sonar_range, self.DEFAULT_NUMER_OF_SAMPLES) self.ping360.set_sample_period(self.sample_period) - self.transmit_duration = self.range_to_transmit(sonar_range) + self.transmit_duration = sonar_utils.range_to_transmit(sonar_range, self.DEFAULT_NUMER_OF_SAMPLES) self.ping360.set_transmit_duration(self.transmit_duration) - def meters_per_sample(self) -> float: - """ - Return the target distance per sample, in meters. - - https://discuss.bluerobotics.com/t/access-ping360-data-for-post-processing-python/10416/2 - - Returns: - float: Distance per sample. - """ - # sample_period is in 25ns increments - # time of flight includes there and back, so divide by 2 - return self.SPEED_OF_SOUND_IN_WATER * self.sample_period * self.SAMPLE_PERIOD_TICK_DURATION / 2.0 - - def get_distance_of_sample_old(self, sample_index: int) -> float: - """ - Get the distance in meters of a sample given its index in the data array returned from the device. - - Computes distance using formula from - https://bluerobotics.com/learn/understanding-and-using-scanning-sonars/. - - Args: - sample_index (int): Index of the sample in the data array, from 0 to N-1, - where N = number of samples. - - Returns: - float: Distance in meters of the sample from the sonar device. - """ - # 0.5 for the average distance of sample - return (sample_index + 0.5) * self.meters_per_sample() - - def get_distance_of_sample(self, pixel_distance: float) -> float: - """ - Get the distance in meters of a sample given its index in the data array returned from the device. - - Computes distance using formula from - https://bluerobotics.com/learn/understanding-and-using-scanning-sonars/. - - Args: - sample_index (int): Index of the sample in the data array, from 0 to N-1, - where N = number of samples. - pixel_distance (float): Pixel distance of the sample. - - Returns: - float: Distance in meters of the sample from the sonar device. - """ - # 0.5 for the average distance of sample - return (pixel_distance + 0.5) * self.meters_per_sample() - def request_data_at_angle(self, angle_in_gradians: float) -> list: """ Set sonar device to provided angle and retrieve data. @@ -255,8 +162,8 @@ def request_data_at_angle(self, angle_in_gradians: float) -> list: break self.get_logger().error(f'Error in getting data at angle {angle_in_gradians}') - response_to_int_array = [int(item) for item in response.data] # converts bytestring to int array - return [0] * self.FILTER_INDEX + response_to_int_array[self.FILTER_INDEX:] + response_to_int_array = [int(item) for item in response.data] + return [0] * self.FILTER_INDEX + response_to_int_array[self.FILTER_INDEX :] def get_sweep(self, range_start: int = 100, range_end: int = 300) -> np.ndarray: """ @@ -277,69 +184,12 @@ def get_sweep(self, range_start: int = 100, range_end: int = 300) -> np.ndarray: for i in range(range_end, range_start + 1, -1 if self.increase_ccw else 1): sonar_scan = self.request_data_at_angle(i) sonar_sweep_data.append(sonar_scan) - sweep = np.vstack(sonar_sweep_data) - cart_grid = sonar_utils.polar2cart(sweep, range_start, range_end) - return sweep, cart_grid - def to_robot_position(self, angle: float, index: int) -> PoseStamped: - """ - Convert a point in sonar space to a robot global position. + return np.vstack(sonar_sweep_data) - Args: - angle (float): Angle in gradians of the point relative to in front - of the sonar device. - index (int | float): Index of the data in the sonar response. - - Returns: - PoseStamped: PoseStamped in the sonar's frame containing x and y position of angle/index item. - """ - x_pos = self.get_distance_of_sample(index)*np.cos( - sonar_utils.centered_gradians_to_radians(angle, self.center_gradians, self.increase_ccw)) - y_pos = -1 * self.get_distance_of_sample(index)*np.sin( - sonar_utils.centered_gradians_to_radians(angle, self.center_gradians, self.increase_ccw)) - pos_of_point = PoseStamped() - pos_of_point.header.stamp = self.get_clock().now().to_msg() - pos_of_point.header.frame_id = 'sonar_ping_360' - pos_of_point.pose.position.x = x_pos - pos_of_point.pose.position.y = y_pos - pos_of_point.pose.position.z = 0.0 # z cord is not really 0 but we don't care - pos_of_point.pose.orientation.x = 0.0 - pos_of_point.pose.orientation.y = 0.0 - pos_of_point.pose.orientation.z = 0.0 - pos_of_point.pose.orientation.w = 1.0 - - return pos_of_point - - def get_xy_of_object_in_sweep_old(self, start_angle: int, end_angle: int) -> \ - tuple[PoseStamped | None, np.ndarray, float | None]: - """ - Get the depth of the sweep of a detected object. For now uses mean value. - - Args: - start_angle (int): Angle to start sweep in gradians. - end_angle (int): Angle to end sweep in gradians. - - Returns: - (PoseStamped, np.ndarray, float): Pose of the object in robot reference frame, sonar sweep array, and normal - angle. - """ - _, cart_grid = self.get_sweep(start_angle, end_angle) - - sonar_index, normal_angle, _ = sonar_image_processing.find_center_point_and_angle( - cart_grid, self.VALUE_THRESHOLD, self.DBSCAN_EPS, - self.DBSCAN_MIN_SAMPLES, True) - - color_image = sonar_image_processing.build_color_sonar_image_from_int_array(cart_grid) - - if sonar_index is None: - return (None, color_image, None) - - sonar_angle = (start_angle + end_angle) / 2 # Take the middle of the sweep - - return (self.to_robot_position(sonar_angle, sonar_index), color_image, normal_angle) - - def get_xy_of_object_in_sweep(self, start_angle: int, end_angle: int) -> \ - tuple[PoseStamped | None, np.ndarray, float | None]: # noqa: PLR0915 + def get_xy_of_object_in_sweep( + self, start_angle: int, end_angle: int, + ) -> tuple[Pose | None, np.ndarray, float | None, float | None]: """ Get the depth of the sweep of a detected object. For now uses mean value. @@ -348,159 +198,59 @@ def get_xy_of_object_in_sweep(self, start_angle: int, end_angle: int) -> \ end_angle (int): Angle to end sweep in gradians. Returns: - (PoseStamped, np.ndarray, float): Pose of the object in robot reference frame, sonar sweep array, and normal - angle. + (Pose, List, float, float): Pose of the object in robot reference frame, sonar sweep array, normal angle, + and angle of wall. """ - sonar_sweep_array, cart_grid = self.get_sweep(start_angle, end_angle) - np.save(f'onboard/src/sonar/sonar/sweep_data/sweep_{self.get_clock().now().nanoseconds}',sonar_sweep_array) - intensity_threshold = 0.6*np.max(sonar_sweep_array) # Example threshold - line_points_mask = cart_grid > intensity_threshold - - # Get the coordinates of the line points - line_points_coords = np.argwhere(line_points_mask) - - center_x = None - center_y = None - line_angle_deg = None - normal_angle_deg = None - - if len(line_points_coords) > 1: - # --- Apply DBSCAN to cluster the points --- - dbscan = DBSCAN(eps=self.DBSCAN_EPS, min_samples=self.DBSCAN_MIN_SAMPLES) # tune eps/min_samples as needed - labels = dbscan.fit_predict(line_points_coords) + sweep = self.get_sweep(start_angle, end_angle) + self.get_logger().info('Finished sweep') - # Ignore noise points (-1) - unique_labels, counts = np.unique(labels[labels >= 0], return_counts=True) - - if len(unique_labels) == 0: - self.get_logger().info('DBSCAN found no clusters.') - color_image = sonar_image_processing.build_color_sonar_image_from_int_array(cart_grid) - return (None, color_image, normal_angle_deg) - - # Select largest cluster - largest_cluster_label = unique_labels[np.argmax(counts)] - largest_cluster_points = line_points_coords[labels == largest_cluster_label] - - # --- PCA on largest DBSCAN cluster --- - pca = PCA(n_components=2) - pca.fit(largest_cluster_points) + if self.stream: + self.raw_image_publisher.publish( + sonar_utils.convert_to_ros_compressed_img( + sonar_object_detection.SonarDenoiser(sweep).init_cartesian().cartesian, + self.cv_bridge, + ), + ) - # The center of the points is the mean - center_y, center_x = pca.mean_ # Note: pca.mean_ gives [mean_y, mean_x] - self.get_logger().info(f'Sonar: Got center of object at ({center_x:.2f}, {center_y:.2f})') + denoiser = sonar_object_detection.SonarDenoiser(sweep) + denoiser.wall_block().percentile_filter().fourier_signal_processing().init_cartesian().normalize().blur() + self.get_logger().info('Finished Denoising') - # The first principal component gives the direction of the line - # The components are the eigenvectors, pca.components_[0] is the first eigenvector [dy, dx] - direction_vector = pca.components_[0] - # Calculate the angle from the direction vector (arctan2 handles quadrants) - line_angle_rad = np.arctan2(direction_vector[0], direction_vector[1]) # arctan2(dy, dx) - line_angle_deg = np.degrees(line_angle_rad) + color_image = sonar_image_processing.build_color_sonar_image_from_int_array(denoiser.cartesian) + self.get_logger().info('Color image built') - # Normalize the angle to be within a specific range (e.g., -180 to 180) - line_angle_deg = np.clip(line_angle_deg, -180, 180) + segmentation = sonar_object_detection.GlobalSonarSegmentation( + denoiser.cartesian, + ) + self.get_logger().info('Segmented') - # Calculate the normal angle (perpendicular to the line) - normal_angle_rad = line_angle_rad + np.pi/2 - normal_angle_deg = np.degrees(normal_angle_rad) - self.get_logger().info(f'Sonar: Got normal angle at {normal_angle_deg:.2f} degrees') - # Normalize the normal angle to be within a specific range (e.g., -180 to 180) + nearest_segment = segmentation.get_nearest_segment() + if nearest_segment is None: + return (None, color_image, None, None) - else: - self.get_logger().info('Not enough line points found to apply PCA.') - color_image = sonar_image_processing.build_color_sonar_image_from_int_array(cart_grid) - return (None, color_image, normal_angle_deg) - - - # Step 3: Visualize the center point and the principal axis (representing the line) - # Overlay identified center point and principal axis onto the Cartesian grid visualization to verify the result. - h, w = cart_grid.shape - dpi = 100 # you can adjust depending on desired output scaling - plot = plt.figure(figsize=(w/dpi, h/dpi), dpi=dpi) - - plt.imshow(cart_grid, cmap='viridis', interpolation='none') # no resampling - plt.axis('off') # no ticks/axes - plt.tight_layout(pad=0) # remove padding - plt.axis('off') # Turn off axis for a clean image - - # Plot the identified center point if found - if center_x is not None and center_y is not None: - plt.plot(center_x, center_y, 'ro') # 'ro' for red circle marker - plt.text(center_x, center_y, f' Center ({center_x:.0f}, {center_y:.0f})', - color='red', fontsize=10, ha='left') - - # Plot the principal axis if PCA was applied - if len(line_points_coords) > 1: - # To plot the line, we can use the center and the direction vector - # We'll plot a line segment around the center - scale_factor = 1000 # Adjust this to make the line visible - start_point = pca.mean_ - direction_vector * scale_factor - end_point = pca.mean_ + direction_vector * scale_factor - # Note: plot takes [x_coords], [y_coords] - plt.plot([start_point[1], end_point[1]], [start_point[0], end_point[0]], 'r-', label='Principal Axis (PCA)') - plt.legend() - - fig = plt.gcf() - canvas = FigureCanvas(fig) - canvas.draw() - - image = np.frombuffer(canvas.tostring_argb(), dtype='uint8') - image = image.reshape((*fig.canvas.get_width_height()[::-1], 4)) - plot_img = cv2.cvtColor(image[:, :, 1:], cv2.COLOR_RGB2BGR) - - # Converting stuff to fit return format: - # Convert plt to array, and convert center_x and center_y to distances. - center = self.DEFAULT_NUMER_OF_SAMPLES - dx = center_x - center - dy = center_y - center - distance = np.sqrt(dx**2 + dy**2) - angle_rad = np.arctan2(dy, dx) - sample_index = round(distance) - meters = self.get_distance_of_sample(sample_index) - x_pos = meters * np.cos(angle_rad) - y_pos = meters * np.sin(angle_rad) - pose = PoseStamped() - pose.header.stamp = self.get_clock().now().to_msg() - pose.header.frame_id = 'map' - pose.pose.position.x = x_pos - pose.pose.position.y = y_pos - pose.pose.position.z = 0.0 - plot.savefig('sonar_plot.png') - self.get_logger().info(f'Sonar: Got pose at ({x_pos:.2f}, {y_pos:.2f}, 0.0)') - - return (pose, plot_img, normal_angle_deg) - - def convert_plt_to_array(self, plt_fig:plt.Figure) -> np.ndarray: - """ - Convert a Matplotlib figure to a NumPy array. - - Args: - plt_fig (plt.Figure): The Matplotlib figure to convert. - - Returns: - np.ndarray: The resulting image as a NumPy array. - """ - buf = io.BytesIO() - plt_fig.savefig(buf, format='png') - buf.seek(0) - return plt.imread(buf) + self.get_logger().info('Got segment') - def convert_to_ros_compressed_img(self, sonar_sweep: np.ndarray, compressed_format: str = 'jpg', - is_color: bool = False) -> CompressedImage: - """ - Convert any kind of image to ROS Compressed Image. - - Args: - sonar_sweep (int): numpy array of int values representing the sonar image. - compressed_format (string): format to compress the image to. - is_color (bool): Whether the image is color or not. - - Returns: - CompressedImage: ROS Compressed Image message. - """ - if not is_color: - sonar_sweep = sonar_image_processing.build_color_sonar_image_from_int_array(sonar_sweep) - return self.cv_bridge.cv2_to_compressed_imgmsg(sonar_sweep, dst_format=compressed_format) + if self.stream: + msg = Float32(data=float(np.arctan(nearest_segment.ortho_regression.slope)+np.pi / 4.)) + self.wall_angle_publisher.publish(msg) + + x_index, y_index = nearest_segment.get_average_coordinate_of_points() + normal_angle = ( + np.arctan2( + nearest_segment.ortho_regression.unit_normal[1], + nearest_segment.ortho_regression.unit_normal[0], + ) + - np.pi / 4. + ) + + self.get_logger().info(f'x: {x_index}, y: {y_index}, normal: {normal_angle}') + + return ( + sonar_utils.to_robot_position(x_index, y_index, self.sample_period, self.NEGATE_POSE), + color_image, + normal_angle, + np.arctan(nearest_segment.ortho_regression.slope)+np.pi / 4.) def constant_sweep(self) -> None: """ @@ -518,16 +268,21 @@ def constant_sweep(self) -> None: try: self.get_logger().info(f'Starting sweep from {self.CONSTANT_SWEEP_START} to {self.CONSTANT_SWEEP_END}') sonar_sweep = self.get_sweep(self.CONSTANT_SWEEP_START, self.CONSTANT_SWEEP_END) - self.get_logger().info('Finishng sweep') + self.get_logger().info('Finishing sweep') if self.stream: - compressed_image = self.convert_to_ros_compressed_img(sonar_sweep) - self.sonar_image_publisher.publish(compressed_image) + self.raw_image_publisher.publish( + sonar_utils.convert_to_ros_compressed_img( + sonar_object_detection.SonarDenoiser(sonar_sweep).init_cartesian().cartesian, + self.cv_bridge, + ), + ) except (RuntimeError, ValueError) as e: self.get_logger().error(f'Error during constant sweep: {e}') rclpy.shutdown() - def perform_sonar_request(self, request: SonarSweepRequest.Request, response: SonarSweepRequest.Response) -> \ - SonarSweepRequest.Response: + def perform_sonar_request( + self, request: SonarSweepRequest.Request, response: SonarSweepRequest.Response, + ) -> SonarSweepRequest.Response: """ Perform a sonar request. @@ -560,13 +315,12 @@ def perform_sonar_request(self, request: SonarSweepRequest.Request, response: So return response left_gradians = sonar_utils.degrees_to_centered_gradians(start_degrees, self.center_gradians, self.increase_ccw) - right_gradians = sonar_utils.degrees_to_centered_gradians(end_degrees, self.center_gradians, - self.increase_ccw) + right_gradians = sonar_utils.degrees_to_centered_gradians(end_degrees, self.center_gradians, self.increase_ccw) self.get_logger().info(f'Recieved Sonar request: {left_gradians}, {right_gradians}, {new_range}') # Angle must be between 0 and 400 and range must be positive - if left_gradians < 0 or right_gradians < 0 or right_gradians > 400 or new_range < 0: # noqa: PLR2004 + if left_gradians < 0 or right_gradians < 0 or right_gradians > 400 or new_range < 0: # noqa: PLR2004 self.get_logger().error('Bad sonar request') return response @@ -574,16 +328,23 @@ def perform_sonar_request(self, request: SonarSweepRequest.Request, response: So self.set_new_range(new_range) try: - object_pose, plot, normal_angle = self.get_xy_of_object_in_sweep(left_gradians, right_gradians) + object_pose, plot, normal_angle, wall_angle = self.get_xy_of_object_in_sweep( + left_gradians, + right_gradians, + ) + self.get_logger().debug('Finished xy_of_object') except RuntimeError as e: response.success = False response.message = str(e) return response if object_pose is not None: - response.pose = object_pose + response.pose.pose = object_pose + response.pose.header.stamp = self.get_clock().now().to_msg() response.normal_angle = normal_angle response.is_object = True + response.pose.header.frame_id = 'robot_sonar' + response.angle_of_wall = wall_angle if object_pose is None: self.get_logger().error('No object found') @@ -592,11 +353,10 @@ def perform_sonar_request(self, request: SonarSweepRequest.Request, response: So response.message = 'Found object.' if self.stream: - sonar_image = self.convert_to_ros_compressed_img(plot, is_color=True) + sonar_image = sonar_utils.convert_to_ros_compressed_img(plot, self.cv_bridge, is_color=True) self.sonar_image_publisher.publish(sonar_image) response.success = True - return response def run(self) -> None: @@ -606,6 +366,7 @@ def run(self) -> None: else: self.create_service(SonarSweepRequest, 'sonar/request', self.perform_sonar_request) + def main(args: list[str] | None = None) -> None: """Initialize and run the Sonar node.""" rclpy.init(args=args) @@ -620,5 +381,6 @@ def main(args: list[str] | None = None) -> None: if rclpy.ok(): rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/onboard/src/sonar/sonar/sonar_image_processing.py b/onboard/src/sonar/sonar/sonar_image_processing.py index 876cf47c..f1d33432 100644 --- a/onboard/src/sonar/sonar/sonar_image_processing.py +++ b/onboard/src/sonar/sonar/sonar_image_processing.py @@ -8,13 +8,15 @@ from rclpy.logging import get_logger from sklearn.cluster import DBSCAN from sklearn.linear_model import LinearRegression +from sklearn.mixture import GaussianMixture from sonar import decode_ping_python_360 SONAR_IMAGE_WIDTH = 16 SONAR_IMAGE_HEIGHT = 2 +MIN_DIMENSIONS_FOR_SEGMENTATION = 2 -def build_color_sonar_image_from_int_array(int_array: list, npy_save_path: str | None = None, +def build_color_sonar_image_from_int_array(int_array: np.ndarray, npy_save_path: str | None = None, jpeg_save_path: str | None = None) -> np.ndarray: """ Build a sonar image from a list of data messages. @@ -30,14 +32,17 @@ def build_color_sonar_image_from_int_array(int_array: list, npy_save_path: str | Returns: ndarray: Sonar image from the scan. """ - sonar_img = cv2.cvtColor(int_array.astype(np.uint8), cv2.COLOR_GRAY2BGR) - sonar_img = cv2.applyColorMap(sonar_img, cv2.COLORMAP_VIRIDIS) + sonar_img = np.stack([int_array, int_array, int_array], axis=-1) if jpeg_save_path: plt.imsave(jpeg_save_path, sonar_img) if npy_save_path: np.save(npy_save_path, sonar_img) - return sonar_img + sonar_img = sonar_img - np.min(sonar_img) + max_val = np.max(sonar_img) + sonar_img = np.zeros_like(sonar_img) if max_val == 0 else sonar_img / max_val * 255 + + return sonar_img.astype(np.uint8) def find_center_point_and_angle(array: np.ndarray, threshold: int, eps: float, min_samples: int, get_plot: bool = True) -> tuple: @@ -197,3 +202,31 @@ def build_sonar_image(data_list: list, display_results: bool = False, npy_save_p cv2.waitKey(0) return sonar_img + +def sonar_gaussian_mixture_model_cluster(sonar_data: np.ndarray) -> np.ndarray: + """ + Cluster a sonar scan into background, walls, and buoys using GMM clustering. Adapted from Pranav Bijith's GMM code. + + Args: + sonar_data (ndarray): a sonar scan in cartesian coordinates which may contain nothing, walls, and buoys + + Returns: + ndarray: ndarray of sonar_data segmented into three categories: nothing, walls, and buoys + """ + finalcopygrid = sonar_data.copy() + finalcopygrid[finalcopygrid != 0] = 255 + h, w = finalcopygrid.shape + x = np.column_stack((finalcopygrid.reshape(-1), np.repeat(np.arange(h), w), np.tile(np.arange(w), h))) + mask = finalcopygrid.reshape(-1) != 0 + x_masked = x[mask] + + if x_masked.shape[0] >= MIN_DIMENSIONS_FOR_SEGMENTATION: + gmm = GaussianMixture(n_components=2, random_state=42) + gmm.fit(x_masked) + cluster_labels = np.full(mask.shape, -1) + cluster_labels[mask] = gmm.predict(x_masked) + else: + print('No nonzero pixels found — skipping GMM') + cluster_labels = np.full(mask.shape, -1) + + return cluster_labels.reshape(finalcopygrid.shape) diff --git a/onboard/src/sonar/sonar/sonar_object_detection.py b/onboard/src/sonar/sonar/sonar_object_detection.py new file mode 100644 index 00000000..ba05a131 --- /dev/null +++ b/onboard/src/sonar/sonar/sonar_object_detection.py @@ -0,0 +1,317 @@ +from enum import Enum + +import numpy as np +from scipy.signal import convolve2d + +NUM_DIMENSIONS_FOR_REPEAT = 3 + + +class SonarDenoiser: + """Class to denoise sonar scans to prepare them for segmentation and pose estimation.""" + + def __init__(self, data: np.ndarray) -> None: + """ + Construct a SonarDenoising object. + + Args: + data (ndarray): data in gradian space + """ + self.data = data + self.shape_theta = min(100, self.data.shape[0]) + self.shape_radius = self.data.shape[1] + + # Reshape data + processed_data = np.zeros( + shape=(100, np.floor(self.shape_radius * 1.41421356).astype(int)), + ) + processed_data[: self.shape_theta, : self.shape_radius] = self.data[: self.shape_theta] + + self.data = processed_data + self.cartesian: np.ndarray + + def wall_block(self, threshold: float = 0.95) -> 'SonarDenoiser': + """ + Remove signal behind a known wall. + + This follows the justification that any signal behind a known object + is noise. + + Args: + threshold (float): the threshold to consider some signal as a "known object". + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + for theta in range(self.shape_theta): + max_along_theta = 0 + for r in range(self.shape_radius): + if self.data[theta][r] > max_along_theta * threshold: + max_along_theta = self.data[theta][r] + else: + self.data[theta][r] = 0 + return self + + def percentile_filter(self, threshold: float = 0.7) -> 'SonarDenoiser': + """ + Apply percentile filtering to reduce noise. + + Args: + threshold (float): the threshold for percentile filtering. + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + nonzero_data = self.data[self.data > 0] + if nonzero_data.size == 0: + return self + + threshold = float(np.percentile(np.percentile(nonzero_data, threshold), threshold)) + self.data[self.data < threshold] = 0 + + return self + + def fourier_signal_processing( + self, + inner_radius: float = 0.001, + outer_radius: float = 0.25, + threshold: float = 40, + ) -> 'SonarDenoiser': + """ + Denoise a sonar scan using the Fast Fourier Transform. Adapted from Pranav Bijith's Fourier analysis. + + Args: + data (ndarray): an ndarray representing the sonar data + inner_radius (float): the radius of a circle in the frequency domain, all signal within will be removed + outer_radius (float): the radius of a circle in the frequency domain, all signal without will be removed + threshold (float): the threshhold + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + xv, yv = np.meshgrid(np.fft.fftfreq(self.data.shape[1]), np.fft.fftfreq(self.data.shape[0])) + xv = np.fft.fftshift(xv) + yv = np.fft.fftshift(yv) + + # Applies the Radial Mask + radius = np.sqrt(xv**2 + yv**2) + mask = (radius < outer_radius) & (radius >= inner_radius) + mask = mask.astype(np.float32) + if self.data.ndim == NUM_DIMENSIONS_FOR_REPEAT and self.data.shape[2] == NUM_DIMENSIONS_FOR_REPEAT: + mask = np.repeat(mask[:, :, np.newaxis], 3, axis=2) + fimg = np.fft.fftshift(np.fft.fft2(self.data, axes=(0, 1))) * mask + + # Filter + self.data = np.fft.ifft2(np.fft.ifftshift(fimg)) + self.data = np.abs(self.data) + self.data[self.data < threshold] = 0 + + # Return self + return self + + def init_cartesian(self) -> 'SonarDenoiser': + """ + Update cartesian data based on gradian data. + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + shape_array = np.arange(self.shape_radius) + x, y = np.meshgrid(shape_array, shape_array) + + theta = np.zeros(shape=(self.shape_radius, self.shape_radius), dtype=x.dtype) + theta[:, 0] = 89 # x=0 + theta[:, 1:] = np.arctan(y[:, 1:] / x[:, 1:]) / np.pi * 180 + theta_gradians = (theta / 90 * 100).astype(int) + r = (np.floor(np.sqrt(x**2 + y**2))).astype(int) + + self.cartesian = self.data[theta_gradians, r] + return self + + def normalize(self) -> 'SonarDenoiser': + """ + Normalize the cartesian image. + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + self.cartesian = self.cartesian - np.min(self.cartesian) + max_value = np.max(self.cartesian) + if np.isclose(max_value, 0): + return self + self.cartesian = self.cartesian / max_value + + return self + + def blur(self, factor: int = 16) -> 'SonarDenoiser': + """ + Apply box blur onto cartesian image. + + Args: + factor (int): the size of the box for the box blur. + + Returns: + SonarDenoiser: returns itself to allow for method chaining. + """ + blur_kernel = np.ones((factor, factor), np.float32) / (factor**2) + self.cartesian = convolve2d(self.cartesian, blur_kernel, mode='same', boundary='symm') + + self.normalize() + + self.cartesian = np.where(self.cartesian > 1 / 5, self.cartesian, 0) + + return self + + +class OrthogonalRegression: + """A class representing the Orthogonal Regression of a group of points.""" + + def __init__(self, points: np.ndarray) -> None: + """ + Construct a OrthogonalRegression object given a group of points. + + Args: + points (ndarray): the points within this orthogonal regression. Points should be (y, x) + """ + self.points = points + + e_val, e_vect = np.linalg.eig(np.cov(self.points, rowvar=False)) + + self.unit_tangent = e_vect[:, np.argmin(e_val)] + self.unit_tangent[1] *= -1 + self.unit_normal = np.array([-self.unit_tangent[1], self.unit_tangent[0]]) + + if self.unit_tangent[0] == 0: + self.slope = (2**31) - 1 + else: + self.slope = self.unit_tangent[1] / self.unit_tangent[0] + + self.intercept = self.points[:, 0].mean() - self.slope * self.points[:, 1].mean() + + self.orthogonal_projections = np.matmul( + np.dot(self.points - np.array([self.intercept, 0]), self.unit_tangent[::-1])[:, np.newaxis], + self.unit_tangent[np.newaxis, ::-1], + ) + self.residual_vectors = self.points - np.array([self.intercept, 0]) - self.orthogonal_projections + + residuals = np.linalg.norm(self.residual_vectors, axis=1) + + self.mse = np.sum(np.square(residuals)) / residuals.shape[0] + self.r2 = 1 - np.sum(np.square(residuals)) / (np.sum(np.square(self.points[:, 0] - np.mean(self.points[:, 0])))) + + def y_given_x(self, x: float) -> float: + """ + Get a value of y for some input of x. + + Args: + x (float): The input for x. + + Returns: + float: The value of y in this regression given a value of x. + """ + return self.slope * x + self.intercept + + def x_given_y(self, y: float) -> float: + """ + Get a value of x for some input of y. + + Args: + y (float): The input for y. + + Returns: + float: The value of x in this regression given a value of y. + """ + return (y - self.intercept) / self.slope + + def set_slope(self, value: float) -> None: + """ + Set the slope of this orthogonal regression. + + Args: + value (float): the new slope for the regression. + """ + if value == 0: + self._slope = np.finfo(type(value)).tiny + else: + self._slope = value + + +class SonarSegmentType(Enum): + """Enum for Sonar Segment types.""" + + NONE = 0 + WALL = 1 + OBJECT = 2 + + +class SonarSegment: + """Class to define a sonar segment segment.""" + + def __init__(self, points: np.ndarray) -> None: + """ + Construct a SonarSegment object. + + Args: + points (ndarray): the points which make up this segment. + """ + self.number = -1 + self.points = points + self.ortho_regression: OrthogonalRegression + self.wall_distance = -1 + self.nearest_object = None + self.nearest_object_distance = max(points.shape[0], points.shape[1]) * 2 + self.type = SonarSegmentType.NONE + + def get_average_coordinate_of_points(self) -> tuple[int, int]: + """ + Get the average (row, col) of the points in this SonarSegment. + + Returns: + tuple(int, int): the coordinates of the average point. + """ + coordinates = np.zeros(2) + for point in self.points: + coordinates = coordinates + point + + coordinates = coordinates / self.points.shape[0] + + return (int(np.round(coordinates[0])), int(np.round(coordinates[1]))) + +class GlobalSonarSegmentation: + """A class which treats all non-zero sonar data as a single segment.""" + + def __init__( + self, + image: np.ndarray, + ) -> None: + + # Store image + self.image = image + self.side_length = image.shape[0] + + # Get ALL non-zero pixels + points = np.argwhere(image > 0) + + if points.shape[0] == 0: + self.raw_segments = [] + self.segments = [] + self.walls = [] + self.objects = [] + return + + # Create single segment + segment = SonarSegment(points) + segment.number = 1 + segment.ortho_regression = OrthogonalRegression(segment.points) + + # Everything is now one segment + self.raw_segments = [segment] + self.segments = [segment] + + def get_nearest_segment(self) -> 'SonarSegment': + """ + Get the nearest segment. + + Returns: + SonarSegment: the nearest segment. + """ + return self.segments[0] diff --git a/onboard/src/sonar/sonar/sonar_utils.py b/onboard/src/sonar/sonar/sonar_utils.py index fda03daf..b24497fe 100644 --- a/onboard/src/sonar/sonar/sonar_utils.py +++ b/onboard/src/sonar/sonar/sonar_utils.py @@ -2,12 +2,22 @@ import rclpy import tf2_geometry_msgs import tf2_ros +from cv_bridge import CvBridge +from geometry_msgs.msg import Pose +from sensor_msgs.msg import CompressedImage + +from sonar import sonar_image_processing RADIANS_PER_GRADIAN = np.pi / 200 GRADIANS_PER_DEGREE = 400 / 360 +SPEED_OF_SOUND_IN_WATER = 1482 # m/s +SAMPLE_PERIOD_TICK_DURATION = 25e-9 # s +TRANSFORMATION_ANGLE = np.pi / 4 + -def transform_pose(buffer: tf2_ros.Buffer, pose: tf2_geometry_msgs.PoseStamped, - source_frame_id: str, target_frame_id: str) -> tf2_geometry_msgs.PoseStamped: +def transform_pose( + buffer: tf2_ros.Buffer, pose: tf2_geometry_msgs.PoseStamped, source_frame_id: str, target_frame_id: str, +) -> tf2_geometry_msgs.PoseStamped: """ Transform pose from source reference frame to target reference frame. @@ -23,14 +33,20 @@ def transform_pose(buffer: tf2_ros.Buffer, pose: tf2_geometry_msgs.PoseStamped, try: # Wait for transform to be available transform = buffer.lookup_transform( - target_frame_id, source_frame_id, rclpy.time.Time(), + target_frame_id, + source_frame_id, + rclpy.time.Time(), ) # Transform the pose return tf2_geometry_msgs.do_transform_pose(pose, transform) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, - tf2_ros.InvalidArgumentException) as e: + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + tf2_ros.InvalidArgumentException, + ) as e: error_message = f'Failed to transform pose: {e}' raise RuntimeError(error_message) from e @@ -66,35 +82,148 @@ def degrees_to_centered_gradians(angle_degrees: float, center_gradians: float, i angle_gradians_centered = angle_gradians + center_gradians return int(angle_gradians_centered) -def polar2cart(sonar_sweep: np.ndarray, deg_low: int, deg_high: int) -> np.ndarray: + +def range_to_period(sonar_range: int, number_of_samples: int) -> int: + """ + From a given range determines the sample_period. + + Sample_period is the time between each sample. Given a distance we can calculate the sample period using the + formula: 2 * range / (number_of_samples * speed_of_sound_in_water * 25e-9) where number of samples is the + number of samples taken between 0m and the set range, speed of sound in water is 1480m/s and 25e-9 is the + sample period tick duration. + + https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3 + + Args: + sonar_range (int): max range in meters of the sonar scan. + number_of_samples (int): number of samples taken between 0m and the set range + + Returns: + sample_period (int): sample period in ms. + """ + period = 2 * sonar_range / (number_of_samples * SPEED_OF_SOUND_IN_WATER * SAMPLE_PERIOD_TICK_DURATION) + return round(period) + + +def range_to_transmit(sonar_range: int, number_of_samples: int) -> int: + """ + From a given range determines the transmit_duration. + + Per firmware engineer: + 1. Starting point is TxPulse in usec = ((one-way range in metres) * 8000) / + (Velocity of sound in metres per second) + 2. Then check that TxPulse is wide enough for currently selected sample interval in usec, + i.e., if TxPulse < (2.5 * sample interval) then TxPulse = (2.5 * sample interval) + 3. Perform limit checking + https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3 + + Args: + sonar_range (int): max range in meters of the sonar scan. + number_of_samples (int): number of samples taken between 0m and the set range + + Returns: + transmit_duration (int): max transmit duration in ms. + """ + # 1 + transmit_duration = 8000 * sonar_range / SPEED_OF_SOUND_IN_WATER + # 2 (transmit duration is microseconds, samplePeriod() is nanoseconds) + transmit_duration = max(range_to_period(sonar_range, number_of_samples) / 40, transmit_duration) + # 3 min_transmit is 5 and max_transmit is 500 + return round(max(5, min(500, transmit_duration))) + + +def meters_per_sample(sample_period: int) -> float: """ - Convert a polar sonar sweep to a Cartesian grid. + Return the target distance per sample, in meters. + + https://discuss.bluerobotics.com/t/access-ping360-data-for-post-processing-python/10416/2 Args: - sonar_sweep (np.ndarray): Polar sonar sweep data. - deg_low (int): Lower bound of the angle in degrees. - deg_high (int): Upper bound of the angle in degrees. + sample_period (int): the sample period of the ping360 Returns: - np.ndarray: Cartesian grid representation of the sonar sweep. - """ - num_angles, num_samples = sonar_sweep.shape - print(num_angles, num_samples) - grid_size = 2 * (num_samples - 1) + 1 - center = num_samples - cartesian_grid = np.zeros((grid_size, grid_size), dtype=float) - - angles = np.linspace(deg_low, deg_high, num_angles, endpoint=True) - - for i, angle_deg in enumerate(angles): - angle_rad = angle_deg * np.pi / 200.0 - for j in range(num_samples): - distance = j - x = round(center + distance * np.cos(angle_rad)) # negative bcs idk why lol - y = round(center + distance * np.sin(angle_rad)) - val = sonar_sweep[i, j] - intensity = val - if 0 <= x < grid_size and 0 <= y < grid_size: - cartesian_grid[y, x] = max(cartesian_grid[y, x], intensity) - - return cartesian_grid + float: Distance per sample. + """ + # sample_period is in 25ns increments + # time of flight includes there and back, so divide by 2 + return SPEED_OF_SOUND_IN_WATER * sample_period * SAMPLE_PERIOD_TICK_DURATION / 2.0 + + +def get_distance_of_sample(sample_period: float, sample_index: int) -> float: + """ + Get the distance in meters of a sample given its index in the data array returned from the device. + + Computes distance using formula from + https://bluerobotics.com/learn/understanding-and-using-scanning-sonars/. + + Args: + sample_period: the sample period of the Ping360 + sample_index (int | float): Index of the sample in the data array, from 0 to N-1, + where N = number of samples. + + Returns: + float: Distance in meters of the sample from the sonar device. + """ + # 0.5 for the average distance of sample + return (sample_index + 0.5) * meters_per_sample(sample_period) + + +def to_robot_position(x_index: int, y_index: int, sample_period: float, negate: bool) -> Pose: + """ + Convert a point in sonar space to a robot global position. + + Args: + x_index (int | float): x-index of the data in the sonar response. + y_index (int | float): y-index of the data in the sonar response. + sample_period (int): the sample period of the ping360 + negate (bool): whether to negate the pose or not + + Returns: + Pose: Pose in target_frame_id containing x and y position of angle/index item. + """ + x_pos = get_distance_of_sample( + sample_period, + x_index, + ) + y_pos = get_distance_of_sample( + sample_period, + y_index, + ) + + adjusted_x = (x_pos * np.cos(TRANSFORMATION_ANGLE) + y_pos * np.sin(TRANSFORMATION_ANGLE)) \ + * (-1.0 if negate else 1.0) + adjusted_y = (-x_pos * np.sin(TRANSFORMATION_ANGLE) + y_pos * np.cos(TRANSFORMATION_ANGLE)) \ + * (-1.0 if negate else 1.0) + + yaw = np.arctan2(adjusted_y, adjusted_x) + + pos_of_point = Pose() + pos_of_point.position.x = adjusted_x + pos_of_point.position.y = adjusted_y + pos_of_point.position.z = 0.0 # z cord is not really 0 but we don't care + pos_of_point.orientation.x = 0.0 + pos_of_point.orientation.y = 0.0 + pos_of_point.orientation.z = np.sin(yaw / 2) + pos_of_point.orientation.w = np.cos(yaw / 2) + + return pos_of_point + + +def convert_to_ros_compressed_img( + sonar_sweep: np.ndarray, cv_bridge: CvBridge, compressed_format: str = 'jpeg', is_color: bool = False, +) -> CompressedImage: + """ + Convert any kind of image to ROS Compressed Image. + + Args: + sonar_sweep (int): numpy array of int values representing the sonar image. + cv_bridge: the CV Bridge to use to convert to CompressedImage. + compressed_format (string): format to compress the image to. + is_color (bool): Whether the image is color or not. + + Returns: + CompressedImage: ROS Compressed Image message. + """ + if not is_color: + sonar_sweep = sonar_image_processing.build_color_sonar_image_from_int_array(sonar_sweep) + return cv_bridge.cv2_to_compressed_imgmsg(sonar_sweep, dst_format=compressed_format) diff --git a/onboard/src/sonar/sweep_data/01-wall_close.npy b/onboard/src/sonar/sweep_data/01-wall_close.npy new file mode 100644 index 00000000..8d25957c Binary files /dev/null and b/onboard/src/sonar/sweep_data/01-wall_close.npy differ diff --git a/onboard/src/sonar/sweep_data/02-wall_farther.npy b/onboard/src/sonar/sweep_data/02-wall_farther.npy new file mode 100644 index 00000000..ae76ab99 Binary files /dev/null and b/onboard/src/sonar/sweep_data/02-wall_farther.npy differ diff --git a/onboard/src/sonar/sweep_data/03-wall_farthest.npy b/onboard/src/sonar/sweep_data/03-wall_farthest.npy new file mode 100644 index 00000000..bae17971 Binary files /dev/null and b/onboard/src/sonar/sweep_data/03-wall_farthest.npy differ diff --git a/onboard/src/sonar/sweep_data/04-wall_wider_frame.npy b/onboard/src/sonar/sweep_data/04-wall_wider_frame.npy new file mode 100644 index 00000000..33a981fd Binary files /dev/null and b/onboard/src/sonar/sweep_data/04-wall_wider_frame.npy differ diff --git a/onboard/src/sonar/sweep_data/05-nothing.npy b/onboard/src/sonar/sweep_data/05-nothing.npy new file mode 100644 index 00000000..9e0e8cc8 Binary files /dev/null and b/onboard/src/sonar/sweep_data/05-nothing.npy differ diff --git a/onboard/src/sonar/sweep_data/06-wall_at_angle.npy b/onboard/src/sonar/sweep_data/06-wall_at_angle.npy new file mode 100644 index 00000000..75410576 Binary files /dev/null and b/onboard/src/sonar/sweep_data/06-wall_at_angle.npy differ diff --git a/onboard/src/sonar/sweep_data/07-closer_wall_at_angle.npy b/onboard/src/sonar/sweep_data/07-closer_wall_at_angle.npy new file mode 100644 index 00000000..61647444 Binary files /dev/null and b/onboard/src/sonar/sweep_data/07-closer_wall_at_angle.npy differ diff --git a/onboard/src/sonar/sweep_data/08-corner.npy b/onboard/src/sonar/sweep_data/08-corner.npy new file mode 100644 index 00000000..9a3f1010 Binary files /dev/null and b/onboard/src/sonar/sweep_data/08-corner.npy differ diff --git a/onboard/src/sonar/sweep_data/09-closer_corner.npy b/onboard/src/sonar/sweep_data/09-closer_corner.npy new file mode 100644 index 00000000..a880fde8 Binary files /dev/null and b/onboard/src/sonar/sweep_data/09-closer_corner.npy differ diff --git a/onboard/src/sonar/sweep_data/10-wall.npy b/onboard/src/sonar/sweep_data/10-wall.npy new file mode 100644 index 00000000..f6325023 Binary files /dev/null and b/onboard/src/sonar/sweep_data/10-wall.npy differ diff --git a/onboard/src/sonar/sweep_data/11-buoy_wall.npy b/onboard/src/sonar/sweep_data/11-buoy_wall.npy new file mode 100644 index 00000000..6aaef915 Binary files /dev/null and b/onboard/src/sonar/sweep_data/11-buoy_wall.npy differ diff --git a/onboard/src/sonar/sweep_data/12-buoy_alone.npy b/onboard/src/sonar/sweep_data/12-buoy_alone.npy new file mode 100644 index 00000000..9cd4c449 Binary files /dev/null and b/onboard/src/sonar/sweep_data/12-buoy_alone.npy differ diff --git a/onboard/src/sonar/sweep_data/13-tarp_unreliable.npy b/onboard/src/sonar/sweep_data/13-tarp_unreliable.npy new file mode 100644 index 00000000..151be790 Binary files /dev/null and b/onboard/src/sonar/sweep_data/13-tarp_unreliable.npy differ diff --git a/onboard/src/sonar/sweep_data/14-tarp_unreliable_2.npy b/onboard/src/sonar/sweep_data/14-tarp_unreliable_2.npy new file mode 100644 index 00000000..d3d4ccc5 Binary files /dev/null and b/onboard/src/sonar/sweep_data/14-tarp_unreliable_2.npy differ diff --git a/onboard/src/sonar/sweep_data/15.npy b/onboard/src/sonar/sweep_data/15.npy new file mode 100644 index 00000000..8622b08e Binary files /dev/null and b/onboard/src/sonar/sweep_data/15.npy differ diff --git a/onboard/src/sonar/sweep_data/16-wall_far_away_(got_a_little_side_wall).npy b/onboard/src/sonar/sweep_data/16-wall_far_away_(got_a_little_side_wall).npy new file mode 100644 index 00000000..3cbb6ceb Binary files /dev/null and b/onboard/src/sonar/sweep_data/16-wall_far_away_(got_a_little_side_wall).npy differ diff --git a/onboard/src/sonar/sweep_data/data.jpeg b/onboard/src/sonar/sweep_data/data.jpeg new file mode 100644 index 00000000..14b87b70 Binary files /dev/null and b/onboard/src/sonar/sweep_data/data.jpeg differ diff --git a/onboard/src/sonar/sweep_data/newdata.jpeg b/onboard/src/sonar/sweep_data/newdata.jpeg new file mode 100644 index 00000000..200e42b5 Binary files /dev/null and b/onboard/src/sonar/sweep_data/newdata.jpeg differ diff --git a/onboard/src/task_planning/task_planning/interface/cv.py b/onboard/src/task_planning/task_planning/interface/cv.py index 690b6609..62103927 100644 --- a/onboard/src/task_planning/task_planning/interface/cv.py +++ b/onboard/src/task_planning/task_planning/interface/cv.py @@ -383,4 +383,3 @@ def get_sonar_sweep_params(self, name: CVObjectType) -> tuple[float, float, floa data = self._bounding_boxes[name] return (data.sonar_start_angle, data.sonar_end_angle, data.sonar_scan_distance) - diff --git a/onboard/src/task_planning/task_planning/robot/oogway.py b/onboard/src/task_planning/task_planning/robot/oogway.py index cbaff9ae..3f888d9e 100644 --- a/onboard/src/task_planning/task_planning/robot/oogway.py +++ b/onboard/src/task_planning/task_planning/robot/oogway.py @@ -1,4 +1,4 @@ -# ruff: noqa: ERA001, F401, N806 +# ruff: noqa: ERA001, F401, N806, F841 from math import radians from task_planning.interface.cv import CVObjectType @@ -8,7 +8,6 @@ from task_planning.tasks import ( buoyancy_tasks, comp_tasks, - ivc_tasks, move_tasks, prequal_tasks, servos_tasks, @@ -22,16 +21,17 @@ async def main(self: Task) -> Task[None, None, None]: """Run the tasks to be performed by Oogway.""" # Constants DIRECTION_OF_TORPEDO_BANNER = 1 - DEPTH = 0.7 + DEPTH = 0.5 # CVObjectType.TORPEDO_REEF_SHARK_TARGET or CVObjectType.TORPEDO_SAWFISH_TARGET FIRST_TARGET = CVObjectType.TORPEDO_REEF_SHARK_TARGET tasks = [ ######## Main competition tasks ######## # ivc_tasks.delineate_ivc_log(parent=self), comp_tasks.initial_submerge(DEPTH, parent=self), + # move_tasks.move_with_directions([(1, 0, 0), (0, 0.5, 0), (-1, 0, 0), (0, -0.5, 0)], parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=-DEPTH, parent=self), - comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH, - direction=DIRECTION_OF_TORPEDO_BANNER, parent=self), + # comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH, + # direction=DIRECTION_OF_TORPEDO_BANNER, parent=self), # TODO: task not found??? # comp_tasks.send_torpedo_ivc(parent=self), # comp_tasks.octagon_task(direction=1, parent=self), @@ -79,6 +79,11 @@ async def main(self: Task) -> Task[None, None, None]: ######## Prequal tasks ######## # prequal_tasks.prequal_task(parent=self), + + ######## Sonar tasks ######## + # sonar_tasks.sonar_test(-45., 45., 10., parent=self) + sonar_tasks.rotate_to_normal(-45., 45., 10., 0.01, parent=self), + # sonar_tasks.rotate_to_angle_from_normal(-45., 45., 5., 5., 3.1415/6., parent=self), ] for task_to_run in tasks: diff --git a/onboard/src/task_planning/task_planning/task.py b/onboard/src/task_planning/task_planning/task.py index 16b93204..a7a4a7bd 100644 --- a/onboard/src/task_planning/task_planning/task.py +++ b/onboard/src/task_planning/task_planning/task.py @@ -13,6 +13,7 @@ if TYPE_CHECKING: from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile +from rclpy.task import Future from std_msgs.msg import Header from task_planning.message_conversion.jsonpickle_custom_handlers import register_custom_jsonpickle_handlers @@ -349,10 +350,12 @@ def __await__(self) -> Generator[YieldType, SendType, ReturnType]: input_ = None output = None while not self._done: - # Yield output and accept input only if the coroutine has been started if self._started: input_ = (yield output) output = self.send(input_) + while isinstance(output, Future) and not output.done() and not self._done: + yield output # keep yielding the same future until rclpy resolves it + input_ = None # after future resolves, send None to continue return output diff --git a/onboard/src/task_planning/task_planning/tasks/sonar_tasks.py b/onboard/src/task_planning/task_planning/tasks/sonar_tasks.py index bd52c1ff..116ee801 100644 --- a/onboard/src/task_planning/task_planning/tasks/sonar_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/sonar_tasks.py @@ -1,15 +1,17 @@ -from typing import TYPE_CHECKING, cast +from typing import cast +import numpy as np from custom_msgs.srv import SonarSweepRequest from rclpy.logging import get_logger from task_planning.interface.sonar import Sonar from task_planning.task import Task, task - -if TYPE_CHECKING: - from custom_msgs.srv import SonarSweepRequest +from task_planning.tasks.move_tasks import create_twist_tolerance, move_to_pose_local +from task_planning.utils import geometry_utils logger = get_logger('sonar_tasks') +MAX_STEPS = 3 + @task async def sonar_test(_self: Task, start_angle: float, end_angle: float, scan_distance: float) -> Task[None, None, None]: @@ -26,3 +28,128 @@ async def sonar_test(_self: Task, start_angle: float, end_angle: float, scan_dis else: service_response = cast('SonarSweepRequest.Response', await future) logger.info(f'Sonar scan response: {service_response}') + + +@task +async def rotate_to_normal(self: Task, + start_angle: float, + end_angle: float, + scan_distance: float, + yaw_threshold: float) -> Task[None, None, None]: + """Rotates to face a normal angle.""" + logger.info(f'Sonar scan from {start_angle} to {end_angle} degrees, distance: {scan_distance} m') + future = Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ) + normal_angle = get_normal_angle( + await future, + ) + logger.info(f'Initial Normal Angle: {normal_angle}') + if np.isnan(normal_angle): + logger.error('Normal angle does not exist, exiting task.') + return + + await move_to_pose_local( + geometry_utils.create_pose(0, 0, 0, 0, 0, -normal_angle), + keep_orientation=True, + pose_tolerances=create_twist_tolerance(angular_yaw=0.1), + parent=self, + ) + + normal_angle = get_normal_angle( + await Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ), + ) + steps = 0 + while abs(normal_angle) > yaw_threshold and steps < MAX_STEPS: + await move_to_pose_local( + geometry_utils.create_pose(0, 0, 0, 0, 0, -normal_angle), + keep_orientation=True, + pose_tolerances=create_twist_tolerance(angular_yaw=0.1), + parent=self, + ) + normal_angle = get_normal_angle( + await Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ), + ) + steps += 1 + logger.info(f'Normal Angle {normal_angle} at step {steps}') + +@task +async def rotate_to_angle_from_normal(self: Task, + start_angle: float, + end_angle: float, + scan_distance: float, + yaw_threshold: float, + rotated_angle: float) -> Task[None, None, None]: + """Rotates to a specified angle using Sonar normal angle.""" + logger.info(f'Sonar scan from {start_angle} to {end_angle} degrees, distance: {scan_distance} m') + + angle = get_normal_angle( + await Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ), + ) + if np.isnan(angle): + logger.error('Normal angle does not exist, exiting task.') + return + + angle = rotated_angle + angle + logger.info(f'Initial Angle: {angle}') + + await move_to_pose_local( + geometry_utils.create_pose(0, 0, 0, 0, 0, -angle), + keep_orientation=True, + pose_tolerances=create_twist_tolerance(angular_yaw=0.1), + parent=self, + ) + angle = get_normal_angle( + await Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ), + ) + angle = rotated_angle + angle + steps = 0 + while abs(angle) > yaw_threshold and steps < MAX_STEPS: + await move_to_pose_local( + geometry_utils.create_pose(0, 0, 0, 0, 0, -angle), + keep_orientation=True, + pose_tolerances=create_twist_tolerance(angular_yaw=0.1), + parent=self, + ) + angle = get_normal_angle( + await Sonar().sweep( + start_angle=start_angle, + end_angle=end_angle, + scan_distance=scan_distance, + ), + ) + angle = rotated_angle + angle + steps += 1 + logger.info(f'Angle {angle} at step {steps}') + +def get_normal_angle(response: SonarSweepRequest.Response) -> float: + """Get a normal angle from the sonar scan.""" + if not response.is_object: + logger.error('No object detected — cannot rotate') + return np.nan + if np.isnan(response.normal_angle): + logger.error('[Sonar] normal_angle was NaN — cannot rotate') + return np.nan + + if response.normal_angle < -np.pi/2.: + return response.normal_angle + np.pi + + return response.normal_angle