From 73fe8568208fb51a84ba31efa6a1c0a3733e656e Mon Sep 17 00:00:00 2001 From: Panagiotis Liampas Date: Sat, 7 Feb 2026 18:31:03 -0500 Subject: [PATCH 1/2] WIP reporting for all tasks & differentiating entry gates from follow path --- all_seaing_autonomy/CMakeLists.txt | 1 + .../all_seaing_autonomy/roboboat/docking.py | 14 + .../roboboat/entry_gates.py | 605 ++++++++++++++++++ .../roboboat/follow_buoy_path.py | 149 +++-- .../roboboat/mechanism_navigation.py | 66 +- .../roboboat/return_home.py | 30 +- .../all_seaing_autonomy/roboboat/run_tasks.py | 27 +- .../roboboat/speed_challenge.py | 56 +- .../config/course/task_locations_real.yaml | 7 + .../config/course/task_locations_sim.yaml | 7 + .../launch/perception.launch.py | 2 +- all_seaing_bringup/launch/sim.launch.py | 18 + all_seaing_bringup/launch/tasks.launch.py | 32 +- all_seaing_bringup/launch/vehicle.launch.py | 2 +- .../all_seaing_common/action_server_base.py | 8 + .../all_seaing_common/task_server_base.py | 6 +- .../msg/LabeledObjectPlane.msg | 1 + .../object_tracking_map.cpp | 2 + 18 files changed, 954 insertions(+), 79 deletions(-) create mode 100755 all_seaing_autonomy/all_seaing_autonomy/roboboat/entry_gates.py diff --git a/all_seaing_autonomy/CMakeLists.txt b/all_seaing_autonomy/CMakeLists.txt index c6fa12c6..ca73c0fd 100644 --- a/all_seaing_autonomy/CMakeLists.txt +++ b/all_seaing_autonomy/CMakeLists.txt @@ -12,6 +12,7 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS ${PROJECT_NAME}/roboboat/delivery_server.py ${PROJECT_NAME}/roboboat/delivery_qual.py + ${PROJECT_NAME}/roboboat/entry_gates.py ${PROJECT_NAME}/roboboat/follow_buoy_path.py ${PROJECT_NAME}/roboboat/follow_buoy_pid.py ${PROJECT_NAME}/roboboat/run_tasks.py diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py index e3071df5..cfbb350e 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py @@ -20,6 +20,8 @@ from all_seaing_autonomy.roboboat.visualization_tools import VisualizationTools +from all_seaing_common.report_pb2 import Docking + import time import math import yaml @@ -253,6 +255,8 @@ def __init__(self): self.inv_label_mappings[value] = key self.state = DockingState.WAITING_DOCK + + self.reported_docking = False def green_indicator(self, dock_label): return dock_label in self.dock_green_labels @@ -544,6 +548,8 @@ def control_loop(self): # PID to go to the detected slot (consider its middle and the angle of the whole dock line) slot_back_mid = self.selected_slot[1][0] slot_dir = self.selected_slot[1][1] + slot_label = self.selected_slot[0] + slot_side = self.selected_slot[2] marker_arr.markers.append(VisualizationTools.visualize_line(slot_back_mid, self.perp_vec(slot_dir), mark_id, (0.0, 0.0, 1.0), self.robot_frame_id)) mark_id = mark_id + 1 @@ -554,6 +560,14 @@ def control_loop(self): self.cancel_navigation() # self.get_logger().info('DOCKING PID') self.state = DockingState.DOCKING + + # to get the reporting points even without necessarily successfully docking + if not self.reported_docking: + self.report_data(Docking( + dock="N" if slot_side == DockSide.NORTH else "S", + slip=self.number_priority[slot_label]+1)) + self.reported_docking = True + # go to that line and forward (negative error if boat left of line, positive if right) offset = -self.dot(self.difference(slot_back_mid, self.robot_pos), self.perp_vec(slot_dir)) approach_angle = self.angle_vec(self.negative(slot_dir), self.robot_dir) # TODO Check sign diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/entry_gates.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/entry_gates.py new file mode 100755 index 00000000..62763c64 --- /dev/null +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/entry_gates.py @@ -0,0 +1,605 @@ +#!/usr/bin/env python3 +from ast import Num +import rclpy +from rclpy.executors import MultiThreadedExecutor + +from all_seaing_interfaces.msg import ObstacleMap, Obstacle +from all_seaing_interfaces.action import Task +from ament_index_python.packages import get_package_share_directory +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import Header, ColorRGBA +from geometry_msgs.msg import Point, Pose, Vector3, Quaternion +from all_seaing_common.task_server_base import TaskServerBase + +from all_seaing_common.report_pb2 import GatePass, GateType + +import os +import yaml +import math +import time +from collections import deque +from functools import partial +from enum import Enum + +TIMER_PERIOD = 1 / 60 + +class ReturnState(Enum): + SETTING_UP = 1 + ENTERING = 2 + +class InternalBuoyPair: + def __init__(self, left_buoy=None, right_buoy=None): + if left_buoy is None: + self.left = Obstacle() + else: + self.left = left_buoy + + if right_buoy is None: + self.right = Obstacle() + else: + self.right = right_buoy + +class EntryGates(TaskServerBase): + def __init__(self): + super().__init__(server_name = "entry_gates_server", action_name = "entry_gates", search_action_name = "search_entry") + + self.map_sub = self.create_subscription( + ObstacleMap, "obstacle_map/global", self.map_cb, 10 + ) + + self.waypoint_marker_pub = self.create_publisher( + MarkerArray, "waypoint_markers", 10 + ) + + self.declare_parameter("is_sim", False) + self.is_sim = self.get_parameter("is_sim").get_parameter_value().bool_value + + self.declare_parameter("red_left", True) + self.red_left = self.get_parameter("red_left").get_parameter_value().bool_value + + self.declare_parameter("duplicate_dist", 0.5) + self.duplicate_dist = self.get_parameter("duplicate_dist").get_parameter_value().double_value + + self.declare_parameter("adaptive_distance", 0.7) + self.adaptive_distance = self.get_parameter("adaptive_distance").get_parameter_value().double_value + + self.declare_parameter("gate_dist_thres", 50.0) + self.gate_dist_thres = self.get_parameter("gate_dist_thres").get_parameter_value().double_value + + self.declare_parameter("circling_buoy_dist_thres", 25.0) + self.circling_buoy_dist_thres = self.get_parameter("circling_buoy_dist_thres").get_parameter_value().double_value + + self.declare_parameter("max_inter_gate_dist", 25.0) + self.max_inter_gate_dist = self.get_parameter("max_inter_gate_dist").get_parameter_value().double_value + + self.declare_parameter("max_gate_pair_dist", 25.0) + self.max_gate_pair_dist = self.get_parameter("max_gate_pair_dist").get_parameter_value().double_value + + self.declare_parameter("buoy_pair_dist_thres", 1.0) + self.buoy_pair_dist_thres = self.get_parameter("buoy_pair_dist_thres").get_parameter_value().double_value + + self.declare_parameter("inter_buoy_pair_dist", 1.0) + self.inter_buoy_pair_dist = self.get_parameter("inter_buoy_pair_dist").get_parameter_value().double_value + + self.declare_parameter("better_angle_thres", 0.2) + self.better_angle_thres = self.get_parameter("better_angle_thres").get_parameter_value().double_value + + self.declare_parameter("gate_dist_back", 1.0) + self.forward_dist_back = self.get_parameter("gate_dist_back").get_parameter_value().double_value + + self.declare_parameter("gate_probe_dist", 1.0) + self.gate_probe_dist = self.get_parameter("gate_probe_dist").get_parameter_value().double_value + + self.obstacles = None + + self.state = ReturnState.SETTING_UP + + bringup_prefix = get_package_share_directory("all_seaing_bringup") + + self.red_labels = set() + self.green_labels = set() + + self.declare_parameter( + "color_label_mappings_file", + os.path.join( + bringup_prefix, "config", "perception", "color_label_mappings.yaml" + ), + ) + + color_label_mappings_file = self.get_parameter( + "color_label_mappings_file" + ).value + with open(color_label_mappings_file, "r") as f: + label_mappings = yaml.safe_load(f) + # hardcoded from reading YAML + if self.is_sim: + self.green_labels.add(label_mappings["green"]) + self.red_labels.add(label_mappings["red"]) + else: + # self.green_labels.add(11) # just to use old rosbags + # self.red_labels.add(17) # just to use old rosbags + # self.green_labels.add(label_mappings["green_buoy"]) + # self.green_labels.add(label_mappings["green_circle"]) + self.green_labels.add(label_mappings["green_pole_buoy"]) + # self.red_labels.add(label_mappings["red_buoy"]) + # self.red_labels.add(label_mappings["red_circle"]) + self.red_labels.add(label_mappings["red_pole_buoy"]) + # self.red_labels.add(label_mappings["yellow_buoy"]) + # self.red_labels.add(label_mappings["yellow_racquet_ball"]) + self.declare_parameter( + "task_locations_file", + os.path.join( + bringup_prefix, "config", "course", "task_locations.yaml" + ), + ) + + self.declare_parameter( + "latlng_locations_file", + os.path.join( + bringup_prefix, "config", "localization", "locations.yaml" + ), + ) + + with open(self.get_parameter("latlng_locations_file").value, "r") as f: + self.latlng_location_mappings = yaml.safe_load(f) + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] + + self.gate_pair = None + + def replace_closest(self, ref_obs, obstacles): + if len(obstacles) == 0: + return ref_obs, False + opt_buoy = self.get_closest_to(self.ob_coords(ref_obs), obstacles) + if self.norm(self.ob_coords(ref_obs), self.ob_coords(opt_buoy)) < self.duplicate_dist: + return opt_buoy, True + else: + return ref_obs, False + + def pair_to_pose(self, pair): + return Pose(position=Point(x=pair[0], y=pair[1])) + + def quaternion_from_euler(self, roll, pitch, yaw): + """ + Converts euler roll, pitch, yaw to quaternion (w in last place) + quat = [x, y, z, w] + Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. + """ + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + + def update_gate_wpt_pos(self, forward_dist = 0.0, tryhard=False): + # split the buoys into red and green + green_buoys, red_buoys = self.split_buoys(self.obstacles) + self.gate_pair.left, res_left_left = self.replace_closest(self.gate_pair.left, red_buoys if self.red_left else green_buoys) + self.gate_pair.right, res_right_right = self.replace_closest(self.gate_pair.right, green_buoys if self.red_left else red_buoys) + if tryhard: + _, res_left_right = self.replace_closest(self.gate_pair.left, green_buoys if self.red_left else red_buoys) + _, res_right_left = self.replace_closest(self.gate_pair.right, red_buoys if self.red_left else green_buoys) + # Check if there is not a buoy of the intended color in close distance and there is one from the other color, then remove the waypoint, it is false + if ((not res_left_left) and (res_left_right)) or ((not res_right_right) and (res_right_left)): + self.get_logger().info('WE ARE GOING TO A FAKE PAIR, FIND PATH AGAIN') + if not self.setup_buoys(): + return self.gate_wpt + self.waypoint_marker_pub.publish(MarkerArray(markers=[Marker(id=0,action=Marker.DELETEALL)])) + gate_wpt, self.buoy_direction = self.midpoint_pair_dir(self.gate_pair, forward_dist) + self.waypoint_marker_pub.publish(self.buoy_pairs_to_markers([(self.gate_pair.left, self.gate_pair.right, self.pair_to_pose(gate_wpt), 0.0)])) + return gate_wpt + + def should_accept_task(self, goal_request): + if self.obstacles is None: + return False + return self.setup_buoys() + + def init_setup(self): + self.get_logger().info("Setup buoys succeeded!") + self.state = ReturnState.ENTERING + self.mark_successful() + + def control_loop(self): + self.enter_course() + self.mark_successful() + + + def map_cb(self, msg): + ''' + Gets the labeled map from all_seaing_perception. + ''' + self.obstacles = msg.obstacles + + def enter_course(self): + self.get_logger().info("Entering the course") + self.new_gate_pair = self.gate_pair + while self.new_gate_pair is not None: + self.gate_pair = self.new_gate_pair + self.gate_wpt, _ = self.midpoint_pair_dir(self.gate_pair, -self.forward_dist_back) + self.move_to_point(self.gate_wpt, busy_wait=True, + goal_update_func=partial(self.update_point, "gate_wpt", self.adaptive_distance, partial(self.update_gate_wpt_pos, -self.forward_dist_back))) + self.new_gate_pair = None + self.gate_wpt, _ = self.midpoint_pair_dir(self.gate_pair, self.gate_probe_dist) + self.move_to_point(self.gate_wpt, busy_wait=True, + exit_func=partial(self.next_pair, self.gate_pair), goal_update_func=partial(self.update_point, "gate_wpt", self.adaptive_distance, partial(self.update_gate_wpt_pos, self.gate_probe_dist))) + + self.report_data(GatePass( + type=GateType.GATE_ENTRY, + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos))) + + return Task.Result(success=True) + + def norm_squared(self, vec, ref=(0, 0)): + return (vec[0] - ref[0])**2 + (vec[1]-ref[1])**2 + + def norm(self, vec, ref=(0, 0)): + return math.sqrt(self.norm_squared(vec, ref)) + + def ob_coords(self, buoy, local=False): + if local: + return (buoy.local_point.point.x, buoy.local_point.point.y) + else: + return (buoy.global_point.point.x, buoy.global_point.point.y) + + def get_closest_to(self, source, buoys, local=False): + return min( + buoys, + key=lambda buoy: math.dist(source, self.ob_coords(buoy, local)), + ) + + def midpoint(self, vec1, vec2): + return ((vec1[0] + vec2[0]) / 2, (vec1[1] + vec2[1]) / 2) + + def midpoint_pair_dir(self, pair, forward_dist): + left_coords = self.ob_coords(pair.left) + right_coords = self.ob_coords(pair.right) + midpoint = self.midpoint(left_coords, right_coords) + + scale = forward_dist + dy = right_coords[1] - left_coords[1] + dx = right_coords[0] - left_coords[0] + norm = math.sqrt(dx**2 + dy**2) + dx /= norm + dy /= norm + midpoint = (midpoint[0] - scale*dy, midpoint[1] + scale*dx) + + return midpoint, (-dy, dx) + + def split_buoys(self, obstacles): + """ + Splits the buoys into red and green based on their labels in the obstacle map + """ + green_bouy_points = [] + red_bouy_points = [] + for obstacle in obstacles: + if obstacle.label in self.green_labels: + green_bouy_points.append(obstacle) + elif obstacle.label in self.red_labels: + red_bouy_points.append(obstacle) + return green_bouy_points, red_bouy_points + + def obs_to_pos(self, obs): + return [self.ob_coords(ob, local=False) for ob in obs] + + def obs_to_pos_label(self, obs): + return [self.ob_coords(ob, local=False) + (ob.label,) for ob in obs] + + def buoy_pairs_to_markers(self, buoy_pairs): + """ + Create the markers from an array of buoy pairs to visualize them (and the respective waypoints) in RViz + """ + marker_array = MarkerArray() + i = 0 + for p_left, p_right, point, radius in buoy_pairs: + marker_array.markers.append( + Marker( + type=Marker.ARROW, + pose=point, + header=Header(frame_id=self.global_frame_id), + scale=Vector3(x=2.0, y=0.15, z=0.15), + color=ColorRGBA(a=1.0, b=1.0), + id=(4 * i), + ) + ) + if self.red_left: + left_color = ColorRGBA(r=1.0, a=1.0) + right_color = ColorRGBA(g=1.0, a=1.0) + else: + left_color = ColorRGBA(g=1.0, a=1.0) + right_color = ColorRGBA(r=1.0, a=1.0) + + marker_array.markers.append( + Marker( + type=Marker.SPHERE, + pose=self.pair_to_pose(self.ob_coords(p_left)), + header=Header(frame_id=self.global_frame_id), + scale=Vector3(x=1.0, y=1.0, z=1.0), + color=left_color, + id=(4 * i) + 1, + ) + ) + marker_array.markers.append( + Marker( + type=Marker.SPHERE, + pose=self.pair_to_pose(self.ob_coords(p_right)), + header=Header(frame_id=self.global_frame_id), + scale=Vector3(x=1.0, y=1.0, z=1.0), + color=right_color, + id=(4 * i) + 2, + ) + ) + i += 1 + return marker_array + + + def setup_buoys(self, ref_pair = None): + """ + Runs when the first obstacle map is received, filters the buoys that are in front of + the robot (x>0 in local coordinates) and finds (and stores) the closest green one and + the closest red one, and because the robot is in the starting position these + are the front buoys of the robot starting box. + """ + self.get_logger().debug("Setting up starting buoys!") + self.get_logger().debug( + f"list of obstacles: {self.obs_to_pos_label(self.obstacles)}" + ) + + # Split all the buoys into red and green + green_init, red_init = self.split_buoys(self.obstacles) + + # lambda function that filters the buoys that are in front of the robot + obstacles_in_front = lambda obs: [ + ob for ob in obs + if ob.local_point.point.x > 0 and self.norm(self.robot_pos, self.ob_coords(ob)) < self.gate_dist_thres + ] + # take the green and red buoys that are in front of the robot + green_buoys, red_buoys = obstacles_in_front(green_init), obstacles_in_front(red_init) + self.get_logger().debug( + f"initial red buoys: {[self.ob_coords(buoy) for buoy in red_buoys]}, green buoys: {[self.ob_coords(buoy) for buoy in green_buoys]}" + ) + if len(red_buoys) == 0 or len(green_buoys) == 0: + self.get_logger().debug("No starting buoy pairs!") + return False + + # From the red buoys that are in front of the robot, take the one that is closest to it. + # And do the same for the green buoys. + # This pair is the front pair of the starting box of the robot. + # want to pick the pair that's far apart but has the closest midpoint + green_to = None + red_to = None + for red_b in red_buoys: + for green_b in green_buoys: + if self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) > self.max_inter_gate_dist: + continue + # elif ((green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True))))) and (self.red_left == self.ccw((0, 0), self.ob_coords(green_b, local=True), self.ob_coords(red_b, local=True))): + elif ((green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True))))): + green_to = green_b + red_to = red_b + if green_to is None: + return False + if self.red_left: + self.gate_pair = InternalBuoyPair(red_to, green_to) + else: + self.gate_pair = InternalBuoyPair(green_to, red_to) + self.get_logger().info(f'FOUND GATE') + self.waypoint_marker_pub.publish(MarkerArray(markers=[Marker(id=0,action=Marker.DELETEALL)])) + self.gate_mid, self.gate_dir = self.midpoint_pair_dir(self.gate_pair, 0.0) + self.waypoint_marker_pub.publish(self.buoy_pairs_to_markers([(self.gate_pair.left, self.gate_pair.right, self.pair_to_pose(self.gate_mid), 0.0)])) + return True + + def filter_front_buoys(self, pair, buoys): + """ + Returns the buoys (from the given array) that are in front of a pair of points, + considering the forward direction to be the one such that + the first point of the pair is in the left and the second is in the right + """ + # (red, green) + return [ + buoy + for buoy in buoys + if (self.ccw( + self.ob_coords(pair.left), + self.ob_coords(pair.right), + self.ob_coords(buoy), + ) and (min(self.norm(self.ob_coords(buoy), self.ob_coords(pair.left)), self.norm(self.ob_coords(buoy), self.ob_coords(pair.right))) > self.buoy_pair_dist_thres)) + ] + + def pick_buoy(self, buoys, prev_mid, ref_buoy): + # sort by distance + buoys.sort(key=lambda buoy: self.norm(prev_mid, self.ob_coords(buoy))) + for buoy in buoys: + if self.norm(self.ob_coords(ref_buoy), self.ob_coords(buoy)) > self.duplicate_dist: + return False, buoy + return True, ref_buoy + + def buoy_pairs_distance(self, p1, p2, mode="min", loc=False): + p1_left, p1_right = self.ob_coords(p1.left, local=loc), self.ob_coords(p1.right, local=loc) + p2_left, p2_right = self.ob_coords(p2.left, local=loc), self.ob_coords(p2.right, local=loc) + if mode=="mid": + p1_mid = ((p1_right[0]+p1_left[0])/2.0, (p1_right[1]+p1_left[1])/2.0) + p2_mid = ((p2_right[0]+p2_left[0])/2.0, (p2_right[1]+p2_left[1])/2.0) + dist = self.norm(p1_mid, p2_mid) + else: + left_diff = (p2_left[0]-p1_left[0], p2_left[1]-p1_left[1]) + right_diff = (p2_right[0]-p1_right[0], p2_right[1]-p1_right[1]) + dist = min(self.norm(left_diff), self.norm(right_diff)) + return dist + + def buoy_pairs_angle(self, p1, p2, loc=False): + p1_left, p1_right = (self.ob_coords(p1.left, local=loc), self.ob_coords(p1.right, local=loc)) + p2_left, p2_right = (self.ob_coords(p2.left, local=loc), self.ob_coords(p2.right, local=loc)) + p1_diff = (p1_right[0]-p1_left[0], p1_right[1]-p1_left[1]) + p2_diff = (p2_right[0]-p2_left[0], p2_right[1]-p2_left[1]) + # TODO: change angle to be away from the boat, use dot product + angle = math.acos((p1_diff[0]*p2_diff[0]+p1_diff[1]*p2_diff[1])/(self.norm(p1_diff)*self.norm(p2_diff))) + return angle + + def get_acute_angle(self, angle): + ret_angle = angle + if ret_angle < 0: + ret_angle = -ret_angle + if ret_angle > math.pi/2.0: + ret_angle = math.pi - ret_angle + return ret_angle + + + def better_buoy_pair_transition(self, p_old, p_new, p_ref, mode="both"): # mode can be both or either + # return ((self.buoy_pairs_distance(p_ref, p_new) <= self.buoy_pair_dist_thres and + # self.buoy_pairs_distance(p_ref, p_new, mode="min") > self.buoy_pairs_distance(p_ref, p_old, mode="min")) or + # return (self.buoy_pairs_distance(p_ref, p_new) > self.buoy_pair_dist_thres and + # (self.buoy_pairs_distance(p_ref, p_old) <= self.buoy_pair_dist_thres or + # self.buoy_pairs_angle(p_ref, p_old) > self.buoy_pairs_angle(p_ref, p_new))) + # want new pair to have midpoint distance from previous pair larger than the threshold + # above that threshold we check if either/both angles are better (closer to right angle) + # TODO: Add a max distance threshold (although will probably not be an issue since we only see the next couple pairs at most) + return (self.buoy_pairs_distance(p_ref, p_new, "mid") > self.buoy_pair_dist_thres and + (self.buoy_pairs_distance(p_ref, p_old, "mid") <= self.buoy_pair_dist_thres or + self.check_better_pair_angles(p_ref, p_old, p_new, mode))) + + + def check_better_pair_angles(self, ref_pair, old_pair, new_pair, mode="both"): # mode can be both or either + """ + Checks if a potential next pair is better than the current next pair, wrt to the old one + A + |\ + | \ + C----D + | \| + | B + | | + E----F + CD is better than AB wrt EF (closer to right angles wrt to E, F) + """ + old_left_duplicate = (self.norm(self.ob_coords(ref_pair.left), self.ob_coords(old_pair.left)) < self.duplicate_dist) + old_right_duplicate = (self.norm(self.ob_coords(ref_pair.right), self.ob_coords(old_pair.right)) < self.duplicate_dist) + new_left_duplicate = (self.norm(self.ob_coords(ref_pair.left), self.ob_coords(new_pair.left)) < self.duplicate_dist) + new_right_duplicate = (self.norm(self.ob_coords(ref_pair.right), self.ob_coords(new_pair.right)) < self.duplicate_dist) + + old_left = 0 if old_left_duplicate else self.get_triangle_angle(ref_pair.left, old_pair.left, old_pair.right) + old_right = 0 if old_right_duplicate else self.get_triangle_angle(ref_pair.right, old_pair.right, old_pair.left) + new_left = 0 if new_left_duplicate else self.get_triangle_angle(ref_pair.left, new_pair.left, new_pair.right) + new_right = 0 if new_right_duplicate else self.get_triangle_angle(ref_pair.right, new_pair.right, new_pair.left) + + # old and new can't be duplicate in both, because they would not be considered as potential next pairs + if ((old_left_duplicate or old_right_duplicate) and (new_left_duplicate or new_right_duplicate)): + old_angle = old_left if old_right_duplicate else old_right + new_angle = new_left if new_right_duplicate else new_right + return new_angle > old_angle + self.better_angle_thres + elif ((old_left_duplicate or old_right_duplicate) and (new_left_duplicate or new_right_duplicate)): + return (old_left_duplicate or old_right_duplicate) + else: + return ((new_left > (old_left + self.better_angle_thres)) and (new_right > (old_right + self.better_angle_thres))) if (mode == "both") else (new_left > (old_left + self.better_angle_thres)) or (new_right > (old_right + self.better_angle_thres)) + + def get_triangle_angle(self, buoy_a, buoy_b, buoy_c): + return self.get_acute_angle(self.buoy_pairs_angle(InternalBuoyPair(buoy_a, buoy_b), InternalBuoyPair(buoy_b, buoy_c))) + + def check_better_one_side(self, ref_buoy, old_buoy, new_buoy): + """ + Returns whether the new buoy is closer to right angle compared to the old one, wrt to the reference buoy (and the other buoy as part of the path) + A + |\ + | \ + | \ + B---C + B is better than C (reference buoy is A) + """ + old_angle = self.get_triangle_angle(ref_buoy, old_buoy, new_buoy) + new_angle = self.get_triangle_angle(ref_buoy, new_buoy, old_buoy) + return new_angle > (old_angle + self.better_angle_thres) + + def next_pair(self, prev_pair): + """ + Returns the next buoy pair from the previous pair, + by checking the closest one to the middle of the previous buoy pair that's in front of the pair + """ + green, red = self.split_buoys(self.obstacles) + front_red = self.filter_front_buoys(prev_pair, red) + front_green = self.filter_front_buoys(prev_pair, green) + self.get_logger().debug( + f"robot pos: {self.robot_pos}, front red buoys: {self.obs_to_pos(front_red)}, front green buoys: {self.obs_to_pos(front_green)}" + ) + + if ((not front_red) and (not front_green)) or ((prev_pair is None) and ((not front_red) or (not front_green))): + prev_coords = self.ob_coords(prev_pair.left), self.ob_coords( + prev_pair.right + ) + red_coords = self.obs_to_pos(red) + green_coords = self.obs_to_pos(green) + + self.get_logger().debug( + f"buoys: {prev_coords} \nred: {red_coords} \ngreen: {green_coords}" + ) + self.get_logger().debug(f"robot: {self.robot_pos}") + self.get_logger().debug("Missing at least one front buoy") + return False + + prev_pair_midpoint, _ = self.midpoint_pair_dir(prev_pair, 0.0) + self.get_logger().debug(f"prev pair midpoint: {prev_pair_midpoint}") + # Add a threshold on the minimum distance to a buoy if the new angle on the not close buoys is worse (diagonal but not the one that improves the pair) + # Instead of only checking the two closest, order by distance and pick either the first one not at duplicate distance or the furthest one + # If duplicate distance for both reject, if one is duplicate distance check angles + left_duplicate, left_next = self.pick_buoy(front_red if self.red_left else front_green, prev_pair_midpoint, prev_pair.left) + right_duplicate, right_next = self.pick_buoy(front_green if self.red_left else front_red, prev_pair_midpoint, prev_pair.right) + + improved = False + for left_buoy in ((True, prev_pair.left), (left_duplicate, left_next)): + for right_buoy in ((True, prev_pair.right), (right_duplicate, right_next)): + if left_buoy[0] and right_buoy[0]: + continue + + if left_buoy[0] and (not self.check_better_one_side(prev_pair.left, prev_pair.right, right_buoy[1])): + continue + + if right_buoy[0] and (not self.check_better_one_side(prev_pair.right, prev_pair.left, left_buoy[1])): + continue + + if self.norm(self.ob_coords(left_buoy[1]), self.ob_coords(right_buoy[1])) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(left_buoy[1]), self.ob_coords(right_buoy[1])) > self.max_inter_gate_dist: + continue + + cur = InternalBuoyPair(left_buoy[1], right_buoy[1]) + + if self.buoy_pairs_distance(prev_pair, cur, "mid") > self.max_gate_pair_dist: + continue + + if (self.new_gate_pair is None) or self.better_buoy_pair_transition(self.new_gate_pair, cur, prev_pair): + self.new_gate_pair = cur + improved = True + return improved + + def ccw(self, a, b, c): + """Return True if the points a, b, c are counterclockwise, respectively""" + area = ( + a[0] * b[1] + + b[0] * c[1] + + c[0] * a[1] + - a[1] * b[0] + - b[1] * c[0] + - c[1] * a[0] + ) + return area > 0 + + +def main(args=None): + rclpy.init(args=args) + node = EntryGates() + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + executor.spin() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py index df46fa1d..5b755e6f 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py @@ -12,6 +12,8 @@ from all_seaing_common.task_server_base import TaskServerBase from all_seaing_controller.pid_controller import PIDController +from all_seaing_common.report_pb2 import ObjectDetected, ObjectType, Color, TaskType + import math import os import yaml @@ -142,10 +144,10 @@ def __init__(self): # self.red_labels.add(17) # just to use old rosbags self.green_labels.add(label_mappings["green_buoy"]) self.green_labels.add(label_mappings["green_circle"]) - self.green_labels.add(label_mappings["green_pole_buoy"]) + # self.green_labels.add(label_mappings["green_pole_buoy"]) self.red_labels.add(label_mappings["red_buoy"]) self.red_labels.add(label_mappings["red_circle"]) - self.red_labels.add(label_mappings["red_pole_buoy"]) + # self.red_labels.add(label_mappings["red_pole_buoy"]) # self.red_labels.add(label_mappings["yellow_buoy"]) # self.red_labels.add(label_mappings["yellow_racquet_ball"]) # self.green_beacon_labels.add(label_mappings["yellow_buoy"]) @@ -192,6 +194,24 @@ def __init__(self): self.first_back = True + self.tracked_buoys = [] + self.max_tracked_buoy_id = -1 + + self.declare_parameter( + "latlng_locations_file", + os.path.join( + bringup_prefix, "config", "localization", "locations.yaml" + ), + ) + + with open(self.get_parameter("latlng_locations_file").value, "r") as f: + self.latlng_location_mappings = yaml.safe_load(f) + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] + def norm_squared(self, vec, ref=(0, 0)): return (vec[0] - ref[0])**2 + (vec[1]-ref[1])**2 @@ -355,7 +375,8 @@ def setup_buoys(self, pointing_direction=None): # lambda function that filters the buoys that are in front of the robot obstacles_in_front = lambda obs: [ ob for ob in obs - if ((pointing_direction is None and ob.local_point.point.x > 0) or (pointing_direction is not None and self.dot(self.difference(self.robot_pos, self.ob_coords(ob)), pointing_direction) > 0)) and self.norm(self.robot_pos, self.ob_coords(ob)) < self.gate_dist_thres + # if ((pointing_direction is None and ob.local_point.point.x > 0) or (pointing_direction is not None and self.dot(self.difference(self.robot_pos, self.ob_coords(ob)), pointing_direction) > 0)) and self.norm(self.robot_pos, self.ob_coords(ob)) < self.gate_dist_thres + if ((pointing_direction is None) or (pointing_direction is not None and self.dot(self.difference(self.robot_pos, self.ob_coords(ob)), pointing_direction) > 0)) and self.norm(self.robot_pos, self.ob_coords(ob)) < self.gate_dist_thres ] # take the green and red buoys that are in front of the robot green_buoys, red_buoys = obstacles_in_front(green_init), obstacles_in_front(red_init) @@ -381,47 +402,48 @@ def setup_buoys(self, pointing_direction=None): # self.get_logger().info("GREEN BUOYS LEFT, RED BUOYS RIGHT") # self.backup_pair = None # want to pick the pair that's far apart but has the closest midpoint - if self.first_setup: - green_to = None - red_to = None - for red_b in red_buoys: - for green_b in green_buoys: - if self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) > self.max_inter_gate_dist: - self.get_logger().debug(f'RED: {self.ob_coords(red_b)}, GREEN: {self.ob_coords(green_b)} REJECTED, INTER-BUOY DIST: {self.norm(self.ob_coords(red_b), self.ob_coords(green_b))}') - continue - elif (green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True)))): - self.get_logger().debug(f'RED: {self.ob_coords(red_b)}, GREEN: {self.ob_coords(green_b)} BETTER, DIST FROM ROBOT: {self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True)))}') - green_to = green_b - red_to = red_b - if green_to is None: - return False - if self.ccw((0, 0), self.ob_coords(green_to, local=True), self.ob_coords(red_to, local=True)): - self.red_left = True - self.pair_to = InternalBuoyPair(red_to, green_to) - self.get_logger().debug("RED BUOYS LEFT, GREEN BUOYS RIGHT") - else: - self.red_left = False - self.pair_to = InternalBuoyPair(green_to, red_to) - self.get_logger().debug("GREEN BUOYS LEFT, RED BUOYS RIGHT") - self.first_setup = False - return True + + # if self.first_setup: + # green_to = None + # red_to = None + # for red_b in red_buoys: + # for green_b in green_buoys: + # if self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) > self.max_inter_gate_dist: + # self.get_logger().debug(f'RED: {self.ob_coords(red_b)}, GREEN: {self.ob_coords(green_b)} REJECTED, INTER-BUOY DIST: {self.norm(self.ob_coords(red_b), self.ob_coords(green_b))}') + # continue + # elif (green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True)))): + # self.get_logger().debug(f'RED: {self.ob_coords(red_b)}, GREEN: {self.ob_coords(green_b)} BETTER, DIST FROM ROBOT: {self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True)))}') + # green_to = green_b + # red_to = red_b + # if green_to is None: + # return False + # if self.ccw((0, 0), self.ob_coords(green_to, local=True), self.ob_coords(red_to, local=True)): + # self.red_left = True + # self.pair_to = InternalBuoyPair(red_to, green_to) + # self.get_logger().debug("RED BUOYS LEFT, GREEN BUOYS RIGHT") + # else: + # self.red_left = False + # self.pair_to = InternalBuoyPair(green_to, red_to) + # self.get_logger().debug("GREEN BUOYS LEFT, RED BUOYS RIGHT") + # self.first_setup = False + # return True + # else: + green_to = None + red_to = None + for red_b in red_buoys: + for green_b in green_buoys: + if self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) > self.max_inter_gate_dist: + continue + elif ((green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True))))) and (self.red_left == self.ccw((0, 0), self.ob_coords(green_b, local=True), self.ob_coords(red_b, local=True))): + green_to = green_b + red_to = red_b + if green_to is None: + return False + if self.red_left: + self.pair_to = InternalBuoyPair(red_to, green_to) else: - green_to = None - red_to = None - for red_b in red_buoys: - for green_b in green_buoys: - if self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) < self.inter_buoy_pair_dist or self.norm(self.ob_coords(red_b), self.ob_coords(green_b)) > self.max_inter_gate_dist: - continue - elif ((green_to is None) or (self.norm(self.midpoint(self.ob_coords(red_b, local=True), self.ob_coords(green_b, local=True))) < self.norm(self.midpoint(self.ob_coords(red_to, local=True), self.ob_coords(green_to, local=True))))) and (self.red_left == self.ccw((0, 0), self.ob_coords(green_b, local=True), self.ob_coords(red_b, local=True))): - green_to = green_b - red_to = red_b - if green_to is None: - return False - if self.red_left: - self.pair_to = InternalBuoyPair(red_to, green_to) - else: - self.pair_to = InternalBuoyPair(green_to, red_to) - return True + self.pair_to = InternalBuoyPair(green_to, red_to) + return True def ccw(self, a, b, c): """Return True if the points a, b, c are counterclockwise, respectively""" @@ -880,6 +902,46 @@ def adapt_pair_to(self): green_buoys, red_buoys = self.split_buoys(self.obstacles) self.pair_to.left, _ = self.replace_closest(self.pair_to.left, red_buoys if self.red_left else green_buoys) self.pair_to.right, _ = self.replace_closest(self.pair_to.right, green_buoys if self.red_left else red_buoys) + + def report_new_obstacles(self): + obstacle: Obstacle + for obstacle in self.obstacles: + # TODO discard objects like dock or miscellaneous that shouldn't be tracked/don't exist in the course + if obstacle.id > self.max_tracked_buoy_id: + # potentially new obstacle + new_obs = True + tracked_obs: Obstacle + for tracked_obs in self.tracked_buoys: + if self.norm(self.ob_coords(tracked_obs), self.ob_coords(obstacle)) < self.duplicate_dist: + new_obs = False + break + if new_obs: + # report & add to tracked obs + self.tracked_buoys.append(obstacle) + obs_type = ObjectType.OBJECT_UNKNOWN + obs_color = Color.COLOR_UNKNOWN + if obstacle.label in self.green_beacon_labels: + obs_type = ObjectType.OBJECT_LIGHT_BEACON + obs_color = Color.COLOR_GREEN + elif obstacle.label in self.red_beacon_labels: + obs_type = ObjectType.OBJECT_LIGHT_BEACON + obs_color = Color.COLOR_RED + elif obstacle.label in self.green_labels: + obs_type = ObjectType.OBJECT_BUOY + obs_color = Color.COLOR_GREEN + elif obstacle.label in self.red_labels: + obs_type = ObjectType.OBJECT_BUOY + obs_color = Color.COLOR_RED + else: + obs_type = ObjectType.OBJECT_BUOY + obs_color = Color.COLOR_BLACK + self.report_data(ObjectDetected( + object_type=obs_type, + color=obs_color, + position=self.pos_to_latlng(self.latlng_origin, self.ob_coords(obstacle)), + object_id=obstacle.id, + task_context=TaskType.TASK_NAV_CHANNEL)) + def map_cb(self, msg): """ @@ -892,6 +954,7 @@ def map_cb(self, msg): return if self.state in [FollowPathState.WAITING_GREEN_BEACON, FollowPathState.CIRCLING_GREEN_BEACON]: self.adapt_pair_to() + self.report_new_obstacles() def probe_green_beacon(self): ''' diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py index 8329014d..e4d31a77 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py @@ -15,7 +15,8 @@ from visualization_msgs.msg import Marker, MarkerArray from geometry_msgs.msg import Pose, Point, Vector3, Quaternion from std_msgs.msg import Header, ColorRGBA -from action_msgs.msg import GoalStatus + +from all_seaing_common.report_pb2 import ObjectDetected, ObjectType, Color, TaskType, ObjectDelivery, DeliveryType from tf_transformations import quaternion_from_euler, euler_from_quaternion @@ -148,6 +149,24 @@ def __init__(self): else: self.water_labels = [self.label_mappings[name] for name in ["black_triangle"]] self.ball_labels = [self.label_mappings[name] for name in ["black_cross"]] + + self.declare_parameter( + "latlng_locations_file", + os.path.join( + bringup_prefix, "config", "localization", "locations.yaml" + ), + ) + + with open(self.get_parameter("latlng_locations_file").value, "r") as f: + self.latlng_location_mappings = yaml.safe_load(f) + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] + + self.tracked_targets = [] + self.max_tracked_banner_id = -1 # update from subs self.water_banners = [] @@ -171,11 +190,44 @@ def ctr_normal(self, plane: LabeledObjectPlane): _,_,theta = euler_from_quaternion([plane.normal_ctr.orientation.x, plane.normal_ctr.orientation.y, plane.normal_ctr.orientation.z, plane.normal_ctr.orientation.w]) return (center_pt, (np.cos(theta), np.sin(theta))) + def banner_coords(self, banner: LabeledObjectPlane): + return self.ctr_normal(banner)[0] + + def report_new_targets(self): + banner: LabeledObjectPlane + for banner in self.plane_msg.objects: + if banner.label not in (self.water_labels+self.ball_labels): + continue + if banner.id > self.max_tracked_banner_id: + # potentially new target + new_banner = True + tracked_banner: LabeledObjectPlane + for tracked_banner in self.tracked_targets: + if self.norm(self.banner_coords(tracked_banner), self.banner_coords(banner)) < self.duplicate_dist: + new_banner = False + break + if new_banner: + # report & add to tracked targets + self.tracked_targets.append(banner) + obs_type = ObjectType.OBJECT_BOAT + obs_color = Color.COLOR_UNKNOWN + if banner.label in self.water_labels: + obs_color = Color.COLOR_YELLOW + else: + obs_color = Color.COLOR_BLACK + self.report_data(ObjectDetected( + object_type=obs_type, + color=obs_color, + position=self.pos_to_latlng(self.latlng_origin, self.banner_coords(banner)), + object_id=banner.id, + task_context=TaskType.TASK_OBJECT_DELIVERY)) + def plane_cb(self, msg: LabeledObjectPlaneArray): self.plane_msg = msg if self.paused: return self.find_target() + self.report_new_targets() def find_target(self): if self.plane_msg is None: @@ -422,10 +474,22 @@ def control_loop(self): self.get_logger().info(f'STARTED SHOOTING {self.selected_target[0]}, TIME: {time.time()}') self.time_started_shooting = time.time() + obs_color = Color.COLOR_UNKNOWN + del_type = DeliveryType.DELIVERY_UNKNOWN + if self.selected_target[0] == TargetType.WATER_TARGET: self.call_delivery_server("water") + obs_color = Color.COLOR_YELLOW + del_type = DeliveryType.DELIVERY_WATER else: self.call_delivery_server("ball") + obs_color = Color.COLOR_BLACK + del_type = DeliveryType.DELIVERY_BOAT + + self.report_data(ObjectDelivery( + color=obs_color, + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos), + delivery_type=del_type)) return # elif (time.time() - self.time_started_shooting > 5): diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/return_home.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/return_home.py index 5fdc4f81..f67ad443 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/return_home.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/return_home.py @@ -11,6 +11,8 @@ from geometry_msgs.msg import Point, Pose, Vector3, Quaternion from all_seaing_common.task_server_base import TaskServerBase +from all_seaing_common.report_pb2 import GatePass, GateType + import os import yaml import math @@ -131,15 +133,22 @@ def __init__(self): ), ) - self.gate_pair = None + self.declare_parameter( + "latlng_locations_file", + os.path.join( + bringup_prefix, "config", "localization", "locations.yaml" + ), + ) - def reset_challenge(self): - ''' - Readies the server for the upcoming speed challenge. - ''' - self.buoy_found = False - self.following_guide = True - self.moved_to_point = False + with open(self.get_parameter("latlng_locations_file").value, "r") as f: + self.latlng_location_mappings = yaml.safe_load(f) + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] + + self.gate_pair = None def replace_closest(self, ref_obs, obstacles): if len(obstacles) == 0: @@ -225,6 +234,11 @@ def return_to_start(self): self.gate_wpt, _ = self.midpoint_pair_dir(self.gate_pair, self.gate_probe_dist) self.move_to_point(self.gate_wpt, busy_wait=True, exit_func=partial(self.next_pair, self.gate_pair), goal_update_func=partial(self.update_point, "gate_wpt", self.adaptive_distance, partial(self.update_gate_wpt_pos, self.gate_probe_dist))) + + self.report_data(GatePass( + type=GateType.GATE_EXIT, + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos))) + return Task.Result(success=True) def norm_squared(self, vec, ref=(0, 0)): diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py index 95d619b8..d25f0b8e 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py @@ -61,9 +61,13 @@ def __init__(self): ActionClient(self, Task, "task_init") ] self.task_list = [ + # ENTRY GATES + [ActionType.SEARCH, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Search, "search_entry"), ReferenceInt(0), ReferenceInt(0), "entry"], + [ActionType.TASK, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Task, "entry_gates"), ReferenceInt(0), ReferenceInt(0)], + # FOLLOW PATH - [ActionType.SEARCH, TaskType.TASK_UNKNOWN, ActionClient(self, Search, "search_followpath"), ReferenceInt(0), ReferenceInt(0), "follow_path"], - [ActionType.TASK, TaskType.TASK_UNKNOWN, ActionClient(self, Task, "follow_buoy_path"), ReferenceInt(0), ReferenceInt(0)], + [ActionType.SEARCH, TaskType.TASK_NAV_CHANNEL, ActionClient(self, Search, "search_followpath"), ReferenceInt(0), ReferenceInt(0), "follow_path"], + [ActionType.TASK, TaskType.TASK_NAV_CHANNEL, ActionClient(self, Task, "follow_buoy_path"), ReferenceInt(0), ReferenceInt(0)], # SPEED CHALLENGE # [ActionType.SEARCH, TaskType.TASK_SPEED_CHALLENGE, ActionClient(self, Search, "search_speed"), ReferenceInt(0), ReferenceInt(0), "speed_challenge"], @@ -77,6 +81,10 @@ def __init__(self): # [ActionType.SEARCH, TaskType.OBJECT_DELIVERY, ActionClient(self, Search, "search_delivery"), ReferenceInt(0), ReferenceInt(0), "delivery"], # [ActionType.TASK, TaskType.OBJECT_DELIVERY, ActionClient(self, Task, "mechanism_navigation"), ReferenceInt(0), ReferenceInt(0)], + # EXIT GATES + # [ActionType.SEARCH, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Search, "search_return"), ReferenceInt(0), ReferenceInt(0), "return"], + # [ActionType.TASK, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Task, "return_home"), ReferenceInt(0), ReferenceInt(0)], + # FALLBACKS # [ActionType.TASK, TaskType.TASK_UNKNOWN, ActionClient(self, Task, "follow_buoy_pid"), ReferenceInt(0), ReferenceInt(0)], # [ActionType.TASK, TaskType.TASK_SPEED_CHALLENGE, ActionClient(self, Task, "speed_challenge_pid"), ReferenceInt(0), ReferenceInt(0)], @@ -191,7 +199,11 @@ def __init__(self): with open(self.get_parameter("latlng_locations_file").value, "r") as f: self.latlng_location_mappings = yaml.safe_load(f) - self.latlng_origin = self.latlng_location_mappings["nbpark"] + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] self.gate_pair = None @@ -249,22 +261,17 @@ def map_cb(self, msg): if self.gate_pair is None: self.setup_buoys() - - def report_shore_heartbeat(self): - EARTH_RADIUS = 6_370_000 RAD_TO_DEG = 180.0 / math.pi - pose = self.get_robot_pose() # (east, north, heading) current_task = TaskType.TASK_NONE # TODO: make sure still works after harbor alert added if self.current_task_type != None: current_task = self.current_task_type self.report_data(all_seaing_common.report_pb2.Heartbeat( state=self.heartbeat_state, - position=LatLng(latitude=self.latlng_origin["lat"] + RAD_TO_DEG * pose[1] / EARTH_RADIUS, - longitude=self.latlng_origin["lon"] - RAD_TO_DEG * pose[0] / EARTH_RADIUS), # Deal with CW / CCW + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos), # (east, north) spd_mps=self.vel, - heading_deg= ((90 - (RAD_TO_DEG) * (self.get_robot_pose()[2])) % 360), # Deal with CW / CCW + heading_deg= ((90 - (RAD_TO_DEG) * (self.get_robot_pose()[2])) % 360), # (east, north, heading), deal with CW / CCW current_task=current_task)) def sim_keyboard_callback(self, msg): diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/speed_challenge.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/speed_challenge.py index 755d45d6..128c697f 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/speed_challenge.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/speed_challenge.py @@ -12,6 +12,8 @@ from geometry_msgs.msg import Point, Pose, Vector3, Quaternion from all_seaing_common.task_server_base import TaskServerBase +from all_seaing_common.report_pb2 import ObjectDetected, ObjectType, Color, TaskType, GatePass, GateType + import os import yaml import math @@ -170,6 +172,21 @@ def __init__(self): self.green_beacon_labels.add(label_mappings["green_indicator"]) self.red_beacon_labels.add(label_mappings["red_indicator"]) + + self.declare_parameter( + "latlng_locations_file", + os.path.join( + bringup_prefix, "config", "localization", "locations.yaml" + ), + ) + + with open(self.get_parameter("latlng_locations_file").value, "r") as f: + self.latlng_location_mappings = yaml.safe_load(f) + + self.declare_parameter("location", "nbpark") + self.location = self.get_parameter("location").get_parameter_value().string_value + + self.latlng_origin = self.latlng_location_mappings[self.location] self.gate_pair = None # self.first_setup = True @@ -258,6 +275,11 @@ def control_loop(self): action_result = Task.Result(success=True) if self.state == SpeedChallengeState.RETURNING: action_result = self.return_to_start() + + self.report_data(GatePass( + type=GateType.GATE_SPEED_END, + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos))) + if action_result.success: self.mark_successful() else: @@ -289,28 +311,46 @@ def control_loop(self): self.mark_unsuccessful() return + self.report_data(GatePass( + type=GateType.GATE_SPEED_START, + position=self.pos_to_latlng(self.latlng_origin, self.robot_pos))) + self.state = SpeedChallengeState.PROBING_BUOY # cancel control loop if a single action fails if not action_result.success: self.mark_unsuccessful() - - def map_cb(self, msg): - ''' - Gets the labeled map from all_seaing_perception. - ''' - self.obstacles = msg.obstacles - if self.paused: - return + def identify_beacon(self): for obstacle in self.obstacles: if self.norm(self.robot_pos, self.ob_coords(obstacle)) > self.beacon_dist_thres: continue # below might toggle some times but will hopefully settle before we start circling, it's fixed once we start circling if obstacle.label in self.red_beacon_labels: self.temp_left_first = False + self.report_data(ObjectDetected( + object_type=ObjectType.OBJECT_LIGHT_BEACON, + color=Color.COLOR_RED, + position=self.pos_to_latlng(self.latlng_origin, self.ob_coords(obstacle)), + object_id=obstacle.id, + task_context=TaskType.TASK_SPEED_CHALLENGE)) elif obstacle.label in self.green_beacon_labels: self.temp_left_first = True + self.report_data(ObjectDetected( + object_type=ObjectType.OBJECT_LIGHT_BEACON, + color=Color.COLOR_GREEN, + position=self.pos_to_latlng(self.latlng_origin, self.ob_coords(obstacle)), + object_id=obstacle.id, + task_context=TaskType.TASK_SPEED_CHALLENGE)) + + def map_cb(self, msg): + ''' + Gets the labeled map from all_seaing_perception. + ''' + self.obstacles = msg.obstacles + if self.paused: + return + self.identify_beacon() def probe_blue_buoy(self): ''' diff --git a/all_seaing_bringup/config/course/task_locations_real.yaml b/all_seaing_bringup/config/course/task_locations_real.yaml index c6305bce..f2b990a6 100644 --- a/all_seaing_bringup/config/course/task_locations_real.yaml +++ b/all_seaing_bringup/config/course/task_locations_real.yaml @@ -1,3 +1,10 @@ +entry: # put the coordinates of the first gate + x: 0.0 + y: 0.0 + search_theta: false + theta: 0.0 + wait_time: 0.0 + follow_path: x: 0.0 y: 0.0 diff --git a/all_seaing_bringup/config/course/task_locations_sim.yaml b/all_seaing_bringup/config/course/task_locations_sim.yaml index 5ac0c06f..e01a36d4 100644 --- a/all_seaing_bringup/config/course/task_locations_sim.yaml +++ b/all_seaing_bringup/config/course/task_locations_sim.yaml @@ -1,3 +1,10 @@ +entry: # put the coordinates of the first gate + x: 0.0 + y: 0.0 + search_theta: false + theta: 0.0 + wait_time: 0.0 + follow_path: x: 0.0 y: 0.0 diff --git a/all_seaing_bringup/launch/perception.launch.py b/all_seaing_bringup/launch/perception.launch.py index 9639f4c8..cc1778ee 100644 --- a/all_seaing_bringup/launch/perception.launch.py +++ b/all_seaing_bringup/launch/perception.launch.py @@ -467,7 +467,7 @@ def generate_launch_description(): return LaunchDescription( [ # DeclareLaunchArgument('use_sim_time', default_value='true'), - DeclareLaunchArgument("location", default_value="pavillion"), + DeclareLaunchArgument("location", default_value="nbpark"), DeclareLaunchArgument("use_slam", default_value='true'), DeclareLaunchArgument("use_gps", default_value='true'), DeclareLaunchArgument("track_banners", default_value='true'), diff --git a/all_seaing_bringup/launch/sim.launch.py b/all_seaing_bringup/launch/sim.launch.py index 2cd8d10b..7daf3f97 100644 --- a/all_seaing_bringup/launch/sim.launch.py +++ b/all_seaing_bringup/launch/sim.launch.py @@ -562,6 +562,23 @@ def launch_setup(context, *args, **kwargs): ], output="screen", ) + + entry_gates = launch_ros.actions.Node( + package="all_seaing_autonomy", + executable="entry_gates.py", + parameters=[ + {"is_sim": True}, + {"red_left": True}, + {"color_label_mappings_file": color_label_mappings}, + {"robot_frame_id": "wamv/wamv/base_link"}, + {"search_task_radius": 50.0}, + # {"gate_dist_back": 5.0}, + {"gate_probe_dist": 10.0}, + {"gate_dist_thres": 50.0}, + ], + remappings=[ + ] + ) follow_buoy_path = launch_ros.actions.Node( package="all_seaing_autonomy", @@ -839,6 +856,7 @@ def launch_setup(context, *args, **kwargs): heartbeat_reporter, run_tasks, task_init_server, + entry_gates, follow_buoy_path, speed_challenge, docking, diff --git a/all_seaing_bringup/launch/tasks.launch.py b/all_seaing_bringup/launch/tasks.launch.py index 4b03efc9..7c505327 100644 --- a/all_seaing_bringup/launch/tasks.launch.py +++ b/all_seaing_bringup/launch/tasks.launch.py @@ -62,6 +62,8 @@ def launch_setup(context, *args, **kwargs): bringup_prefix, "config", "perception", "contour_matching_color_ranges.yaml" ) + location = context.perform_substitution(LaunchConfiguration("location")) + use_waypoint_client = LaunchConfiguration("use_waypoint_client") run_tasks = launch_ros.actions.Node( @@ -70,7 +72,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"is_sim": False}, {"red_left": True}, - {"global_frame_id": "map"}, + {"location": location}, # {"color_label_mappings_file": buoy_label_mappings}, {"color_label_mappings_file": all_label_mappings}, {"task_locations_file": task_locations}, @@ -87,13 +89,31 @@ def launch_setup(context, *args, **kwargs): ] ) + entry_gates = launch_ros.actions.Node( + package="all_seaing_autonomy", + executable="entry_gates.py", + parameters=[ + {"is_sim": False}, + {"red_left": True}, + {"location": location}, + # {"color_label_mappings_file": buoy_label_mappings}, + {"color_label_mappings_file": all_label_mappings}, + {"search_task_radius": 50.0}, + {"gate_dist_back": 5.0}, + {"gate_probe_dist": 10.0}, + {"gate_dist_thres": 50.0}, + ], + remappings=[ + ] + ) + follow_buoy_path = launch_ros.actions.Node( package="all_seaing_autonomy", executable="follow_buoy_path.py", parameters=[ {"is_sim": False}, {"red_left": True}, - {"global_frame_id": "map"}, + {"location": location}, # {"color_label_mappings_file": buoy_label_mappings}, {"color_label_mappings_file": all_label_mappings}, {"search_task_radius": 50.0}, @@ -126,9 +146,9 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"is_sim": False}, {"red_left": True}, + {"location": location}, # {"color_label_mappings_file": buoy_label_mappings}, {"color_label_mappings_file": all_label_mappings}, - {"robot_frame_id": "base_link"}, {"search_task_radius": 50.0}, {"gate_dist_thres": 40.0}, {"beacon_dist_thres": 15.0}, @@ -158,7 +178,6 @@ def launch_setup(context, *args, **kwargs): # {"shape_label_mappings_file": buoy_label_mappings}, # {"shape_label_mappings_file": shape_label_mappings}, {"shape_label_mappings_file": all_label_mappings}, - {"robot_frame_id": "base_link"}, {"search_task_radius": 50.0}, {"dock_width": 3.0}, {"dock_length": 5.0}, # TODO change to actual length: 7 @@ -191,10 +210,10 @@ def launch_setup(context, *args, **kwargs): executable="mechanism_navigation.py", parameters=[ {"is_sim": False}, + {"location": location}, # {"shape_label_mappings_file": buoy_label_mappings}, # {"shape_label_mappings_file": shape_label_mappings}, {"shape_label_mappings_file": all_label_mappings}, - {"robot_frame_id": "base_link"}, {"search_task_radius": 50.0}, {"wpt_banner_dist": 3.0}, {"navigation_dist_thres": 5.0}, @@ -219,9 +238,9 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"is_sim": False}, {"red_left": False}, + {"location": location}, # {"color_label_mappings_file": buoy_label_mappings}, {"color_label_mappings_file": all_label_mappings}, - {"robot_frame_id": "base_link"}, {"search_task_radius": 50.0}, {"gate_dist_back": 5.0}, {"gate_probe_dist": 10.0}, @@ -412,6 +431,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( [ + DeclareLaunchArgument("location", default_value="nbpark"), DeclareLaunchArgument( "use_waypoint_client", default_value="false", choices=["true", "false"] ), diff --git a/all_seaing_bringup/launch/vehicle.launch.py b/all_seaing_bringup/launch/vehicle.launch.py index 84874017..7f150460 100644 --- a/all_seaing_bringup/launch/vehicle.launch.py +++ b/all_seaing_bringup/launch/vehicle.launch.py @@ -441,7 +441,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( [ - DeclareLaunchArgument("location", default_value="boathouse"), + DeclareLaunchArgument("location", default_value="nbpark"), DeclareLaunchArgument( "comms", default_value="custom", choices=["wifi", "lora", "custom"] ), diff --git a/all_seaing_common/all_seaing_common/action_server_base.py b/all_seaing_common/all_seaing_common/action_server_base.py index 56c1b7d5..5201551b 100644 --- a/all_seaing_common/all_seaing_common/action_server_base.py +++ b/all_seaing_common/all_seaing_common/action_server_base.py @@ -15,6 +15,9 @@ from std_msgs.msg import ByteMultiArray from all_seaing_common.report import report_factory +import math +from all_seaing_common.report_pb2 import LatLng + class ActionServerBase(ABC, Node): """ @@ -151,3 +154,8 @@ def report_data(self, data): # Pass in only the subobject - the Report / date / msg.data = [bytes([b]) for b in report_factory(data).SerializeToString()] self.reporter_pub.publish(msg) + def pos_to_latlng(self, latlng_origin, pose): + EARTH_RADIUS = 6_370_000 + RAD_TO_DEG = 180.0 / math.pi + return LatLng(latitude=latlng_origin["lat"] + RAD_TO_DEG * pose[1] / EARTH_RADIUS, + longitude=latlng_origin["lon"] - RAD_TO_DEG * pose[0] / EARTH_RADIUS), # Deal with CW / CCW \ No newline at end of file diff --git a/all_seaing_common/all_seaing_common/task_server_base.py b/all_seaing_common/all_seaing_common/task_server_base.py index 620c258a..1f505c9a 100644 --- a/all_seaing_common/all_seaing_common/task_server_base.py +++ b/all_seaing_common/all_seaing_common/task_server_base.py @@ -8,6 +8,9 @@ from all_seaing_common.action_server_base import ActionServerBase from action_msgs.msg import GoalStatus +from all_seaing_common.report_pb2 import LatLng +import all_seaing_common.report_pb2 + import os import yaml import time @@ -498,4 +501,5 @@ def search_execute_callback(self, goal_handle): self.end_process(f"Searching Server for [{self.server_name}] task completed with result {self.found_task}") goal_handle.succeed() - return Search.Result(success=self.found_task) \ No newline at end of file + return Search.Result(success=self.found_task) + diff --git a/all_seaing_interfaces/msg/LabeledObjectPlane.msg b/all_seaing_interfaces/msg/LabeledObjectPlane.msg index 780c668e..841969d3 100644 --- a/all_seaing_interfaces/msg/LabeledObjectPlane.msg +++ b/all_seaing_interfaces/msg/LabeledObjectPlane.msg @@ -1,3 +1,4 @@ +uint32 id int8 label geometry_msgs/Pose normal_ctr # represents the normal of the plane, centered at the centroid of int8 -> x is the normal (towards the robot), y is left (horizontal to ground), z is up geometry_msgs/Vector3 size # size of the plane in y,z axes (x = 0) \ No newline at end of file diff --git a/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp b/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp index 13508fa2..303dc041 100644 --- a/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp +++ b/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp @@ -1554,6 +1554,7 @@ void ObjectTrackingMap::banners_cb(const all_seaing_interfaces::msg::LabeledObje // increase object count and expand & initialize matrices m_num_banners++; // add object to tracked obstacles vector + detected_banners[i]->plane_msg.id = m_obstacle_id++; m_tracked_banners.push_back(detected_banners[i]); Eigen::Matrix init_new_cov{ {(float)m_banner_init_new_cov*m_banner_init_new_cov, 0, 0}, @@ -1641,6 +1642,7 @@ void ObjectTrackingMap::banners_cb(const all_seaing_interfaces::msg::LabeledObje if (banner_label_indicator.count(detected_banners[i]->label) && (!banner_label_indicator[detected_banners[i]->label])){ detected_banners[i]->label = m_tracked_banners[tracked_id]->label; } + detected_banners[i]->plane_msg.id = m_tracked_banners[tracked_id]->plane_msg.id; detected_banners[i]->time_seen = m_tracked_banners[tracked_id]->time_seen; detected_banners[i]->last_dead = m_tracked_banners[tracked_id]->last_dead; detected_banners[i]->time_dead = m_tracked_banners[tracked_id]->time_dead; From 9f741d62487976c0f5470cbfe504921b6d1a5fce Mon Sep 17 00:00:00 2001 From: Panagiotis Liampas Date: Sat, 7 Feb 2026 19:58:25 -0500 Subject: [PATCH 2/2] everything working --- .../all_seaing_autonomy/roboboat/docking.py | 9 +++++--- .../roboboat/follow_buoy_path.py | 2 +- .../roboboat/mechanism_navigation.py | 4 ++-- .../all_seaing_autonomy/roboboat/run_tasks.py | 8 +++---- .../config/course/task_locations_real.yaml | 2 +- .../config/course/task_locations_sim.yaml | 2 +- all_seaing_bringup/launch/sim.launch.py | 2 ++ all_seaing_bringup/launch/tasks.launch.py | 16 +++++++++++++- all_seaing_bringup/launch/vehicle.launch.py | 10 --------- .../all_seaing_common/action_server_base.py | 2 +- all_seaing_common/all_seaing_common/report.py | 2 +- .../all_seaing_driver/rover_lora_reporter.py | 22 ++++++++++++++----- .../object_tracking_map.cpp | 2 ++ 13 files changed, 53 insertions(+), 30 deletions(-) diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py index cfbb350e..4b120c05 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py @@ -20,7 +20,7 @@ from all_seaing_autonomy.roboboat.visualization_tools import VisualizationTools -from all_seaing_common.report_pb2 import Docking +import all_seaing_common.report_pb2 import time import math @@ -390,6 +390,7 @@ def find_docking_slot(self): self.x_pid.reset() self.y_pid.reset() self.theta_pid.reset() + self.reported_docking = False if self.state == DockingState.NAVIGATING_DOCK: self.state = DockingState.CANCELLING_NAVIGATION else: @@ -413,6 +414,7 @@ def find_docking_slot(self): self.x_pid.reset() self.y_pid.reset() self.theta_pid.reset() + self.reported_docking = False self.get_logger().info(f'WILL DOCK INTO {self.inv_label_mappings[self.selected_slot[0]], dock_side}') if (not self.picked_slot) or (not self.updated_slot_pos): @@ -423,6 +425,7 @@ def find_docking_slot(self): self.x_pid.reset() self.y_pid.reset() self.theta_pid.reset() + self.reported_docking = False if self.state == DockingState.NAVIGATING_DOCK: # was going to a fake slot (misdetection) self.state = DockingState.CANCELLING_NAVIGATION @@ -563,9 +566,9 @@ def control_loop(self): # to get the reporting points even without necessarily successfully docking if not self.reported_docking: - self.report_data(Docking( + self.report_data(all_seaing_common.report_pb2.Docking( dock="N" if slot_side == DockSide.NORTH else "S", - slip=self.number_priority[slot_label]+1)) + slip=str(self.number_priority[slot_label]+1))) self.reported_docking = True # go to that line and forward (negative error if boat left of line, positive if right) diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py index 5b755e6f..e5eed700 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/follow_buoy_path.py @@ -1105,7 +1105,7 @@ def exit_angle_met(): dist_to_buoy = self.norm(self.green_beacon_pos, self.robot_pos) dt = (self.get_clock().now() - self.prev_update_time).nanoseconds / 1e9 - self.get_logger().info(f"PID values: set_point {t_o}, effort {pid_output:.3f}, sense value {dist_to_buoy:.3f}") + # self.get_logger().info(f"PID values: set_point {t_o}, effort {pid_output:.3f}, sense value {dist_to_buoy:.3f}") self.turn_pid.update(dist_to_buoy,dt) # TODO: update in_Circling correctly (check 90 degrees) diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py index e4d31a77..4df2fd6e 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/mechanism_navigation.py @@ -484,10 +484,10 @@ def control_loop(self): else: self.call_delivery_server("ball") obs_color = Color.COLOR_BLACK - del_type = DeliveryType.DELIVERY_BOAT + del_type = DeliveryType.DELIVERY_BALL self.report_data(ObjectDelivery( - color=obs_color, + vessel_color=obs_color, position=self.pos_to_latlng(self.latlng_origin, self.robot_pos), delivery_type=del_type)) diff --git a/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py b/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py index d25f0b8e..b4aa0daa 100755 --- a/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py +++ b/all_seaing_autonomy/all_seaing_autonomy/roboboat/run_tasks.py @@ -62,8 +62,8 @@ def __init__(self): ] self.task_list = [ # ENTRY GATES - [ActionType.SEARCH, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Search, "search_entry"), ReferenceInt(0), ReferenceInt(0), "entry"], - [ActionType.TASK, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Task, "entry_gates"), ReferenceInt(0), ReferenceInt(0)], + # [ActionType.SEARCH, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Search, "search_entry"), ReferenceInt(0), ReferenceInt(0), "entry"], + # [ActionType.TASK, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Task, "entry_gates"), ReferenceInt(0), ReferenceInt(0)], # FOLLOW PATH [ActionType.SEARCH, TaskType.TASK_NAV_CHANNEL, ActionClient(self, Search, "search_followpath"), ReferenceInt(0), ReferenceInt(0), "follow_path"], @@ -78,8 +78,8 @@ def __init__(self): # [ActionType.TASK, TaskType.TASK_DOCKING, ActionClient(self, Task, "docking"), ReferenceInt(0), ReferenceInt(0)], # DELIVERY - # [ActionType.SEARCH, TaskType.OBJECT_DELIVERY, ActionClient(self, Search, "search_delivery"), ReferenceInt(0), ReferenceInt(0), "delivery"], - # [ActionType.TASK, TaskType.OBJECT_DELIVERY, ActionClient(self, Task, "mechanism_navigation"), ReferenceInt(0), ReferenceInt(0)], + # [ActionType.SEARCH, TaskType.TASK_OBJECT_DELIVERY, ActionClient(self, Search, "search_delivery"), ReferenceInt(0), ReferenceInt(0), "delivery"], + # [ActionType.TASK, TaskType.TASK_OBJECT_DELIVERY, ActionClient(self, Task, "mechanism_navigation"), ReferenceInt(0), ReferenceInt(0)], # EXIT GATES # [ActionType.SEARCH, TaskType.TASK_ENTRY_EXIT, ActionClient(self, Search, "search_return"), ReferenceInt(0), ReferenceInt(0), "return"], diff --git a/all_seaing_bringup/config/course/task_locations_real.yaml b/all_seaing_bringup/config/course/task_locations_real.yaml index f2b990a6..63ed78e6 100644 --- a/all_seaing_bringup/config/course/task_locations_real.yaml +++ b/all_seaing_bringup/config/course/task_locations_real.yaml @@ -7,7 +7,7 @@ entry: # put the coordinates of the first gate follow_path: x: 0.0 - y: 0.0 + y: 40.0 search_theta: false theta: 0.0 wait_time: 0.0 diff --git a/all_seaing_bringup/config/course/task_locations_sim.yaml b/all_seaing_bringup/config/course/task_locations_sim.yaml index e01a36d4..986feb71 100644 --- a/all_seaing_bringup/config/course/task_locations_sim.yaml +++ b/all_seaing_bringup/config/course/task_locations_sim.yaml @@ -7,7 +7,7 @@ entry: # put the coordinates of the first gate follow_path: x: 0.0 - y: 0.0 + y: 40.0 search_theta: false theta: 0.0 wait_time: 0.0 diff --git a/all_seaing_bringup/launch/sim.launch.py b/all_seaing_bringup/launch/sim.launch.py index 7daf3f97..71f62088 100644 --- a/all_seaing_bringup/launch/sim.launch.py +++ b/all_seaing_bringup/launch/sim.launch.py @@ -559,6 +559,8 @@ def launch_setup(context, *args, **kwargs): remappings=[ ], parameters=[ + {"port": "/dev/ttyACM0"}, + {"is_sim": True}, ], output="screen", ) diff --git a/all_seaing_bringup/launch/tasks.launch.py b/all_seaing_bringup/launch/tasks.launch.py index 7c505327..7416d0bb 100644 --- a/all_seaing_bringup/launch/tasks.launch.py +++ b/all_seaing_bringup/launch/tasks.launch.py @@ -407,14 +407,28 @@ def launch_setup(context, *args, **kwargs): ], ) + heartbeat_reporter = launch_ros.actions.Node( + package="all_seaing_driver", + executable="rover_lora_reporter.py", + remappings=[ + ], + parameters=[ + {"port": "/dev/ttyACM3"}, + {"is_sim": False}, + ], + output="screen", + ) + return [ controller_server, # navigation_server, navigation_server_tangent, # navigation_server_nomap, rviz_waypoint_sender, + heartbeat_reporter, run_tasks, - task_init_server, + task_init_server, + entry_gates, follow_buoy_path, speed_challenge, docking, diff --git a/all_seaing_bringup/launch/vehicle.launch.py b/all_seaing_bringup/launch/vehicle.launch.py index 7f150460..08e53f01 100644 --- a/all_seaing_bringup/launch/vehicle.launch.py +++ b/all_seaing_bringup/launch/vehicle.launch.py @@ -370,15 +370,6 @@ def launch_setup(context, *args, **kwargs): ), ) - heartbeat_reporter = launch_ros.actions.Node( - package="all_seaing_driver", - executable="rover_lora_reporter.py", - remappings=[ - ], - parameters=[{"port": "/dev/ttyACM3"}], - output="screen", - ) - perception_ld = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -432,7 +423,6 @@ def launch_setup(context, *args, **kwargs): pcl_to_scan_node, rf2o_node, ekf_node_rf2o, - heartbeat_reporter, # perception_ld, # tasks_ld, ] diff --git a/all_seaing_common/all_seaing_common/action_server_base.py b/all_seaing_common/all_seaing_common/action_server_base.py index 5201551b..fc94219c 100644 --- a/all_seaing_common/all_seaing_common/action_server_base.py +++ b/all_seaing_common/all_seaing_common/action_server_base.py @@ -158,4 +158,4 @@ def pos_to_latlng(self, latlng_origin, pose): EARTH_RADIUS = 6_370_000 RAD_TO_DEG = 180.0 / math.pi return LatLng(latitude=latlng_origin["lat"] + RAD_TO_DEG * pose[1] / EARTH_RADIUS, - longitude=latlng_origin["lon"] - RAD_TO_DEG * pose[0] / EARTH_RADIUS), # Deal with CW / CCW \ No newline at end of file + longitude=latlng_origin["lon"] - RAD_TO_DEG * pose[0] / EARTH_RADIUS) # Deal with CW / CCW \ No newline at end of file diff --git a/all_seaing_common/all_seaing_common/report.py b/all_seaing_common/all_seaing_common/report.py index a8ac4e69..47e38c2b 100644 --- a/all_seaing_common/all_seaing_common/report.py +++ b/all_seaing_common/all_seaing_common/report.py @@ -29,7 +29,7 @@ def report_factory(data): team_id=TEAM_ID, vehicle_id=VEHICLE_ID, seq=__report_seq[0], - object_detected=data + gate_pass=data ) case ObjectDelivery(): ret = Report( diff --git a/all_seaing_driver/all_seaing_driver/rover_lora_reporter.py b/all_seaing_driver/all_seaing_driver/rover_lora_reporter.py index 804024f4..c890387f 100755 --- a/all_seaing_driver/all_seaing_driver/rover_lora_reporter.py +++ b/all_seaing_driver/all_seaing_driver/rover_lora_reporter.py @@ -7,6 +7,7 @@ import serial import struct import time +from all_seaing_common.report_pb2 import * class RoverLoraReporter(Node): def __init__(self): @@ -14,23 +15,34 @@ def __init__(self): port = self.declare_parameter( "port", "/dev/ttyACM0").get_parameter_value().string_value + + self.declare_parameter("is_sim", False) + self.is_sim = self.get_parameter("is_sim").get_parameter_value().bool_value self.report_queue = Queue() - # Setup serial connection - self.serial_port = serial.Serial(port, 115200, timeout=1) - time.sleep(2) # Allow serial port to stabilize + if not self.is_sim: + + # Setup serial connection + self.serial_port = serial.Serial(port, 115200, timeout=1) + time.sleep(2) # Allow serial port to stabilize self.reporter_subscriber = self.create_subscription(ByteMultiArray, "task_reporter", self.add_message, 10) self.timer = self.create_timer(0.2, self.send_message_if_exists) def send_message_if_exists(self): + # TODO we need to process heartbeat in a separate queue and publish it as it arrives, to not have it get dropped/put back in the queue if not self.report_queue.empty(): raw_msg = self.report_queue.get() frame = struct.pack("!I", len(raw_msg)) + raw_msg + "test".encode() - self.serial_port.write(frame) - self.serial_port.flush() + if not self.is_sim: + self.serial_port.write(frame) + self.serial_port.flush() + else: + msg = Report() + msg.ParseFromString(raw_msg) + self.get_logger().info(msg.__str__()) def add_message(self, msg: ByteMultiArray): self.report_queue.put(b"".join(msg.data)) diff --git a/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp b/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp index 303dc041..63161e40 100644 --- a/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp +++ b/all_seaing_perception/all_seaing_perception/object_tracking_map.cpp @@ -216,6 +216,8 @@ ObjectTrackingMap::ObjectTrackingMap() : Node("object_tracking_map") { m_gps_based_dx = 0; m_gps_based_dy = 0; m_gps_based_dtheta = 0; + + m_obstacle_id = 0; } template std::string matrix_to_string(T_matrix matrix) {