Alars safety

behaviours/alars/alars/alars_bt.py

@@ -17,7 +17,6 @@
 
 from std_msgs.msg import String, Float32, Int32
 from geographic_msgs.msg import GeoPointStamped, GeoPoint
-from nav_msgs.msg import Odometry
 from geometry_msgs.msg import  PointStamped
 
 
@@ -38,7 +37,7 @@ def __init__(self,
 
             self._node : Node = node
 
-            self.move_to_search_action = A_ActionClient(node, action_client_name='move_to', bt_action_name='move_to_search')
+            self.search_height_action = A_ActionClient(node, action_client_name='move_to', bt_action_name='search_height')
             self.search_action = A_ActionClient(node, 'alars_search')
 
             self.localize_auv_action = A_ActionClient(node, action_client_name='alars_localize', bt_action_name='localize_auv')
@@ -54,6 +53,7 @@ def __init__(self,
 
 
             self._action_clients = [
+                self.search_height_action,
                 self.move_to_delivery_action,
                 self.search_action,
                 self.localize_auv_action,
@@ -134,7 +134,6 @@ def publish_status():
             )
 
             self._goal : dict = {
-                "initial_travel_alt": None,
                 "search_position": {
                     "latitude": None,
                     "longitude": None,
@@ -190,6 +189,7 @@ def _on_goal_received(self, goal_request: dict) -> bool:
         try:
             if not (goal_request.keys() >= self._goal.keys()):
                 self.log("Goal request missing required fields, rejecting.")
+                self.log(f"Missing fields: {self._goal.keys() - goal_request.keys()}")
                 return False
         except Exception as e:
             self.log(f"Exception while checking goal request fields: {e}")
@@ -239,18 +239,25 @@ def _buoy_geopoint_known(self) -> bool:
 
     @property
     def _status_str(self) -> str:
-        str = "\n\nStates:"
+        tip = self._bt.tip() if self._bt is not None else None
+        if tip is None:
+            tip_str = "-"
+        else:
+            tip_str = f"{tip.name}({tip.status})"
+        str = ""
+        str += f"Tip: {tip_str}"
+        str += "\nStates:"
         str += f"\n Delivered: {self.delivered}"
         if self._load_cell_weight is not None:
-            str += f"\n Captured AUV (load cell): {self.captured_auv}({self._load_cell_weight})"
+            str += f"\n Captured(kg): {self.captured_auv}({self._load_cell_weight})"
         elif self._load_cell_raw is not None:
-            str += f"\n Captured AUV (load cell raw): {self.captured_auv}({self._load_cell_raw})"
+            str += f"\n Captured(raw): {self.captured_auv}({self._load_cell_raw})"
         else:
-            str += f"\n Captured AUV: {self.captured_auv} (no load cell data)"
-        str += f"\n AUV in view: {self.auv_in_view}"
-        str += f"\n AUV geopoint known: {self._auv_geopoint_known}"
-        str += f"\n Buoy geopoint known: {self._buoy_geopoint_known}"
-        str += f"\n First search done: {self.first_search_done}"
+            str += f"\n Captured AUV: {self.captured_auv} (none)"
+        str += f"\n AUV visible: {self.auv_in_view}"
+        str += f"\n AUV GP: {self._auv_geopoint_known}"
+        str += f"\n Buoy GP: {self._buoy_geopoint_known}"
+        str += f"\n 1st search done: {self.first_search_done}"
         return str
 
 
@@ -312,7 +319,6 @@ def _set_move_to_goal_delivery(self) -> bool:
         except:
             self.log("Failed to set move_to delivery goal.")
             return False
-
 
 
 
@@ -378,6 +384,25 @@ def _set_buoy_position_from_drone(self) -> bool:
         self._buoy_geopoint_stamped.header.stamp = self._node.get_clock().now().to_msg()
         return True
 
+    def _set_goal_search_height(self) -> bool:
+        if self._drone_geopoint is None:
+            self.log("Drone geopoint not known, cannot set search height.")
+            return False
+        try:
+            g = {"waypoint": {
+                    "latitude": self._drone_geopoint.latitude,
+                    "longitude": self._drone_geopoint.longitude,
+                    "altitude": float(self._goal["search_position"]["altitude"]),
+                    "tolerance": 1.0
+                    }   
+                }
+            self.search_height_action.set_goal(json.dumps(g))
+            self.log("Set search height goal.")
+            return True
+        except Exception as e:
+            self.log(f"Failed to set search height goal: {e}")
+            return False
+
     def _set_goal_search(self) -> bool:
         if self.first_search_done:
             if self._drone_geopoint is None:
@@ -468,6 +493,8 @@ def setup(self) -> bool:
         # if this is the first time searching, we use the given search position
         # in the goal, otherwise we search from where we are
         search = Sequence("SQ Search AUV", memory=True)
+        search.add_child(FuncToStatus("Set search height goal", self._set_goal_search_height))
+        search.add_child(self.search_height_action)
         search.add_child(FuncToStatus("Set search goal", self._set_goal_search))
         search.add_child(self.search_action)
         root.add_child(search)
@@ -476,7 +503,7 @@ def setup(self) -> bool:
 
 
     def _give_feedback(self) -> str:
-        return "No feedback implemented yet."
+        return self._status_str
 
 
 def main():

behaviours/alars/alars/alars_move_to_action_server.py

@@ -173,7 +173,18 @@ def _loop_inner(self) -> bool|None:
                 goal_position[1] = self_position[1]
 
         goal_error = goal_position - self_position
-        goal_error_mag = np.linalg.norm(goal_error)
+        if self._VERTICAL_FIRST_MODE:
+            # consider only the vertical error first
+            vertical_error = abs(goal_error[2])
+            if vertical_error < self._goal_tolerance:
+                # vertically close enough, consider the full 3D error now
+                goal_error_mag = np.linalg.norm(goal_error)
+            else:
+                goal_error_mag = vertical_error
+        else:
+            # otherwise, sphere
+            goal_error_mag = np.linalg.norm(goal_error)
+
         self._distance_remaining = float(goal_error_mag)
 
         # maybe we reached already

behaviours/alars/alars/alars_recover_action_server.py

@@ -114,7 +114,7 @@ def _on_goal_received(self, goal_request: dict) -> bool:
         # |          |
         # B----O-----C 
         # A-B = dipping altitude
-        # B-C = forward distance
+        # B-C = forward distance, at forward altitude
         # C-D = raising altitude
         # O = where the object and buoy are, perpendicular to screen
 

messages/dji_msgs/msg/Topics.msg

@@ -22,6 +22,9 @@ string ESTIMATED_AUV_TOPIC = 'alars_detection/auv' # PointStamped
 string ESTIMATED_MIDDLE_TOPIC = 'alars_detection/middle' # PointStamped
 string ESTIMATED_CNN_HOOK_ANCHOR_TOPIC = 'alars_detection/cnn' # PointStamped
 string ESTIMATED_CNN_AFTER_CATCH_FLY_DIRECTION_TOPIC = 'alars_detection/cnn_vec' # PointStamped
+# instead of captain subbing to image and so on
+# let the consumer of camera data tell it if the camera is good, detection can happen etc.
+string CAM_PROCESSOR_HAPPY_TOPIC = 'alars_detection/cam_processor_happy' # Bool
 
 # object detectors in 3D
 string ESTIMATED_HOOK_TOPIC = 'alars_detection/hook_est' #PoseStamped

perception/alars/auv_yolo_detector/auv_yolo_detector/auv_yolo_detector.py

@@ -19,6 +19,7 @@
 from sensor_msgs.msg import Image
 from geometry_msgs.msg import PointStamped
 from tf2_ros import Buffer, TransformListener
+from std_msgs.msg import Bool
 from std_srvs.srv import Trigger
 
 from dji_msgs.msg import Topics
@@ -56,6 +57,9 @@ def __init__(self, name = 'auv_yolo_detector'):
                                 self.model_params["topics.predicted_position.sam"], 10)
         self.buoy_position_pub = self.create_publisher(PointStamped, 
                                 self.model_params["topics.predicted_position.buoy"], 10)
+
+        self.cam_processor_happy_pub = self.create_publisher(Bool, Topics.CAM_PROCESSOR_HAPPY_TOPIC, 10)
+        self.create_timer(1.0, lambda: self.cam_processor_happy_pub.publish(Bool(data = self.image_is_fresh)))
 
 
         # models 
@@ -77,6 +81,15 @@ def __init__(self, name = 'auv_yolo_detector'):
         self.create_service(Trigger, Topics.ENABLE_ALARS_DETECTOR_SERVICE_TOPIC , self.handle_enable_detector)
         self.create_service(Trigger, Topics.DISABLE_ALARS_DETECTOR_SERVICE_TOPIC , self.handle_disable_detector)
 
+    @property
+    def image_is_fresh(self) -> bool:
+        if self.image is None:
+            return False
+        image_time = self.image.header.stamp.sec + self.image.header.stamp.nanosec * 1e-9
+        now = self.get_clock().now()
+        now_time = now.to_msg().sec + now.to_msg().nanosec * 1e-9
+        return now_time - image_time < 5
+
 
     def classify_callback(self):
         """ 

scripts/smarc_bringups/scripts/dji_bringup.sh

@@ -44,6 +44,12 @@ if [[ -z "$HOME_ABOVE_WATER" ]]; then
     exit 1
 fi
 
+if ! [[ "$HOME_ABOVE_WATER" =~ ^[0-9]+\.[0-9]+$ ]]; then
+    echo "HOME_ABOVE_WATER must be a floating point number! Adding a decimal point for you..."
+    HOME_ABOVE_WATER="${HOME_ABOVE_WATER}.0"
+    echo "HOME_ABOVE_WATER is set to $HOME_ABOVE_WATER"
+fi
+
 # New variables for wasp_bt.launch and wasp_mqtt_agent.launch
 AGENT_TYPE=air
 PULSE_RATE=10.0

vehicles/hardware/dji/dji_captain/dji_captain/dji_captain.py

@@ -12,7 +12,7 @@
 from tf2_ros import Buffer, TransformListener
 
 
-from std_msgs.msg import Float32, Int8, String
+from std_msgs.msg import Float32, Int8, String, Bool
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import NavSatFix, Joy, BatteryState, JoyFeedback
 from nav_msgs.msg import Odometry
@@ -106,6 +106,13 @@ def __init__(self, node: Node):
             self.log("Captain will not run. Exiting.")
             sys.exit(1)
 
+        self._node.declare_parameter("min_altitude_above_water", 1.5)
+        self.MIN_ALTITUDE_ABOVE_WATER = self._node.get_parameter("min_altitude_above_water").get_parameter_value().double_value
+        if self.MIN_ALTITUDE_ABOVE_WATER <= 0:
+            self.log(f"Warning: min_altitude_above_water parameter not set or invalid! It is {self.MIN_ALTITUDE_ABOVE_WATER}")
+            self.log("Setting it to 1.5m to prevent damage to the vehicle, but you should set it to something appropriate for your mission!")
+            self.MIN_ALTITUDE_ABOVE_WATER = 1.5
+
 
 
         self._move_to_setpoint : PoseStamped | None = None
@@ -161,6 +168,7 @@ def __init__(self, node: Node):
         self._flying : bool = False
         self._carrying_payload : bool = False
         self._battery_percent : float | None = None
+        self._cam_processor_happy : bool = False
 
         # this could be a param, but really we likely will never run this on anything except
         # the M350 which has a nominal 3kg max payload, so hardcoding it here is fine.
@@ -296,6 +304,13 @@ def __init__(self, node: Node):
             DjiTopics.LOAD_CELL_WEIGHT_TOPIC,
             self._load_cell_callback,
             qos_profile=10)
+
+        node.create_subscription(
+            Bool,
+            DjiTopics.CAM_PROCESSOR_HAPPY_TOPIC,
+            self._cam_processor_happy_callback,
+            qos_profile=10
+        )
 
 
 
@@ -385,28 +400,56 @@ def now_time(self):
     def setpoint_received_at(self) -> float|None:
         return self._move_to_setpoint.header.stamp.sec + self._move_to_setpoint.header.stamp.nanosec * 1e-9 if self._move_to_setpoint is not None else None
 
+    @property
+    def altitude_above_water(self) -> float:
+        if self._base_pose_in_home is not None:
+            return self._base_pose_in_home.pose.position.z + self._HOME_ALT_ABOVE_WATER
+        else:
+            return -1.0
+
 
     @property
     def status_str(self) -> str:
         s = "\nDjiCaptain Status:\n"
-        s += f"  Home in UTM: {format_point_stamped(self._home_point_in_utm)} ({self._utm_zb_label})\n"
-        s += f"\n  Position in Home: {format_pose_stamped(self._base_pose_in_home)}\n"
-
         if self._battery_percent is not None:
             s += f"  Battery Percent: {self._battery_percent:.2f} (ready:{self.READY_BATTERY_PERCENTAGE}, error:{self.ERROR_BATTERY_PERCENTAGE})\n"
         else:
             s += f"  Battery Percent: N/A\n"
 
+        s += f"  Cam Proc Happy: {self._cam_processor_happy}\n"
+
         if self._load_cell_weight is not None:
             s += f"  Load Cell Weight: {self._load_cell_weight:.2f} kg (max: {self._MAX_LOAD_KG} kg)\n"
         else:
             s += f"  Load Cell Weight: N/A\n"
+
+        s += f"  Got Control: {self._got_control}\n"
+        s += f"  Flying: {self._flying}\n"
 
         if self._base_pose_in_home is not None:
-            s += f"  Altitude from water: {self._base_pose_in_home.pose.position.z + self._HOME_ALT_ABOVE_WATER:.2f} m\n"
+            s += f"  Altitude from water: {self.altitude_above_water:.2f} m\n"
         else:
             s += f"  Altitude from water: N/A, base pose in home not known!\n"
 
+        if self._last_pubbed_fluvel_joy is not None:
+            a = self._last_pubbed_fluvel_joy.axes
+            t = self._last_pubbed_fluvel_joy.header.stamp.sec + self._last_pubbed_fluvel_joy.header.stamp.nanosec * 1e-9
+            s += f"  Last FLUVel Joy (XYZ): [{a[0]:.2f}, {a[1]:.2f}, {a[2]:.2f}, {a[3]:.2f}] ({self.now_time - t:.2f}s ago)\n"
+        else:
+            s += f"  Last FLUVel Joy: None\n"
+
+        if self._vehicle_health.data == SmarcTopics.VEHICLE_HEALTH_READY:
+            s += f"  Vehicle Health: READY\n"
+        elif self._vehicle_health.data == SmarcTopics.VEHICLE_HEALTH_ERROR:
+            s += f"  Vehicle Health: ERROR\n"
+        else:
+            s += f"  Vehicle Health: WAITING\n"
+
+        s += "========================\n"
+
+        s += f"  Home in UTM: {format_point_stamped(self._home_point_in_utm)} ({self._utm_zb_label})\n"
+        s += f"  Position in Home: {format_pose_stamped(self._base_pose_in_home)}\n"
+
         if self._heading_deg is not None: s += f"  Heading: {self._heading_deg:.2f}\n"
         else: s += f"  Heading: N/A\n"
 
@@ -416,34 +459,18 @@ def status_str(self) -> str:
         s += f"  Velocity Ground: {format_vector3_stamped(self._velocity_ground)}\n"
         s += f"  Angular Rate Ground: {format_vector3_stamped(self._angular_rate_ground)}\n"
 
-        s += f"\n  Smarc Topics: {self._smarc_pub_status}\n"
+        s += f"  Smarc Topics:{self._smarc_pub_status}\n"
 
         s += f"  TF: {self._tf_pub_status}\n"
 
-        s += f"\n  Got Control: {self._got_control}\n"
-
         if self.setpoint_received_at is None and self._move_to_setpoint is None:
             s += f"  No setpoint set.\n"
         elif self.setpoint_received_at is None and self._move_to_setpoint is not None:
             s += f"  Setpoint received time unknown, this is a bug! FIX THIS\n"
         elif self.setpoint_received_at is not None and self._move_to_setpoint is not None:
             s += f"  Current target setpoint: {format_pose_stamped(self._move_to_setpoint)} ({self.now_time - self.setpoint_received_at:.2f}s ago)\n"
 
-        s += f"  Flying: {self._flying}\n"
-
-        if self._last_pubbed_fluvel_joy is not None:
-            a = self._last_pubbed_fluvel_joy.axes
-            t = self._last_pubbed_fluvel_joy.header.stamp.sec + self._last_pubbed_fluvel_joy.header.stamp.nanosec * 1e-9
-            s += f"  Last FLUVel Joy (XYZ): [{a[0]:.2f}, {a[1]:.2f}, {a[2]:.2f}, {a[3]:.2f}] ({self.now_time - t:.2f}s ago)\n"
-        else:
-            s += f"  Last FLUVel Joy: None\n"
 
-        if self._vehicle_health.data == SmarcTopics.VEHICLE_HEALTH_READY:
-            s += f"  Vehicle Health: READY\n"
-        elif self._vehicle_health.data == SmarcTopics.VEHICLE_HEALTH_ERROR:
-            s += f"  Vehicle Health: ERROR\n"
-        else:
-            s += f"  Vehicle Health: WAITING\n"
 
         return s
 
@@ -497,6 +524,9 @@ def _geo_alt_cb(self, msg: Float32):
     def _load_cell_callback(self, msg: Float32):
         self._load_cell_weight = msg.data
 
+    def _cam_processor_happy_callback(self, msg: Bool):
+        self._cam_processor_happy = msg.data
+
 
     def _move_to_setpoint_callback(self, msg: PoseStamped):
         # check if the message even has anything in it
@@ -963,6 +993,11 @@ def _publish_vehicle_health(self):
                 self._vehicle_health.data = SmarcTopics.VEHICLE_HEALTH_ERROR
                 self.log(f"WEIGHT ABOVE LIMIT: {self._load_cell_weight:.2f} > {self._MAX_LOAD_KG:.2f}")
 
+            water_altitude_error = self.altitude_above_water < self.MIN_ALTITUDE_ABOVE_WATER
+            if water_altitude_error:
+                self._vehicle_health.data = SmarcTopics.VEHICLE_HEALTH_ERROR
+                self.log(f"TOO CLOSE TO WATER: {self.altitude_above_water:.2f} < {self.MIN_ALTITUDE_ABOVE_WATER:.2f}")
+
 
         self._vehicle_health_pub.publish(self._vehicle_health)