Skip to content

Detect side of dock #451

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
pliam1105 merged 1 commit into from
Feb 5, 2026
Merged

Detect side of dock #451

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 26 additions & 11 deletions all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@ class DockingState(Enum):
CANCELLING_NAVIGATION = 4
NEW_NAVIGATION = 5

class DockSide(Enum):
NORTH = 1
SOUTH = 2

class Docking(TaskServerBase):
def __init__(self):
super().__init__(server_name = "docking_server", action_name = "docking", search_action_name = "search_docking")
Expand Down Expand Up @@ -202,9 +206,14 @@ def __init__(self):
if self.is_sim:
self.dock_labels = [self.label_mappings[name] for name in ["blue_circle", "blue_cross", "blue_triangle", "green_circle", "green_cross", "green_square", "green_triangle", "red_circle", "red_cross", "red_triangle", "red_square"]]
self.boat_labels = [self.label_mappings[name] for name in ["black_cross", "black_triangle"]]

self.dock_green_labels = self.dock_labels
self.dock_red_labels = []

for label in self.dock_labels:
self.noindicator_label[label] = label
self.indicator_priority[label] = 0
self.number_priority[label] = 0
else:
self.dock_labels = [self.label_mappings[name] for name in ["number_1", "number_2", "number_3", "number_1_green", "number_2_green", "number_3_green", "number_1_red", "number_2_red", "number_3_red"]]
self.boat_labels = [self.label_mappings[name] for name in ["black_cross", "black_triangle"]]
Expand Down Expand Up @@ -285,6 +294,12 @@ def better_slot(self, slot, ref_slot):
return True
else:
return False

def dock_side(self, dock_normal):
if 0 < np.arctan2(dock_normal[1], dock_normal[0]) < np.pi: # facing north
return DockSide.SOUTH
else:
return DockSide.NORTH # facing south

def point_cloud_cb(self, msg):
self.lidar_point_cloud = msg
Expand Down Expand Up @@ -330,7 +345,7 @@ def find_docking_slot(self):
if (obj_plane.label in self.dock_labels):
self.got_dock = True
ctr, normal = self.ctr_normal(obj_plane)
new_dock_banners.append((obj_plane.label, (ctr, normal)))
new_dock_banners.append((obj_plane.label, (ctr, normal), self.dock_side(normal)))
# self.get_logger().info(f'SLOT: {self.inv_label_mappings[obj_plane.label]} -> {(ctr, normal).__str__()}')

# update global variables
Expand All @@ -343,9 +358,9 @@ def find_docking_slot(self):

if self.got_dock:
# check for taken docks and stuff
for dock_label, (dock_ctr, dock_normal) in self.dock_banners:
for (dock_label, (dock_ctr, dock_normal), dock_side) in self.dock_banners:
# dock_label = self.noindicator_label[dock_label]
if self.noindicator_label[dock_label] in self.taken:
if (self.noindicator_label[dock_label], dock_side) in self.taken:
# self.get_logger().info(f'DOCK {self.inv_label_mappings[dock_label]} IS TAKEN BASED ON PREVIOUS OBSERVATIONS')
continue # just ignore, since we saw that it was taken once it's always taken
# check if any boat is in that slot
Expand All @@ -362,8 +377,8 @@ def find_docking_slot(self):
self.get_logger().info(f'DOCK {self.inv_label_mappings[dock_label]} IS TAKEN')
taken = True
if taken:
self.taken.append(self.noindicator_label[dock_label])
if(self.picked_slot and self.selected_slot[0] == self.noindicator_label[dock_label]):
self.taken.append((self.noindicator_label[dock_label], dock_side))
if self.picked_slot and self.selected_slot[0] == self.noindicator_label[dock_label] and self.selected_slot[2] == dock_side:
# we're cooked
self.selected_slot = None
self.picked_slot = False
Expand All @@ -375,26 +390,26 @@ def find_docking_slot(self):
self.state = DockingState.CANCELLING_NAVIGATION
else:
self.state = DockingState.WAITING_DOCK
self.get_logger().info(f'DOCK {self.inv_label_mappings[dock_label]} IS TAKEN')
self.get_logger().info(f'DOCK {self.inv_label_mappings[dock_label], dock_side} IS TAKEN')
continue
# found empty docking slot
self.picked_slot = True
if self.selected_slot is None:
self.updated_slot_pos = True
if (self.selected_slot is not None) and (self.selected_slot[0] == self.noindicator_label[dock_label]) and (self.norm(self.selected_slot[1][0], dock_ctr) < self.duplicate_dist):
if (self.selected_slot is not None) and (self.selected_slot[0] == self.noindicator_label[dock_label]) and (self.selected_slot[2] == dock_side) and (self.norm(self.selected_slot[1][0], dock_ctr) < self.duplicate_dist):
# same slot, update position & normal
self.selected_slot = (self.noindicator_label[dock_label], (dock_ctr, dock_normal))
self.selected_slot = (self.noindicator_label[dock_label], (dock_ctr, dock_normal), dock_side)
self.updated_slot_pos = True
if (self.selected_slot is None) or self.better_slot((dock_label, (dock_ctr, dock_normal)), self.selected_slot):
if (self.selected_slot is None) or self.better_slot((dock_label, (dock_ctr, dock_normal), dock_side), self.selected_slot):
self.state = DockingState.NEW_NAVIGATION
# found an empty one closer
self.selected_slot = (self.noindicator_label[dock_label], (dock_ctr, dock_normal))
self.selected_slot = (self.noindicator_label[dock_label], (dock_ctr, dock_normal), dock_side)
self.updated_slot_pos = True
# self.pid.reset()
self.x_pid.reset()
self.y_pid.reset()
self.theta_pid.reset()
self.get_logger().info(f'WILL DOCK INTO {self.inv_label_mappings[self.selected_slot[0]]}')
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):
# if (not self.picked_slot):
Expand Down
1 change: 1 addition & 0 deletions all_seaing_bringup/launch/sim.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -781,6 +781,7 @@ def launch_setup(context, *args, **kwargs):
# "world": "follow_path_task.sdf",
# "world": "speed_course_world.sdf",
# "world": "scan_dock_deliver_task.sdf",
# "world": "roboboat_full_course.sdf",
"urdf": f"{description_prefix}/urdf/xdrive_wamv/wamv_target.urdf",
"extra_gz_args": extra_gz_args,
}.items(),
Expand Down
Loading