diff --git a/behaviours/alars/alars/alars_bt.py b/behaviours/alars/alars/alars_bt.py index e7e9d4162..6a043355c 100755 --- a/behaviours/alars/alars/alars_bt.py +++ b/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(): diff --git a/behaviours/alars/alars/alars_move_to_action_server.py b/behaviours/alars/alars/alars_move_to_action_server.py index 983f91508..5beb67c9e 100644 --- a/behaviours/alars/alars/alars_move_to_action_server.py +++ b/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 diff --git a/behaviours/alars/alars/alars_recover_action_server.py b/behaviours/alars/alars/alars_recover_action_server.py index 30f9fd5f7..c6cf1096a 100644 --- a/behaviours/alars/alars/alars_recover_action_server.py +++ b/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 diff --git a/messages/dji_msgs/msg/Topics.msg b/messages/dji_msgs/msg/Topics.msg index ff7d6da41..46b406fc8 100644 --- a/messages/dji_msgs/msg/Topics.msg +++ b/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 diff --git a/perception/alars/auv_yolo_detector/auv_yolo_detector/auv_yolo_detector.py b/perception/alars/auv_yolo_detector/auv_yolo_detector/auv_yolo_detector.py index 7e80c88dc..c27306192 100644 --- a/perception/alars/auv_yolo_detector/auv_yolo_detector/auv_yolo_detector.py +++ b/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): """ diff --git a/scripts/smarc_bringups/scripts/dji_bringup.sh b/scripts/smarc_bringups/scripts/dji_bringup.sh index a96834b69..ce396a456 100755 --- a/scripts/smarc_bringups/scripts/dji_bringup.sh +++ b/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 diff --git a/vehicles/hardware/dji/dji_captain/dji_captain/dji_captain.py b/vehicles/hardware/dji/dji_captain/dji_captain/dji_captain.py index 8d6b68a83..a68ded315 100644 --- a/vehicles/hardware/dji/dji_captain/dji_captain/dji_captain.py +++ b/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,12 +459,10 @@ 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: @@ -429,21 +470,7 @@ def status_str(self) -> str: 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)