diff --git a/controls/sae_2025_ws/README.md b/controls/sae_2025_ws/README.md index 670d9220..05e59670 100644 --- a/controls/sae_2025_ws/README.md +++ b/controls/sae_2025_ws/README.md @@ -120,6 +120,26 @@ You might run into the following issues during the build process. Here are solut cd ~/{path_to_monorepo}/controls/sae_2025_ws/src git clone git@github.com:rudislabs/actuator_msgs.git ``` +5. **Missing `tf_transformations`**: + + ```bash + sudo apt install tf_transformations + ``` + +6. **`numpy` version is wrong:** + + You might see one of two errors: + - `AttributeError: module 'numpy' has no attribute `** + - `AttributeError: `np.maximum_sctype` was removed in the NumPy 2.0 release` + ```bash + pip install numpy==1.21.5 + ``` + +7. **Missing `generate_parameter_library`:** + + ```bash + sudo apt install ros-humble-generate-parameter-library + ``` --- diff --git a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml index b00b8a5f..894a1d9b 100644 --- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml +++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml @@ -1,16 +1,16 @@ # launch_params.yaml -mission_name: basic_waypoints +mission_name: save_image uav_debug: false -vision_debug: false +vision_debug: true run_mission: true -airframe: standard_vtol # Enter a preset below or a valid airframe ID (integer), either custom or from PX4 +airframe: quadcopter # Enter a preset below or a valid airframe ID (integer), either custom or from PX4 # quadcopter, tiltrotor_vtol, fixed_wing, standard_vtol, quadtailsitter # Other useful IDs: 4014 for mono cam down # IDs available for simulation can be found in PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes # These IDs load an airframe file and the corresponding model file in PX4-Autopilot/Tools/simulation/gz/models -use_camera: false +use_camera: true custom_airframe_model: '' # Leave blank if using default PX4 model or enter file name of custom model -save_vision: false +save_vision: true servo_only: false camera_offsets: [0, 0, 0] # Should denote the position of the camera relative to the payload mechanism, in meters # In NED frame: x is forward, y is right, and z is down. diff --git a/controls/sae_2025_ws/src/uav/launch/main.launch.py b/controls/sae_2025_ws/src/uav/launch/main.launch.py index c8a6b887..5df86700 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -67,6 +67,7 @@ def launch_setup(context, *args, **kwargs): )) for node in extract_vision_nodes(YAML_PATH): + print(f"Adding vision node: {node}") vision_nodes.append(node) # Convert CamelCase node names to snake_case executable names. s1 = re.sub('(.)([A-Z][a-z]+)', r'\1_\2', node) @@ -78,6 +79,7 @@ def launch_setup(context, *args, **kwargs): output='screen', parameters=[{'debug': vision_debug_bool, 'sim': sim_bool, 'save_vision': save_vision_bool}], )) + print(f"Final vision nodes to launch: {vision_nodes}") # Clear vision node actions if none are found. if len(vision_nodes) == 0: diff --git a/controls/sae_2025_ws/src/uav/setup.py b/controls/sae_2025_ws/src/uav/setup.py index c41a7a62..6522acab 100644 --- a/controls/sae_2025_ws/src/uav/setup.py +++ b/controls/sae_2025_ws/src/uav/setup.py @@ -35,6 +35,7 @@ 'global_position_offboard_control = uav.global_position_offboard_control:main', 'mission = uav.mission:main', 'payload_tracking_node = uav.vision_nodes.PayloadTrackingNode:main', + 'save_image_node = uav.vision_nodes.SaveImageNode:main', 'camera = uav.CameraNode:main', ], }, diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py new file mode 100644 index 00000000..c14480ad --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py @@ -0,0 +1,113 @@ +import math +from typing import List +from uav.autonomous_modes import Mode +from rclpy.node import Node +from uav import UAV +from uav.vision_nodes import SaveImageNode +class SaveImageMode(Mode): + """ + A mode for navigating to a GPS coordinate + """ + + def __init__(self, node: Node, uav: UAV, coordinates: List[tuple[tuple[float, float, float], float, str]], margin: float = 0.5): + """ + Initialize the NavigateToGPSMode. + + Args: + node (Node): ROS 2 node managing the UAV. + uav (UAV): The UAV instance to control. + coordinates (List[tuple[tuple[float, float, float], float, str]]): The coordinates to navigate to (x/y/z or lon/lat/alt, wait time, GPS/LOCAL). + Local are NED coordinates, relative to the starting position (https://docs.px4.io/main/en/ros2/user_guide.html#ros-2-px4-frame-conventions). + margin (float): The margin of error for the GPS coordinate. + """ + super().__init__(node, uav) + self.coordinates = coordinates + self.goal = None + self.margin = margin + self.circle_pause_time = 2 + self.circle_radius = 0 + self.is_circling = False + self.index = -1 + self.angle = 0 + self.angle_increment = 15 + self.circle_cycle = 360 / self.angle_increment + + def on_update(self, time_delta: float) -> None: + """ + Periodic logic for setting gps coord. + """ + dist = 0 + if self.index != -1: + dist = self.uav.distance_to_waypoint(self.coordinate_system, self.goal) + # self.log(f"Distance to waypoint: {dist}, current position: {self.uav.get_local_position()}") + + if dist >= self.margin and not self.is_circling: + self.uav.publish_position_setpoint(self.target) # PX4 expects stream of setpoints + self.circle_radius = -self.target[2] / 3 + elif self.goal is None or dist < self.margin + self.circle_radius: + if self.index == -1 or self.wait_time <= 0: + self.index += 1 + if self.index >= len(self.coordinates): + return + self.goal, self.wait_time, self.coordinate_system = self.coordinates[self.index] + self.target = self.get_local_target() + self.uav.publish_position_setpoint(self.target) + else: + if self.circle_cycle >= 0: + self.is_circling = True + circle_target = list(self.target) + angle_in_radians = self.angle * math.pi / 180 + circle_target[0] += self.circle_radius * math.cos(angle_in_radians) + circle_target[1] += self.circle_radius * math.sin(angle_in_radians) + + self.uav.publish_position_setpoint(tuple(circle_target)) + + if self.circle_pause_time > 0: + self.circle_pause_time -= time_delta + # self.log(f"Angle: {self.angle}") + # self.log(f"Pausing to take photos for: {self.circle_pause_time} more seconds") + return + + self.circle_cycle -= 1 + self.angle += 5 + + if self.angle % 15 == 0: + self.circle_pause_time = 2 + else: + pass + # self.log(f"Angle: {self.angle}") + # self.log(f"Holding - circling for {self.circle_cycle} more cycles.") + + elif self.wait_time >= 0: + self.wait_time -= time_delta + if self.wait_time <= 0: + self.angle = 0 + self.is_circling = False + self.circle_cycle = 360 / self.angle_increment + # self.log(f"Holding - waiting for {self.wait_time} more seconds") + + def get_local_target(self) -> tuple[float, float, float]: + """ + Get the local target of the UAV. + + Returns: + tuple[float, float, float]: The local target of the UAV. + """ + if self.coordinate_system == "GPS": + return self.uav.gps_to_local(self.goal) + elif self.coordinate_system == "LOCAL": + return tuple(float(x) for x in self.goal) + else: + raise ValueError(f"Invalid coordinate system {self.coordinate_system}") + + def check_status(self) -> str: + """ + Check the status of the mode. + + Returns: + str: The status of the mode. + """ + if self.index >= len(self.coordinates): + return "complete" + else: + return "continue" \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py index 6dabab0c..6195d293 100644 --- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py @@ -5,4 +5,5 @@ from .NavGPSMode import NavGPSMode from .TransitionMode import TransitionMode from .ServoDropoffMode import ServoDropoffMode -from .WaypointMission import WaypointMission \ No newline at end of file +from .WaypointMission import WaypointMission +from .SaveImageMode import SaveImageMode \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml new file mode 100644 index 00000000..ef4d8757 --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml @@ -0,0 +1,9 @@ +start: + class: uav.autonomous_modes.SaveImageMode + params: + coordinates: '[((0, 0, -6), 10, "LOCAL"), ((0, 20, -3), 10, "LOCAL")]' + transitions: + complete: land + +land: + class: uav.autonomous_modes.LandingMode \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py new file mode 100644 index 00000000..d4c2f7cc --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py @@ -0,0 +1,54 @@ +# payload_tracking_node.py +import cv2 +import numpy as np +from uav.vision_nodes.VisionNode import VisionNode +import rclpy +from cv_bridge import CvBridge +from uav.utils import pink, green, blue, yellow +from geometry_msgs.msg import Vector3 +from std_msgs.msg import String, Float64MultiArray +import time +import os +from std_srvs.srv import Empty + +from px4_msgs.msg import VehicleLocalPosition +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + +class SaveImageNode(VisionNode): + """ + ROS node for hoop tracking with OpenCV Algorithm. + """ + srv = Empty + def __init__(self): + super().__init__('save_image_node', display=False, use_service=False) + self.bridge = CvBridge() + self.begin_time = time.time() + self.time_elapsed = self.begin_time + # add an empty service server for ModeManager + self.create_service(self.srv, self.service_name(), self.service_callback) + self.get_logger().info(f"save_image empty service '{self.service_name()}' started.") + + def service_callback(self, request, response): + self.get_logger().info("Empty service called (placeholder).") + return response + def image_callback(self, msg): + if time.time() - self.time_elapsed > 2: + self.time_elapsed = time.time() + self.get_logger().info("Received image for save") + image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + image_time = int(time.time()) + os.system(f"mkdir -p ~/saved_imgs/{int(self.begin_time)}") + path = os.path.expanduser(f"~/saved_imgs/{int(self.begin_time)}") + self.get_logger().info(os.path.join(path, f"payload_{image_time}.png")) + cv2.imwrite(os.path.join(path, f"payload_{image_time}.png"), image) + +def main(): + rclpy.init() + node = SaveImageNode() + try: + rclpy.spin(node) + except Exception as e: + print(e) + node.publish_failsafe() + + rclpy.shutdown() \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py index 41b97014..97f08684 100644 --- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py +++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py @@ -1,2 +1,3 @@ from .VisionNode import VisionNode # make sure to import the parent class FIRST (to avoid circular imports) -from .PayloadTrackingNode import PayloadTrackingNode \ No newline at end of file +from .PayloadTrackingNode import PayloadTrackingNode +from .SaveImageNode import SaveImageNode \ No newline at end of file