feat: mission 3 + mission 1 more robust

.github/workflows/docker-ci.yml

@@ -5,9 +5,9 @@ on:
     branches:
       - main
 
-  # push:
-  #   branches:
-  #     - sm_dev
+  push:
+    branches:
+      - test_feat
       # - nav_test
       # - main
 

bb_state_machine/bb_state_machine/base_state.py

@@ -53,11 +53,19 @@ def angle_difference(self, target, current):
             diff -= 2 * np.pi
         return diff
 
-    def execute_rotation(self, angle, max_angular_speed, control=True):
-        current_theta = self.normalize_angle(self.shared_data.theta)
+    def execute_rotation(self, angle, max_angular_speed, control=True, use_odom=False):
+        if use_odom:
+            current_theta = self.normalize_angle(self.shared_data.odom_theta)
+        else:
+            current_theta = self.normalize_angle(self.shared_data.theta)
+
         target_theta = self.normalize_angle(angle)
         angle_diff = self.angle_difference(target_theta, current_theta)
 
+        # self.logger.info(f"Current theta: {current_theta}")
+        # self.logger.info(f"Target theta: {target_theta}")
+        # self.logger.info(f"Angle diff: {angle_diff}")
+
         if control:
             angular_speed = np.exp(np.abs(angle_diff)*2-1) * np.abs(max_angular_speed)
             angular_speed = max(min(angular_speed, max_angular_speed), self.min_angular_speed)
@@ -101,12 +109,11 @@ def load_data(self, file_path, data_type):
 
                     if data_type == 'waypoints_t':
                         if len(line) == 4:
-                            try:
-                                target_list.append((float(line[0]), float(line[1]), float(line[2]), int(line[3])))
-                            except ValueError:
-                                self.logger.error(f"Invalid number format in line: {line}")
+                            target_list.append((float(line[0]), float(line[1]), float(line[2]), int(line[3]), None, None))
+                        elif len(line) == 6:
+                            target_list.append((float(line[0]), float(line[1]), float(line[2]), int(line[3]), float(line[4]), float(line[5])))
                         else:
-                            self.logger.error(f"Malformed line in command file, expected 4 elements but got {len(line)}: {line}")
+                            self.logger.error(f"Malformed line in command file, expected 4 or 6 elements but got {len(line)}: {line}")
 
                     elif data_type == 'commands':
                         if len(line) == 2:

bb_state_machine/bb_state_machine/bb_state_machine_node.py

@@ -2,7 +2,7 @@
 from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
-from bb_state_machine.missions import Mission1, Mission2
+from bb_state_machine.missions import Mission1, Mission2, Mission3
 from bb_state_machine.shared_data import SharedData
 from bb_state_machine.robot_state_machine import RobotStateMachine
 from bb_state_machine.utils import quaternion_to_euler, is_point_in_zone
@@ -28,7 +28,7 @@ def __init__(self):
 
         self.get_logger().info('Blockbuster State Machine node successfully started')
 
-        self.distance_threshold = 0.4
+        self.distance_threshold = 0.3
         self.alpha = 0.5
         self.display_marker = False
 
@@ -91,7 +91,8 @@ def _init_state_machine(self):
         self.state_machine = RobotStateMachine(self.get_logger())
         self.state_machine.add_mission("MISSION_1", Mission1("MISSION_1", self.shared_data, self.perform_action, self.get_logger()))
         self.state_machine.add_mission("MISSION_2", Mission2("MISSION_2", self.shared_data, self.perform_action, self.get_logger()))
-        self.state_machine.set_mission("MISSION_1")
+        self.state_machine.add_mission("MISSION_3", Mission3("MISSION_3", self.shared_data, self.perform_action, self.get_logger()))
+        self.state_machine.set_mission("MISSION_2")
 
     def timer_tf_callback(self):
         try:
@@ -122,6 +123,7 @@ def timer_sm_callback(self):
 
     def imu_callback(self, msg):
         pitch, _, _ = quaternion_to_euler(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
+        # self.get_logger().info(f'Pitch cam: {pitch}')
         self.shared_data.update_pitch(pitch)
 
     def sys_info_callback(self, msg):
@@ -135,9 +137,9 @@ def odom_callback(self, msg):
     def scan_callback(self, msg):
         front_index = int((0 - msg.angle_min) / msg.angle_increment)
         front_distance = msg.ranges[front_index]
+
         if front_distance!=float('inf'):
             self.shared_data.update_front_distance(front_distance)
-            # self.get_logger().info(f'Distance in front of the robot: {front_distance:.2f} meters')
 
 
     def yolov6_callback(self, msg):
@@ -179,6 +181,49 @@ def yolov6_callback(self, msg):
 
         self._publish_markers_if_enabled(detection_dict)
 
+    # def yolov6_callback(self, msg):
+    #     detection_dict = self.shared_data.detection_dict
+    #     for detection in msg.detections:
+    #         # self.get_logger().info(f"detection: {detection}")
+
+    #         point_in_base_frame = None
+    #         try:
+    #             point_in_camera_frame = PointStamped()
+    #             point_in_camera_frame.header.frame_id = "oak_rgb_camera_optical_frame"
+    #             point_in_camera_frame.header.stamp = msg.header.stamp
+    #             point_in_camera_frame.point.x = detection.position.x
+    #             point_in_camera_frame.point.y = detection.position.y
+    #             point_in_camera_frame.point.z = detection.position.z
+    #             point_in_base_frame = self.tf_buffer.transform(point_in_camera_frame, "map", rclpy.duration.Duration(seconds=0.5))
+    #         except TransformException as ex:
+    #             self.get_logger().info(f'Could not transform oak_rgb_camera_optical_frame to base_link: {ex}')
+
+    #         if point_in_base_frame:
+    #             # self.get_logger().info(f"point_in_base_frame: {point_in_base_frame}")
+
+    #             # if point_in_base_frame.point.z < -0.05 or point_in_base_frame.point.z > 0.15:
+    #             #     continue
+
+    #             object_position = (point_in_base_frame.point.x, point_in_base_frame.point.y)
+
+    #             already_stored = False
+    #             for object_id, stored_position in detection_dict.items():
+    #                 distance = np.sqrt((stored_position[0] - object_position[0])**2 + (stored_position[1] - object_position[1])**2)
+    #                 if distance < self.distance_threshold:
+    #                     already_stored = True
+    #                     ema_position = [(self.alpha * object_position[j] + (1 - self.alpha) * stored_position[j]) for j in range(2)]
+    #                     detection_dict[object_id] = tuple(ema_position)
+    #                     break
+
+    #             if not already_stored and not self.shared_data.is_circle_free(object_position[0], object_position[1], 0, 150):
+    #                 detection_dict[self.get_new_detection_id(detection_dict)] = object_position
+
+    #             self.shared_data.update_detection_dict(detection_dict)
+
+    #     self.get_logger().info(f"Duplos dict: {self.shared_data.detection_dict}")
+
+    #     self._publish_markers_if_enabled(detection_dict)
+
     def _publish_markers_if_enabled(self, detection_dict):
         if self.display_marker:
             marker_array = MarkerArray()

bb_state_machine/bb_state_machine/missions.py

@@ -13,7 +13,8 @@ def __init__(self, name, shared_data, action_interface, logger):
         self.add_substate("GOBACKTO_Z3", AutoNavA("GOBACKTO_Z3", self.shared_data, action_interface, logger, filename="/mission1/m1_gobackto_z3.csv"))
         self.add_substate("HOMING", AutoNavA("HOMING", self.shared_data, action_interface, logger, filename="/mission1/m1_homing.csv"))
         self.add_substate("OUT_Z3", AutoNavA("OUT_Z3", self.shared_data, action_interface, logger, filename="/mission1/m1_out_z3.csv"))
-        self.add_substate("PUSH_BUTTON", ManNav("PUSH_BUTTON", self.shared_data, action_interface, logger, filename="/mission1/m1_push_button.csv"))
+        self.add_substate("PUSH_BUTTON", ManNav("PUSH_BUTTON", self.shared_data, action_interface, logger, use_odom=False, filename="/mission1/m1_push_button.csv"))
+        self.add_substate("REATTEMPT_BUTTON", ManNav("REATTEMPT_BUTTON", self.shared_data, action_interface, logger, filename="/mission1/m1_reattempt_button.csv"))
         self.add_substate("UNLOADING", ManNav("UNLOADING", self.shared_data, action_interface, logger, filename="/general/unloading.csv"))
         self.default_substate = "GOTO_Z3"
 
@@ -27,6 +28,12 @@ def determine_next_state(self):
             return "PUSH_BUTTON"
 
         elif current_ss_name == "PUSH_BUTTON" and current_ss_status == "COMPLETED":
+            if self.shared_data.front_distance > 3:
+                return "SEARCH_Z3"
+            else:
+                return "REATTEMPT_BUTTON"
+
+        elif current_ss_name == "REATTEMPT_BUTTON" and current_ss_status == "COMPLETED":
             if self.shared_data.front_distance > 3:
                 return "SEARCH_Z3"
             else:
@@ -38,15 +45,15 @@ def determine_next_state(self):
                 return "HOMING"
             elif current_ss_status == "COMPLETED":
                 self.zone_3_completed = True
-                if self.shared_data.duplos_stored == 0:
-                    self.status = "COMPLETED"
-                    return None
-                else:
-                    return "OUT_Z3"
+                return "OUT_Z3"
 
         elif current_ss_name == "OUT_Z3" and current_ss_status == "COMPLETED":
-            if self.shared_data.duplos_stored < self.shared_data.max_duplos_stored:
+            if self.shared_data.duplos_stored < self.shared_data.max_duplos_stored and \
+                self.shared_data.duplos_stored != 0:
                 return "SEARCH_Z1"
+            elif self.shared_data.duplos_stored == 0:
+                self.status = "COMPLETED"
+                return None
             else:
                 return "HOMING"
 
@@ -71,9 +78,11 @@ class Mission2(SuperState):
     def __init__(self, name, shared_data, action_interface, logger):
         super().__init__(name, shared_data, action_interface, logger)
         self.add_substate("SEARCH_Z4", AutoNavT("SEARCH_Z4", self.shared_data, action_interface, logger, filename="/mission2/m2_search_z4.csv", zone='ZONE_4'))
+        self.add_substate("SEARCH_Z1", AutoNavT("SEARCH_Z1", self.shared_data, action_interface, logger, filename="/mission2/m2_search_z1.csv", zone='ZONE_1'))
         self.add_substate("GOTO_Z4", AutoNavA("GOTO_Z4", self.shared_data, action_interface, logger, filename="/mission2/m2_goto_z4.csv"))
-        self.add_substate("SLOPE_UP_1", SlopeClimbing("SLOPE_UP_1", self.shared_data, action_interface, logger, speed=0.5, angle_limit=1.51, angular_speed_z = 0.1, direction_up=True))
-        self.add_substate("SLOPE_UP_2", SlopeClimbing("SLOPE_UP_2", self.shared_data, action_interface, logger, speed=0.5, angle_limit=1.51, angular_speed_z = 0.1, direction_up=False))
+        self.add_substate("SLOPE_UP_1", SlopeClimbing("SLOPE_UP_1", self.shared_data, action_interface, logger, speed=0.5, angle_limit=1.51, angular_speed_z = 0.13, direction_up=True))
+        self.add_substate("SLOPE_UP_2", SlopeClimbing("SLOPE_UP_2", self.shared_data, action_interface, logger, speed=0.5, angle_limit=1.51, angular_speed_z = 0.13, direction_up=False))
+        self.add_substate("RECOVERY_BEHAVIOR", ManNav("RECOVERY_BEHAVIOR", self.shared_data, action_interface, logger, use_odom = True, filename="/mission2/m2_recovery_behavior.csv"))
         self.add_substate("APPROACH_SLOPE_LOW", ManNav("APPROACH_SLOPE_LOW", self.shared_data, action_interface, logger, filename="/mission2/m2_approach_slope_low.csv"))
         self.add_substate("LEAVE_SLOPE_HIGH", ManNav("LEAVE_SLOPE_HIGH", self.shared_data, action_interface, logger, filename="/mission2/m2_leave_slope_high.csv"))
         self.add_substate("GOTO_SLOPE_HIGH", AutoNavA("GOTO_SLOPE_HIGH", self.shared_data, action_interface, logger, filename="/mission2/m2_goto_slope_high.csv"))
@@ -97,11 +106,20 @@ def determine_next_state(self):
         elif current_ss_name == "APPROACH_SLOPE_LOW" and current_ss_status == "COMPLETED":
             return "SLOPE_UP_1"
 
-        elif current_ss_name == "SLOPE_UP_1" and current_ss_status == "COMPLETED":
-            return "SLOPE_UP_2"
+        elif current_ss_name == "SLOPE_UP_1":
+            if current_ss_status == "COMPLETED":
+                return "SLOPE_UP_2"
+            elif current_ss_status == "FAILED":
+                return "RECOVERY_BEHAVIOR"
+
+        elif current_ss_name == "SLOPE_UP_2":
+            if current_ss_status == "COMPLETED":
+                return "LEAVE_SLOPE_HIGH"
+            elif current_ss_status == "FAILED":
+                return "RECOVERY_BEHAVIOR"
 
-        elif current_ss_name == "SLOPE_UP_2" and current_ss_status == "COMPLETED":
-            return "LEAVE_SLOPE_HIGH"
+        elif current_ss_name == "RECOVERY_BEHAVIOR" and current_ss_status == "COMPLETED":
+            return "SLOPE_UP_1"
 
         elif current_ss_name == "LEAVE_SLOPE_HIGH" and current_ss_status == "COMPLETED":
             return "SEARCH_Z4"
@@ -122,7 +140,14 @@ def determine_next_state(self):
             return "LEAVE_SLOPE_LOW"
 
         elif current_ss_name == "LEAVE_SLOPE_LOW" and current_ss_status == "COMPLETED":
-            return "HOMING"
+            if self.shared_data.duplos_stored < self.shared_data.max_duplos_stored and \
+                self.shared_data.duplos_stored != 0:
+                return "SEARCH_Z1"
+            elif self.shared_data.duplos_stored == 0:
+                self.status = "COMPLETED"
+                return None
+            else:
+                return "HOMING"
 
         elif current_ss_name == "HOMING" and current_ss_status == "COMPLETED":
             return "UNLOADING"
@@ -136,4 +161,40 @@ def determine_next_state(self):
 
         elif current_ss_name == "SEARCH_Z4" and current_ss_status == "COMPLETED":
             self.mission_2_completed = True
-            return "GOTO_SLOPE_HIGH"
+            return "GOTO_SLOPE_HIGH"
+
+class Mission3(SuperState):
+    def __init__(self, name, shared_data, action_interface, logger):
+        super().__init__(name, shared_data, action_interface, logger)
+        self.add_substate("SEARCH_Z1", AutoNavT("SEARCH_Z1", self.shared_data, action_interface, logger, filename="/mission3/m3_search_z1.csv", zone='ZONE_1'))
+        self.add_substate("HOMING", AutoNavA("HOMING", self.shared_data, action_interface, logger, filename="/mission3/m3_homing.csv"))
+        self.add_substate("UNLOADING", ManNav("UNLOADING", self.shared_data, action_interface, logger, filename="/general/unloading.csv"))
+        self.default_substate = "SEARCH_Z1"
+
+        self.mission_3_completed = False
+
+    def determine_next_state(self):
+        current_ss_name = self.current_substate.name
+        current_ss_status = self.current_substate.status
+
+        if current_ss_name == "SEARCH_Z1":
+            if current_ss_status == "STORAGE_FULL":
+                return "HOMING"
+            elif current_ss_status == "COMPLETED":
+                self.mission_3_completed = True
+                if self.shared_data.duplos_stored == 0:
+                    self.status = "COMPLETED"
+                    return None
+                else:
+                    return "HOMING"
+
+        elif current_ss_name == "HOMING" and current_ss_status == "COMPLETED":
+            return "UNLOADING"
+
+        elif current_ss_name == "UNLOADING" and current_ss_status == "COMPLETED":
+            if self.mission_3_completed:
+                self.status = "COMPLETED"
+                return None
+            else:
+                return "SEARCH_Z1"
+

bb_state_machine/bb_state_machine/shared_data.py

@@ -36,6 +36,7 @@ def __init__(self):
         self._costmap = None
 
         self._front_distance = None
+        self._rear_distance = None
 
     @property
     def x(self):
@@ -105,6 +106,10 @@ def duplo_left_z4(self):
     def current_zone(self):
         return self._current_zone
 
+    @property
+    def zone_2(self):
+        return self._zone_2
+
     @property
     def zone_3(self):
         return self._zone_3
@@ -120,6 +125,10 @@ def costmap(self):
     @property
     def front_distance(self):
         return self._front_distance
+
+    @property
+    def rear_distance(self):
+        return self._rear_distance
 
     def update_position(self, x, y, theta):
         self._x = x
@@ -168,6 +177,9 @@ def update_costmap(self, costmap):
     def update_front_distance(self, front_distance):
         self._front_distance = front_distance
 
+    def update_rear_distance(self, rear_distance):
+        self._rear_distance = rear_distance
+
     def is_circle_free(self, x, y, r, threshold):
         if self.costmap is None:
             return False

bb_state_machine/bb_state_machine/state_auto_nav_a.py

@@ -70,7 +70,11 @@ def update_navigation_status(self):
         dist_current_target = self.distance_to_current_waypoint()
 
         if self.is_reached(dist_current_target):
-            self.advance_waypoint()
+            check_x, check_y, _ = self.waypoints[self.waypoint_index+1]
+            if not self.shared_data.is_circle_free(check_x, check_y, 5, 50):
+                self.waypoints.pop(self.waypoint_index+1)
+            else:
+                self.advance_waypoint()
         elif self.is_last_waypoint(dist_current_target) and not self.manually_navigating:
             self.start_manual_navigation()