Task-specific reporting, differentiating entry gates from follow path

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

all_seaing_autonomy/all_seaing_autonomy/roboboat/docking.py

@@ -20,6 +20,8 @@
 
 from all_seaing_autonomy.roboboat.visualization_tools import VisualizationTools
 
+import all_seaing_common.report_pb2
+
 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
@@ -386,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:
@@ -409,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):
@@ -419,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
@@ -544,6 +551,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 +563,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(all_seaing_common.report_pb2.Docking(
+                    dock="N" if slot_side == DockSide.NORTH else "S",
+                    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)
             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