diff --git a/README.md b/README.md index b972df97..3fd3f903 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Sample repository showcasing model predictive control with PX4's offboard mode and ROS 2. +# UAV Forge's ROS2 package for GN&C and Aerial Imagery Object Detection. ## Usage @@ -18,6 +18,16 @@ ## Install instructions +### Install required and local Python libraries + +1. cd into this repo's root directory. + +2. Run: + ``` + pip install -e . + ``` + + ### [ROS 2 Foxy for Ubuntu 20.04 Focal](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) 1. Uninstall ROS Noetic. @@ -159,13 +169,3 @@ 1. [Follow these instructions to install Acados.](https://docs.acados.org/installation/) 2. [Install the Python interface afterwards.](https://docs.acados.org/python_interface/index.html) - - -### Install required and local Python libraries (again, for MPC usage) - -1. cd into this repo's root directory. - -2. Run: - ``` - pip install -e . - ``` diff --git a/uavf_2024/__init__.py b/include/uavf_2024/gnc/README.md similarity index 100% rename from uavf_2024/__init__.py rename to include/uavf_2024/gnc/README.md diff --git a/include/uavf_2024/imaging/README.md b/include/uavf_2024/imaging/README.md new file mode 100644 index 00000000..e69de29b diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 00000000..e69de29b diff --git a/scripts/commander_node.py b/scripts/commander_node.py index 4d4e3b7c..b727bdd1 100644 --- a/scripts/commander_node.py +++ b/scripts/commander_node.py @@ -7,7 +7,7 @@ OffboardControlMode, VehicleCommand, VehicleStatus, VehicleOdometry, VehicleGlobalPosition from uavf_ros2_msgs.msg import GpsAltitudePosition, NedEnuOdometry, NedEnuSetpoint, NedEnuWaypoint -from uavf_2024.conversions import convert_quaternion_to_euler_angles, convert_NED_ENU_in_inertial,\ +from uavf_2024.gnc.conversions import convert_quaternion_to_euler_angles, convert_NED_ENU_in_inertial,\ convert_NED_ENU_in_body, convert_body_to_inertial_frame, convert_inertial_to_body_frame import numpy as np from scipy.spatial.transform import Rotation @@ -74,7 +74,7 @@ def __init__(self) -> None: VehicleOdometry, '/fmu/out/vehicle_odometry', self.odom_cb, qos_profile) - self.traj_planner_is_ENU = True + #self.traj_planner_is_ENU = True self.all_wp_reached = False self.waypoint_is_ENU = False self.waypoint = np.zeros(3, dtype=np.float32) @@ -88,7 +88,9 @@ def make_decision(self) -> None: ''' self.publish_heartbeat() if not self.all_wp_reached: - self.publish_trajectory_planner_command(self, self.waypoint, self.waypoint_is_ENU) + self.publish_trajectory_planner_command( + pos_decision=self.waypoint, + pos_is_ENU=self.waypoint_is_ENU) else: self.land() self.disarm() @@ -97,11 +99,20 @@ def make_decision(self) -> None: def traj_plan_cb(self, ned_enu_setpt:NedEnuSetpoint) -> None: ''' Reads the setpoint calculated by the trajectory planner and publishes the corresponding setpoints to PX4. - Incomplete. ''' - # assert something - self.traj_planner_is_ENU = ned_enu_setpt.is_enu - pass + #self.traj_planner_is_ENU = ned_enu_setpt.is_enu + self.publish_trajectory_setpoint( + pos=ned_enu_setpt.position_setpoint, + vel=ned_enu_setpt.velocity_setpoint, + is_ENU=ned_enu_setpt.is_enu) + self.publish_euler_angle_setpoint( + ang=ned_enu_setpt.euler_angle_setpoint, + is_ENU=ned_enu_setpt.is_enu) + self.publish_euler_angle_rate_setpoint( + ang_rate=ned_enu_setpt.euler_angle_rate_setpoint, + is_ENU=ned_enu_setpt.is_enu, + is_inertial=ned_enu_setpt.is_inertial + ) def wp_tracker_cb(self, ned_enu_wp:NedEnuWaypoint) -> None: @@ -110,7 +121,7 @@ def wp_tracker_cb(self, ned_enu_wp:NedEnuWaypoint) -> None: ''' if not ned_enu_wp.all_waypoints_reached: self.waypoint_is_ENU = ned_enu_wp.is_enu - self.waypoint = ned_enu_wp.position_waypoint + self.waypoint = np.float32(ned_enu_wp.position_waypoint) else: self.destroy_subscription(self.wp_tracker_sub) self.all_wp_reached = ned_enu_wp.all_waypoints_reached @@ -265,8 +276,8 @@ def publish_trajectory_setpoint(self, pos:np.ndarray, vel:np.ndarray, is_ENU:boo vel_f32 = np.float32(vel) if is_ENU: - pos_f32 = self.convert_NED_ENU_in_inertial(pos_f32) - vel_f32 = self.convert_NED_ENU_in_inertial(vel_f32) + pos_f32 = convert_NED_ENU_in_inertial(pos_f32) + vel_f32 = convert_NED_ENU_in_inertial(vel_f32) msg = TrajectorySetpoint() msg.position = pos_f32 @@ -286,7 +297,7 @@ def publish_euler_angle_setpoint(self, ang:np.ndarray, is_ENU:bool) -> None: assert type(is_ENU) == bool ang_f32 = np.float32(ang) if is_ENU: - ang_f32 = self.convert_NED_ENU_in_inertial(ang_f32) + ang_f32 = convert_NED_ENU_in_inertial(ang_f32) msg = VehicleAttitudeSetpoint() msg.roll_body = ang_f32[0] @@ -309,9 +320,9 @@ def publish_euler_angle_rate_setpoint(self, ang_rate:float, is_ENU:bool, is_iner assert type(is_inertial) == bool ang_rate_f32 = np.float32(ang_rate) if is_ENU: - ang_rate_f32 = self.convert_NED_ENU_in_body(ang_rate_f32) + ang_rate_f32 = convert_NED_ENU_in_body(ang_rate_f32) if is_inertial: - ang_rate_f32 = self.convert_inertial_to_body_frame(ang_rate_f32, is_ENU=is_ENU) + ang_rate_f32 = convert_inertial_to_body_frame(ang_rate_f32, is_ENU=is_ENU) msg = VehicleRatesSetpoint() msg.roll = ang_rate_f32[0] diff --git a/scripts/trajectory_planner_node.py b/scripts/trajectory_planner_node.py index 21223dde..0a0b0b4c 100644 --- a/scripts/trajectory_planner_node.py +++ b/scripts/trajectory_planner_node.py @@ -5,7 +5,7 @@ from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy from uavf_ros2_msgs.msg import NedEnuOdometry, NedEnuSetpoint -from uavf_2024.sqp_nlmpc import derive_quad_dynamics, SQP_NLMPC +from uavf_2024.gnc.sqp_nlmpc import derive_quad_dynamics, SQP_NLMPC import numpy as np import atexit diff --git a/uavf_2024/gnc/README.md b/uavf_2024/gnc/README.md new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/gnc/__init__.py b/uavf_2024/gnc/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/conversions.py b/uavf_2024/gnc/conversions.py similarity index 100% rename from uavf_2024/conversions.py rename to uavf_2024/gnc/conversions.py diff --git a/uavf_2024/sqp_nlmpc.py b/uavf_2024/gnc/sqp_nlmpc.py similarity index 100% rename from uavf_2024/sqp_nlmpc.py rename to uavf_2024/gnc/sqp_nlmpc.py diff --git a/uavf_2024/imaging/README.md b/uavf_2024/imaging/README.md new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/imaging/__init__.py b/uavf_2024/imaging/__init__.py new file mode 100644 index 00000000..e69de29b