From fb5577a284c772324fb24c1b9893075a2adc6d40 Mon Sep 17 00:00:00 2001 From: Kanishke Gamagedara Date: Sun, 1 Sep 2024 22:29:54 -0400 Subject: [PATCH] ROS2 upgrade (#14) * Migrate to ROS2 * Add Qt-based GUI * Update docker and README --- .gitignore | 32 + Dockerfile | 14 +- README.md | 40 +- docker_run.sh | 6 +- scripts/gui.py | 224 ------- scripts/main.py | 116 +++- scripts/rover.py | 104 +-- scripts/thread_control.py | 82 ++- scripts/thread_gps.py | 57 +- scripts/thread_imu.py | 80 ++- src/fdcl_uav/fdcl_uav/__init__.py | 0 {scripts => src/fdcl_uav/fdcl_uav}/control.py | 246 +++++++- .../fdcl_uav/fdcl_uav}/estimator.py | 160 ++++- src/fdcl_uav/fdcl_uav/freq_utils.py | 29 + src/fdcl_uav/fdcl_uav/gui.py | 591 ++++++++++++++++++ .../fdcl_uav/fdcl_uav}/integral_utils.py | 0 .../fdcl_uav/fdcl_uav}/matrix_utils.py | 0 .../fdcl_uav/fdcl_uav}/trajectory.py | 172 +++-- src/fdcl_uav/launch/fdcl_uav_launch.py | 30 + src/fdcl_uav/package.xml | 18 + src/fdcl_uav/resource/fdcl_uav | 0 src/fdcl_uav/setup.cfg | 4 + src/fdcl_uav/setup.py | 32 + src/uav_gazebo/CMakeLists.txt | 42 +- src/uav_gazebo/launch/simple_world.launch | 16 - src/uav_gazebo/launch/uav_gazebo.launch.py | 60 ++ src/uav_gazebo/model.config | 16 + src/uav_gazebo/msg/DesiredData.msg | 28 + src/uav_gazebo/msg/ErrorData.msg | 8 + src/uav_gazebo/msg/ModeData.msg | 5 + src/uav_gazebo/msg/StateData.msg | 7 + src/uav_gazebo/package.xml | 61 +- src/uav_gazebo/urdf/uav.gazebo | 40 -- src/uav_gazebo/urdf/uav.urdf.xacro | 72 +++ src/uav_gazebo/urdf/uav.xacro | 26 - src/uav_gazebo/worlds/simple.world | 11 + src/uav_plugins/CMakeLists.txt | 212 +------ src/uav_plugins/package.xml | 62 +- ...trol_plugin.cpp => uav_control_plugin.cpp} | 125 ++-- 39 files changed, 1931 insertions(+), 897 deletions(-) delete mode 100644 scripts/gui.py create mode 100644 src/fdcl_uav/fdcl_uav/__init__.py rename {scripts => src/fdcl_uav/fdcl_uav}/control.py (70%) rename {scripts => src/fdcl_uav/fdcl_uav}/estimator.py (63%) create mode 100644 src/fdcl_uav/fdcl_uav/freq_utils.py create mode 100644 src/fdcl_uav/fdcl_uav/gui.py rename {scripts => src/fdcl_uav/fdcl_uav}/integral_utils.py (100%) rename {scripts => src/fdcl_uav/fdcl_uav}/matrix_utils.py (100%) rename {scripts => src/fdcl_uav/fdcl_uav}/trajectory.py (67%) create mode 100644 src/fdcl_uav/launch/fdcl_uav_launch.py create mode 100644 src/fdcl_uav/package.xml create mode 100644 src/fdcl_uav/resource/fdcl_uav create mode 100644 src/fdcl_uav/setup.cfg create mode 100644 src/fdcl_uav/setup.py delete mode 100644 src/uav_gazebo/launch/simple_world.launch create mode 100644 src/uav_gazebo/launch/uav_gazebo.launch.py create mode 100644 src/uav_gazebo/model.config create mode 100644 src/uav_gazebo/msg/DesiredData.msg create mode 100644 src/uav_gazebo/msg/ErrorData.msg create mode 100644 src/uav_gazebo/msg/ModeData.msg create mode 100644 src/uav_gazebo/msg/StateData.msg delete mode 100644 src/uav_gazebo/urdf/uav.gazebo create mode 100644 src/uav_gazebo/urdf/uav.urdf.xacro delete mode 100644 src/uav_gazebo/urdf/uav.xacro rename src/uav_plugins/src/{control_plugin.cpp => uav_control_plugin.cpp} (50%) diff --git a/.gitignore b/.gitignore index 1693048..abf12aa 100644 --- a/.gitignore +++ b/.gitignore @@ -366,6 +366,38 @@ tags # Persistent undo [._]*.un~ +### ROS2 ### +install/ +log/ +build/ + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +*~ + +# Emacs +.#* + +# Colcon custom files +COLCON_IGNORE +AMENT_IGNORE + + ### vscode ### .vscode/* !.vscode/settings.json diff --git a/Dockerfile b/Dockerfile index 6ec5823..655624a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,15 +1,11 @@ -FROM ros:noetic-ros-core-focal +FROM ros:iron-ros-core-jammy RUN apt update -RUN apt -y install python3-pip python3-gi python3-pip tmux -RUN apt -y install mesa-utils libgl1-mesa-glx dbus -RUN apt -y install libgtk-3-dev python3-gi gobject-introspection -RUN apt -y install ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control ros-noetic-xacro +RUN apt -y install python3-pip tmux +RUN apt -y install python3-colcon-common-extensions +RUN apt -y install ros-iron-gazebo-ros-pkgs ros-iron-gazebo-ros2-control ros-iron-xacro RUN python3 -m pip install pip --upgrade RUN python3 -m pip install numpy pandas matplotlib - -RUN python3 -m pip install -U rosdep -RUN sudo rosdep init -RUN rosdep update +RUN python3 -m pip install PyQt5 RUN adduser --disabled-password sim USER sim diff --git a/README.md b/README.md index d15dad9..c1303a0 100644 --- a/README.md +++ b/README.md @@ -95,11 +95,8 @@ If you want to install everything locally, follow [Local Install](#local-install If you want to run a docker container instead, skip to [Docker Setup](#docker-setup). #### Local Install -1. [ROS](http://wiki.ros.org/): this repository has been developed using ROS Noetic, on Ubuntu 20.04. If you are on ROS Melodic with Ubuntu 18.04, please checkout `ros-melodic` branch before installing dependencies. -1. Python GTK libraries for GUI (not required if you opt to not to use the GUI) - ```sh - sudo apt-get install python3-pip python3-gi - ``` +1. [ROS2](http://wiki.ros.org/): this repository has been developed using ROS2 Iron, on Ubuntu 22.04. If you are on a different version of Ubunto or ROS, please check the previous releases before installing dependencies. We recommend installing the ROS2 full version. + 1. Python modules: these libraries must be installed in the system 1. NumPy 1. Pandas @@ -127,7 +124,7 @@ The first time you run the build command will take a while as it installs all th You can skip the build command altogether by pulling the built docker from the Docker Hub with the following command. This is NOT required if you are building it locally using the build command. ```sh -docker pull kanishgama/uav_simulator:ros-noetic +docker pull kanishgama/uav_simulator:ros2-iron bash docker_run.sh ``` @@ -142,40 +139,47 @@ You only need to do the followings once (unless you change the Gazebo plugins) 1. Make the plugging. ```sh # From uav_simulator - catkin_make + colcon build ``` 1. Source the relevant directories (**NOTE**: you need to do this on every new terminal). ```sh # From uav_simulator - cd devel && source setup.bash && cd ../ + source install/local_setup.bash ``` ### Running the simulation environment 1. In the current terminal window, launch the Gazebo environment: ```sh # From uav_simulator - roslaunch uav_gazebo simple_world.launch + ros2 launch uav_gazebo uav_gazebo.launch.py ``` -1. Once the Gazebo is launched, run the rover code from a different rover terminal (if you already don't know, you may find [**tmux**](https://github.com/tmux/tmux/wiki) a life-saver): +1. Once the Gazebo is launched, run the UAV code from a different terminal (if you already don't know, you may find [**tmux**](https://github.com/tmux/tmux/wiki) a life-saver): ```sh - # From uav_simulator/scripts - python3 main.py + # From uav_simulator/src/fdcl_uav/launch + ros2 launch fdcl_uav_launch.py ``` - If you change the Python code, simply re-run the Python code. - The code has been tested with Python3.8.10, which comes default with Ubuntu 20.04. + + If you change the Python code, run the following command + ```sh + # From uav_simulator + colcon build --packages-select fdcl_uav + ``` + Then, re-run the above launch command. + + The code has been tested with Python3.10.12, which comes default with Ubuntu 22.04. ![Terminal](images/running.gif) ### Tips 1. Every time you change the simulation environment, you have to kill the program, `catkin_make` and re-run it. 1. If you do not make any changes to the simulation environment, you only need to kill the Python program. -1. The UAV will re-spawn at the position and orientation defined in `reset_uav()` in `rover.py` when you run the Python code. + ## Control Guide * Simply click on the buttons on the GUI to control the UAV. * You can easily switch between each trajectory mode simply clicking on the radio buttons. * Stay mode simply commands the UAV to stay at the current position. -* When take-off, stay, and circle trajectories end, the UAV switched to the "manual" mode. +* When take-off, stay, and circle trajectories end, the UAV switches to the "manual" mode. * When the UAV is in manual, you can use following keys (these are not case sensitive): * `WASD`: to move in horizontal plane * `P`: increase altitude @@ -192,4 +196,6 @@ You only need to do the followings once (unless you change the Gazebo plugins) * Make sure you are in the main directory. * Run `python -m unittest`. * Unit tests have only been tested on Python 3.9. -* Currently, unit test only covers the `matrix_utils.py` module. \ No newline at end of file +* Currently, unit test only covers the `matrix_utils.py` module. + + \ No newline at end of file diff --git a/docker_run.sh b/docker_run.sh index e18794d..74eeec4 100644 --- a/docker_run.sh +++ b/docker_run.sh @@ -1,5 +1,5 @@ docker run \ - --rm \ + --name uav-simulator-container \ --mount source="$(pwd)",target=/home/uav_simulator,type=bind \ --net host \ -v /tmp/.X11-unix:/tmp/.X11-unix \ @@ -7,3 +7,7 @@ docker run \ -e DISPLAY=unix$DISPLAY \ --privileged \ -it uav_simulator bash + +# For debugging: once the container is first run with the above command, you +# can run the following command for the second and subsequent runs. +# docker start --interactive uav-simulator-container \ No newline at end of file diff --git a/scripts/gui.py b/scripts/gui.py deleted file mode 100644 index b5c8c44..0000000 --- a/scripts/gui.py +++ /dev/null @@ -1,224 +0,0 @@ -from rover import rover -from plot_utils import plot_data - -import gi -import numpy as np -import os - -gi.require_version('Gtk', '3.0') -from gi.repository import Gtk, GLib, Gdk - - -class Gui(): - def __init__(self): - - self.builder = Gtk.Builder() - self.builder.add_from_file("gui.glade") - - self.window = self.builder.get_object('window_main') - self.window.set_title('UAV Simulator') - self.window.connect('destroy', Gtk.main_quit) - self.window.connect('key-press-event', self.on_key_press) - - self.btn_close = self.builder.get_object('btn_close') - self.btn_close.connect('clicked', self.on_btn_close_clicked) - - self.tgl_motor_on = self.builder.get_object('tgl_motor_on') - self.tgl_motor_on.connect('toggled', self.on_tgl_motor_on_toggled) - - self.tgl_save = self.builder.get_object('tgl_save') - self.tgl_save.connect('toggled', self.on_tgl_save_toggled) - - self.lbl_save = self.builder.get_object('lbl_save') - self.lbl_save.set_text(rover.t0.strftime('log_%Y%m%d_%H%M%S.txt')) - - self.btn_plot = self.builder.get_object('btn_plot') - self.btn_plot.connect('clicked', self.on_btn_plot_clicked) - - self.lbl_t = self.builder.get_object('lbl_t') - self.lbl_freq_imu = self.builder.get_object('lbl_freq_imu') - self.lbl_freq_gps = self.builder.get_object('lbl_freq_gps') - self.lbl_freq_control = self.builder.get_object('lbl_freq_control') - self.lbl_freq_log = self.builder.get_object('lbl_freq_log') - - self.modes = ['Idle', 'Warm-up', 'Take-off', 'Land', 'Stay', 'Circle'] - self.rdo_mode = [] - for i in range(len(self.modes)): - self.rdo_mode.append( - self.builder.get_object('rdo_mode_{}'.format(i))) - self.rdo_mode[i].set_label(self.modes[i]) - self.rdo_mode[i].connect('toggled', self.on_rdo_buton_clicked, i) - - self.x = self.get_vbox('lbl_x') - self.v = self.get_vbox('lbl_v') - self.a = self.get_vbox('lbl_a') - self.W = self.get_vbox('lbl_W') - self.R = self.get_grid('lbl_R') - - self.xd = self.get_vbox('lbl_xd') - self.vd = self.get_vbox('lbl_vd') - self.Wd = self.get_vbox('lbl_Wd') - self.b1d = self.get_vbox('lbl_b1d') - self.Rd = self.get_grid('lbl_Rd') - - self.window.connect('destroy', Gtk.main_quit) - self.window.show_all() - - - def on_btn_close_clicked(self, *args): - print('GUI: close button clicked') - rover.on = False - Gtk.main_quit() - - - def on_btn_plot_clicked(self, widget): - print('GUI: generating plots ..') - # plot_data() - os.system('python3 plot_utils.py') - - - def on_tgl_save_toggled(self, widget): - if self.tgl_save.get_active(): - print('GUI: saving data started ..') - rover.save_on = True - else: - print('GUI: saving data stopped') - rover.save_on = False - - - def on_tgl_motor_on_toggled(self, widget): - if self.tgl_motor_on.get_active(): - print('GUI: motor on ..') - rover.motor_on = True - else: - print('GUI: motor off') - rover.motor_on = False - - - def on_rdo_buton_clicked(self, widget, num): - if widget.get_active(): - print('GUI: mode switched to {}'.format(self.modes[num])) - rover.mode = num - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - - - def on_key_press(self, widget, event): - key = event.keyval - - x_step = 0.1 - yaw_step = 0.02 - - if key == Gdk.KEY_M or key == Gdk.KEY_m: - print('GUI: turning motors off') - rover.motor_on = False - self.tgl_motor_on.set_active(False) - elif key == Gdk.KEY_0: - self.rdo_mode[0].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_1: - self.rdo_mode[1].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_2: - self.rdo_mode[2].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_3: - self.rdo_mode[3].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_4: - self.rdo_mode[4].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_5: - self.rdo_mode[5].set_active(True) - rover.x_offset = np.zeros(3) - rover.yaw_offset = 0.0 - elif key == Gdk.KEY_W or key == Gdk.KEY_w: - rover.x_offset[0] += x_step - elif key == Gdk.KEY_S or key == Gdk.KEY_s: - rover.x_offset[0] -= x_step - elif key == Gdk.KEY_D or key == Gdk.KEY_d: - rover.x_offset[1] += x_step - elif key == Gdk.KEY_A or key == Gdk.KEY_a: - rover.x_offset[1] -= x_step - elif key == Gdk.KEY_L or key == Gdk.KEY_l: - rover.x_offset[2] += x_step - elif key == Gdk.KEY_P or key == Gdk.KEY_p: - rover.x_offset[2] -= x_step - elif key == Gdk.KEY_E or key == Gdk.KEY_e: - rover.yaw_offset += yaw_step - elif key == Gdk.KEY_Q or key == Gdk.KEY_q: - rover.yaw_offset -= yaw_step - - - def update_gui(self): - self.lbl_t.set_text('t = {:0.1f} s'.format(rover.t)) - self.lbl_freq_imu.set_text( \ - 'IMU = {:3.0f} Hz'.format(rover.freq_imu)) - self.lbl_freq_gps.set_text( - 'GPS = {:3.0f} Hz'.format(rover.freq_gps)) - self.lbl_freq_control.set_text( - 'CTRL = {:3.0f} Hz'.format(rover.freq_control)) - self.lbl_freq_log.set_text( - 'LOG = {:3.0f} Hz'.format(rover.freq_log)) - - self.update_vbox(self.x, rover.x) - self.update_vbox(self.v, rover.v) - self.update_vbox(self.a, rover.a) - self.update_vbox(self.W, rover.W) - self.update_grid(self.R, rover.R) - - self.update_vbox(self.xd, rover.control.xd) - self.update_vbox(self.vd, rover.control.xd_dot) - self.update_vbox(self.b1d, rover.control.b1d) - self.update_vbox(self.Wd, rover.control.Wd) - self.update_grid(self.Rd, rover.control.Rd) - - return True - - - def get_vbox(self, name): - vbox = [] - for i in range(3): - vbox.append(self.builder.get_object('{}_{}'.format(name, i + 1))) - return vbox - - - def update_vbox(self, vbox, data): - for i in range(3): - vbox[i].set_text('{:8.2f}'.format(data[i])) - - - def get_grid(self, name): - vbox = [] - for i in range(1, 4): - for j in range(1, 4): - vbox.append(self.builder.get_object('{}_{}{}'.format(name, \ - i, j))) - return vbox - - - def update_grid(self, grid, data): - for i in range(3): - for j in range(3): - k = 3 * i + j - grid[k].set_text('{:8.2f}'.format(data[i, j])) - - -def thread_gui(): - print('GUI: starting thread ..') - - gui = Gui() - GLib.idle_add(gui.update_gui) - Gtk.main() - - rover.on = False - print('GUI: thread closed!') - - -if __name__=='__main__': - thread_gui() \ No newline at end of file diff --git a/scripts/main.py b/scripts/main.py index 31bf1d3..b859c18 100644 --- a/scripts/main.py +++ b/scripts/main.py @@ -2,45 +2,103 @@ from rover import rover, reset_uav -from gui import thread_gui -from thread_imu import thread_imu -from thread_gps import thread_gps -from thread_control import thread_control -from thread_log import thread_log +from gui import GuiNode +from thread_imu import ImuNode +from thread_gps import GpsNode +from thread_control import ControlNode +from estimator import EstimatorNode +# from thread_log import thread_log import numpy as np -import rospy +import rclpy import std_msgs import threading import time +from PyQt5.QtWidgets import QApplication +# run_uav_completed = False -def run_uav(): +# def run_uav(): +# global run_uav_completed - rospy.init_node('uav', anonymous=True) - reset_uav() +# print("UAV thread started") - # Create threads - t1 = threading.Thread(target=thread_control) - t2 = threading.Thread(target=thread_imu) - t3 = threading.Thread(target=thread_gps) - t4 = threading.Thread(target=thread_gui) - t5 = threading.Thread(target=thread_log) - - # Start threads. - t1.start() - t2.start() - t3.start() - t4.start() - t5.start() +# reset_uav() - # Wait until all threads close. - t1.join() - t2.join() - t3.join() - t4.join() - t5.join() +# executor = rclpy.executors.MultiThreadedExecutor(num_threads=3) +# executor.add_node(ControlNode()) +# executor.add_node(ImuNode()) +# executor.add_node((GpsNode())) + +# try: +# while rclpy.ok(): +# executor.spin_once() +# finally: +# print("UAV thread stopping") +# executor.shutdown() + +# run_uav_completed = True +# print("UAV thread closed") if __name__ == '__main__': - run_uav() + print("Main program started") + + rclpy.init() + + app = QApplication([]) + app.processEvents() + + nodes = [] + nodes.append(GuiNode()) + nodes.append(ControlNode()) + nodes.append(EstimatorNode()) + nodes.append(GpsNode()) + + # Display the GUI + nodes[0].show() + + num_nodes = len(nodes) + + executor = rclpy.executors.MultiThreadedExecutor(num_threads=num_nodes) + for node in nodes: + executor.add_node(node) + # executor.add_node(gui) + # executor.add_node(ControlNode()) + # executor.add_node(ImuNode()) + # executor.add_node(GpsNode()) + + # threads = [] + # threads.append(threading.Thread(target=run_uav)) + + # for thread in threads: + # thread.start() + + while rclpy.ok(): + executor.spin_once() + app.processEvents() + app.quit() + # try: + # while rclpy.ok(): + # executor.spin_once() + # app.processEvents() + # app.quit() + # except Exception as e: + # print(e) + # print("Closing nodes") + + # print("GUI node closed") + # gui.destroy_node() + + + # for thread in threads: + # thread.join() + + # print("Waiting for all threads to end") + # while run_uav_completed: + # time.sleep(1) + + # rclpy.shutdown() + + print("Main program ended") + diff --git a/scripts/rover.py b/scripts/rover.py index 172406c..fdaff68 100644 --- a/scripts/rover.py +++ b/scripts/rover.py @@ -1,14 +1,15 @@ from matrix_utils import hat, vee, q_to_R -from control import Control -from estimator import Estimator from trajectory import Trajectory import datetime import numpy as np +import os import pdb -import rospy import threading +import rclpy +from rclpy.node import Node + from geometry_msgs.msg import Pose, Twist, Wrench from geometry_msgs.msg import Vector3, Point, Quaternion from gazebo_msgs.msg import ModelState @@ -59,10 +60,13 @@ def __init__(self): self.V_x_gps = np.diag([0.01, 0.01, 0.01]) self.V_v_gps = np.diag([0.01, 0.01, 0.01]) - self.control = Control() - self.control.use_integral = True # Enable integral control + self.a_imu = np.zeros(3) + self.W_imu = np.zeros(3) + + self.x_gps = np.zeros(3) + self.v_gps = np.zeros(3) - self.estimator = Estimator() + # self.estimator = Estimator() self.trajectory = Trajectory() self.lock = threading.Lock() @@ -79,70 +83,80 @@ def get_current_time(self): def run_controller(self): - self.update_current_time() + pass + # self.update_current_time() - with self.lock: - states = self.estimator.get_states() - desired = self.trajectory.get_desired(rover.mode, states, \ - self.x_offset, self.yaw_offset) - fM = self.control.run(states, desired) + # with self.lock: + # states = self.estimator.get_states() + + # desired = self.trajectory.get_desired(rover.mode, states, \ + # self.x_offset, self.yaw_offset) + # fM = self.control.run(states, desired) - self.x, self.v, self.a, self.R, self.W = states + # self.x, self.v, self.a, self.R, self.W = states - return fM + # return fM - def ros_imu_callback(self, message): - q_gazebo = message.orientation - a_gazebo = message.linear_acceleration - W_gazebo = message.angular_velocity + def ros_imu_callback(self, msg): + q_gazebo = msg.orientation + # a_gazebo = msg.linear_acceleration + # W_gazebo = msg.angular_velocity - q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) + # q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) - R_gi = q_to_R(q) # IMU to Gazebo frame - R_fi = self.R_fg.dot(R_gi) # IMU to FDCL frame (NED freme) + # R_gi = q_to_R(q) # IMU to Gazebo frame + # R_fi = self.R_fg.dot(R_gi) # IMU to FDCL frame (NED freme) - # FDCL-UAV expects IMU accelerations without gravity. - a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) - a_i = R_gi.T.dot(R_gi.dot(a_i) - self.ge3) + # # FDCL-UAV expects IMU accelerations without gravity. + # a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) + # a_i = R_gi.T.dot(R_gi.dot(a_i) - self.ge3) - W_i = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) + # W_i = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) - with self.lock: - self.estimator.prediction(a_i, W_i) - self.estimator.imu_correction(R_fi, self.V_R_imu) + # with self.lock: + # self.estimator.prediction(a_i, W_i) + # self.estimator.imu_correction(R_fi, self.V_R_imu) - def ros_gps_callback(self, message): - x_gazebo = message.pose.pose.position - v_gazebo = message.twist.twist.linear + def ros_gps_callback(self, msg): + x_gazebo = msg.pose.pose.position + v_gazebo = msg.twist.twist.linear # Gazebo uses ENU frame, but NED frame is used in FDCL. x_g = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z]) v_g = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z]) - with self.lock: - self.estimator.gps_correction(x_g, v_g, self.V_x_gps, self.V_v_gps) + # with self.lock: + # self.estimator.gps_correction(x_g, v_g, self.V_x_gps, self.V_v_gps) def reset_uav(): - rospy.wait_for_service('/gazebo/set_model_state') - - init_position = Point(x=0.0, y=0.0, z=0.2) - init_attitude = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - init_pose = Pose(position=init_position, orientation=init_attitude) + print("Reset: init") - zero_motion = Vector3(x=0.0, y=0.0, z=0.0) - init_velocity = Twist(linear=zero_motion, angular=zero_motion) + service_call = 'ros2 service call ' + arguments = '/gazebo/set_entity_state gazebo_msgs/SetEntityState ' + + entity_name = 'name: uav.urdf' - model_state = ModelState(model_name='uav', reference_frame='world', \ - pose=init_pose, twist=init_velocity) + position = 'position:{x: 0.0, y: 0.0, z: 0.5}' + orientation = 'orientation:{x: 0.0, y: 0.0, z: 0.0, w: 1.0}' + pose = 'pose: {' + position + ',' + orientation + '}' - reset_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) - reset_state(model_state) + linear = 'linear:{x: 0.0, y: 0.0, z: 0.0}' + angular = 'angular:{x: 0.0, y: 0.0, z: 0.0}' + twist = 'twist: {' + linear + ',' + angular + '}' + + reference_frame = 'reference_frame: world' + + state = '\"state: {' + entity_name + ', ' + pose + ', ' + twist + ', ' \ + + reference_frame + '}\"' + + command = service_call + arguments + state + response = os.system(command) - print('Resetting UAV successful ..') + print('Reset: successful') rover = Rover() diff --git a/scripts/thread_control.py b/scripts/thread_control.py index bea7ae9..927a941 100644 --- a/scripts/thread_control.py +++ b/scripts/thread_control.py @@ -2,43 +2,71 @@ import datetime import numpy as np -import rospy +import time -from geometry_msgs.msg import Wrench -from geometry_msgs.msg import Vector3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import WrenchStamped +from geometry_msgs.msg import Vector3 -def thread_control(): - print('CONTROL: thread starting ..') - pub = rospy.Publisher('uav_fm', Wrench, queue_size=1) - rate = rospy.Rate(200) # 200 hz +class ControlNode(Node): + def __init__(self): + super().__init__('control') + # self.publisher_ = self.create_publisher(WrenchStamped, '/uav/fm', 10) + + # timer_period = 0.005 # seconds + # self.timer = self.create_timer(timer_period, self.timer_callback) - freq = 0.0 - t = datetime.datetime.now() - t_pre = datetime.datetime.now() - avg_number = 1000 + def timer_callback(self): - while not rospy.is_shutdown() and rover.on: - t = datetime.datetime.now() - dt = (t - t_pre).total_seconds() - if dt < 1e-6: - continue - - freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number - t_pre = t - rover.freq_control = freq + # start = time.time() fM = rover.run_controller() - + if (not rover.motor_on) or (rover.mode < 2): - fM_message = Wrench(force=Vector3(x=0.0, y=0.0, z=0.0), \ + fM_message = WrenchStamped(force=Vector3(x=0.0, y=0.0, z=0.0), \ torque=Vector3(x=0.0, y=0.0, z=0.0)) else: - fM_message = Wrench(force=Vector3(x=0.0, y=0.0, z=fM[0]), \ - torque=Vector3(x=fM[1], y=fM[2], z=fM[3])) - pub.publish(fM_message) + fM_message = WrenchStamped(force=Vector3(x=0.0, y=0.0, z=fM[0][0]), \ + torque=Vector3(x=fM[1][0], y=fM[2][0], z=fM[3][0])) + + self.publisher_.publish(fM_message) + + + +# def thread_control(): +# print('CONTROL: thread starting ..') + +# pub = rospy.Publisher('uav_fm', Wrench, queue_size=1) +# rate = rospy.Rate(200) # 200 hz + +# freq = 0.0 +# t = datetime.datetime.now() +# t_pre = datetime.datetime.now() +# avg_number = 1000 + +# while not rospy.is_shutdown() and rover.on: +# t = datetime.datetime.now() +# dt = (t - t_pre).total_seconds() +# if dt < 1e-6: +# continue + +# freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number +# t_pre = t +# rover.freq_control = freq + +# fM = rover.run_controller() + +# if (not rover.motor_on) or (rover.mode < 2): +# fM_message = Wrench(force=Vector3(x=0.0, y=0.0, z=0.0), \ +# torque=Vector3(x=0.0, y=0.0, z=0.0)) +# else: +# fM_message = Wrench(force=Vector3(x=0.0, y=0.0, z=fM[0]), \ +# torque=Vector3(x=fM[1], y=fM[2], z=fM[3])) +# pub.publish(fM_message) - rate.sleep() +# rate.sleep() - print('CONTROL: thread closed!') +# print('CONTROL: thread closed!') diff --git a/scripts/thread_gps.py b/scripts/thread_gps.py index 1fbf82e..94dde32 100644 --- a/scripts/thread_gps.py +++ b/scripts/thread_gps.py @@ -2,33 +2,52 @@ import datetime import numpy as np -import rospy +import rclpy +from rclpy.node import Node from nav_msgs.msg import Odometry +class GpsNode(Node): -def thread_gps(): - print('GPS: thread starting ..') + def __init__(self): + super().__init__('gps_sensor') + self.subscription = self.create_subscription( + Odometry, + '/uav/gps', + self.listener_callback, + 1) + self.subscription # prevent unused variable warning - rospy.Subscriber('uav_pos', Odometry, rover.ros_gps_callback) - rate = rospy.Rate(10) # 10 hz + self.first = True - freq = 10.0 - t = datetime.datetime.now() - t_pre = datetime.datetime.now() - avg_number = 10 + def listener_callback(self, msg): + # print(msg) + pass + # rover.ros_gps_callback(msg) - while not rospy.is_shutdown() and rover.on: - t = datetime.datetime.now() - dt = (t - t_pre).total_seconds() - if dt < 1e-3: - continue +# def thread_gps(): +# print('GPS: thread starting ..') - freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number - t_pre = t - rover.freq_gps = freq +# rospy.Subscriber('uav_pos', Odometry, rover.ros_gps_callback) +# rate = rospy.Rate(10) # 10 hz - rate.sleep() +# freq = 10.0 +# t = datetime.datetime.now() +# t_pre = datetime.datetime.now() +# avg_number = 10 + +# while not rospy.is_shutdown() and rover.on: + +# t = datetime.datetime.now() +# dt = (t - t_pre).total_seconds() +# if dt < 1e-3: +# continue + +# freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number +# t_pre = t +# rover.freq_gps = freq + +# rate.sleep() - print('GPS: thread closed!') +# print('GPS: thread closed!') diff --git a/scripts/thread_imu.py b/scripts/thread_imu.py index 230dfe7..c2631ce 100644 --- a/scripts/thread_imu.py +++ b/scripts/thread_imu.py @@ -2,32 +2,74 @@ import datetime import numpy as np -import rospy +import time +import rclpy +from rclpy.node import Node from sensor_msgs.msg import Imu -def thread_imu(): - print('IMU: thread starting ..') +class ImuNode(Node): - rospy.Subscriber('uav_imu', Imu, rover.ros_imu_callback) - rate = rospy.Rate(100) # 100 hz + def __init__(self): + super().__init__('imu_sensor') + self.subscription = self.create_subscription( + Imu, + '/uav/imu', + self.imu_callback, + 1) + self.t_pre = 0.0 + self.count = 0 - freq = 100.0 - t = datetime.datetime.now() - t_pre = datetime.datetime.now() - avg_number = 100 + def listener_callback(self, msg): + # print(msg) + # self.get_logger().info("IMU") + self.count = self.count + 1 + t_now = time.time() - while not rospy.is_shutdown() and rover.on: - t = datetime.datetime.now() - dt = (t - t_pre).total_seconds() - if dt < 1e-6: - continue + if self.count > 100: + self.count = 0 + print(t_now - self.t_pre) + + self.t_pre = t_now - freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number - t_pre = t - rover.freq_imu = freq + # pass + # rover.ros_imu_callback(msg) - rate.sleep() + + +# def thread_imu(): +# print('IMU: thread starting ..') + +# max_freq = 200 + +# imu_sub = ImuNode() + +# print('IMU: waiting for messages') +# rclpy.spin(imu_sub) + +# # while rclpy.ok(): +# # rclpy.spin(imu_sub) +# # rate.sleep() + +# # rospy.Subscriber('uav_imu', Imu, rover.ros_imu_callback) +# # rate = rospy.Rate(100) # 100 hz + +# # freq = 100.0 +# # t = datetime.datetime.now() +# # t_pre = datetime.datetime.now() +# # avg_number = 100 + +# # while not rospy.is_shutdown() and rover.on: +# # t = datetime.datetime.now() +# # dt = (t - t_pre).total_seconds() +# # if dt < 1e-6: +# # continue + +# # freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number +# # t_pre = t +# # rover.freq_imu = freq + +# # rate.sleep() - print('IMU: thread closed!') +# print('IMU: thread closed!') diff --git a/src/fdcl_uav/fdcl_uav/__init__.py b/src/fdcl_uav/fdcl_uav/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/control.py b/src/fdcl_uav/fdcl_uav/control.py similarity index 70% rename from scripts/control.py rename to src/fdcl_uav/fdcl_uav/control.py index c7e7b01..7ed135f 100644 --- a/scripts/control.py +++ b/src/fdcl_uav/fdcl_uav/control.py @@ -1,12 +1,19 @@ -from matrix_utils import hat, vee, deriv_unit_vector, saturate -from integral_utils import IntegralError, IntegralErrorVec3 +from .matrix_utils import hat, vee, deriv_unit_vector, saturate +from .integral_utils import IntegralError, IntegralErrorVec3 import datetime import numpy as np -import pdb +import rclpy +from rclpy.node import Node -class Control: +from geometry_msgs.msg import Vector3, WrenchStamped +from std_msgs.msg import Int32, Bool + +from uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData + + +class ControlNode(Node): """Controller for the UAV trajectory tracking. This class detemines the control outputs for a quadrotor UAV given it's @@ -102,8 +109,9 @@ class Control: """ def __init__(self): + Node.__init__(self, 'control') - self.t0 = datetime.datetime.now() + self.t0 = self.get_clock().now() self.t = 0.0 self.t_pre = 0.0 self.dt = 1e-9 @@ -189,6 +197,12 @@ def __init__(self): ]) self.fM_to_forces_inv = np.linalg.inv(fM_to_forces) # Force to # force-moment conversion matrix + + # Control errors + self.ex = np.zeros(3) + self.ev = np.zeros(3) + self.eR = np.zeros(3) + self.eW = np.zeros(3) # Integral errors self.eIX = IntegralErrorVec3() # Position integral error @@ -196,12 +210,18 @@ def __init__(self): self.eI1 = IntegralError() # Attitude integral error for roll axis self.eI2 = IntegralError() # Attitude integral error for pitch axis self.eIy = IntegralError() # Attitude integral error for yaw axis - self.eIX = IntegralError() # Position integral error self.sat_sigma = 1.8 + self.is_landed = True + self.mode = 0 + self.motor_on = False - def run(self, states, desired): + self.init_subscribers() + self.init_publishers() + + + def control_run(self): """Run the controller to get the force-moments required to achieve the the desired states from the current state. @@ -214,19 +234,31 @@ def run(self, states, desired): fM: (4x1 numpy array) force-moments vector """ - self.x, self.v, self.a, self.R, self.W = states - self.xd, self.xd_dot, self.xd_2dot, self.xd_3dot, self.xd_4dot, \ - self.b1d, self.b1d_dot, self.b1d_2dot, is_landed = desired - # If the UAV is landed, do not run the controller and produce zero # force-moments. - if is_landed: - return np.zeros(4) - - self.position_control() - self.attitude_control() + if self.is_landed: + self.fM = np.zeros(4) + else: + self.position_control() + self.attitude_control() + + if (not self.motor_on) or (self.mode < 2): + force = Vector3(x=0.0, y=0.0, z=0.0) + torque = Vector3(x=0.0, y=0.0, z=0.0) + else: + # FDCL uses NED, Gazebo uses NWU + force = Vector3(x=0.0, y=0.0, z=self.fM[0]) + torque = Vector3(x=self.fM[1], y=-self.fM[2], z=-self.fM[3]) + + + fM_message = WrenchStamped() + fM_message.wrench.force = force + fM_message.wrench.torque = torque + + self.pub_fM.publish(fM_message) - return self.fM + self.publish_desired() + self.publish_errors() def position_control(self): @@ -356,6 +388,9 @@ def position_control(self): self.wc3 = e3 @ (R_T @ Rd @ Wd) self.wc3_dot = e3 @ (R_T @ Rd @ Wd_dot) \ - e3 @ (hatW @ R_T @ Rd @ Wd) + + self.ex = eX + self.ev = eV def attitude_control(self): @@ -433,8 +468,7 @@ def attitude_control(self): if self.use_integral: M3 += - self.kyI * self.eIy.error - # Gazebo uses ENU frame, but NED frame is used in FDCL. - M = np.array([M1, -M2, -M3]) + M = np.array([M1, M2, M3]) self.fM[0] = self.f_total for i in range(3): @@ -444,10 +478,12 @@ def attitude_control(self): # For saving: RdtR = Rd_T @ R - eR = 0.5 * vee(RdtR - RdtR.T) + self.eR = 0.5 * vee(RdtR - RdtR.T) self.eIR.error = np.array([self.eI1.error, self.eI2.error, \ self.eIy.error]) - eW = W - R_T @ Rd @ Wd + self.eW = W - R_T @ Rd @ Wd + + # self.get_logger().info('eR ' + str(self.eR)) def set_integral_errors_to_zero(self): @@ -464,15 +500,167 @@ def update_current_time(self): """Update the current time since epoch.""" self.t_pre = self.t - t_now = datetime.datetime.now() - self.t = (t_now - self.t0).total_seconds() + t_now = self.get_clock().now() + self.t = float((t_now - self.t0).nanoseconds) * 1e-9 + + + + def init_publishers(self): + self.pub_desired = self.create_publisher(DesiredData, '/uav/desired', 1) + self.pub_errors = self.create_publisher(ErrorData, '/uav/errors', 1) + self.pub_fM = self.create_publisher(WrenchStamped, '/uav/fm', 1) + timer_period = 0.005 + self.timer = self.create_timer(timer_period, self.control_run) + - def get_current_time(self): - """Return the current time since epoch. + def init_subscribers(self): + self.sub_trajectory = self.create_subscription( \ + StateData, + '/uav/states', + self.states_callback, + 1) - Return: - t: (float) time since epoch [s] + self.sub_trajectory = self.create_subscription( \ + DesiredData, + '/uav/trajectory', + self.trajectory_callback, + 1) + + self.sub_mode = self.create_subscription( \ + ModeData, + '/uav/mode', + self.mode_callback, + 1) + + + self.sub_motors_on = self.create_subscription( \ + Bool, + '/uav/motors_on', + self.motors_on_callback, + 1) + + + def mode_callback(self, msg): + if self.mode == msg.mode: + return + + self.mode = msg.mode + self.get_logger().info('Mode switched to {}'.format(self.mode)) + + + def states_callback(self, msg): + + for i in range(3): + self.x[i] = msg.position[i] + self.v[i] = msg.velocity[i] + self.a[i] = msg.acceleration[i] + self.W[i] = msg.angular_velocity[i] + + for j in range(3): + self.R[i, j] = msg.attitude[3*i + j] + + + def trajectory_callback(self, msg): + """Callback function for the trajectory subscriber. + + Args: + msg: (DesiredData) Trajectory data message """ - t_now = datetime.datetime.now() - return (t_now - self.t0).total_seconds() + + for i in range(3): + self.xd[i] = msg.position[i] + self.xd_dot[i] = msg.velocity[i] + self.xd_2dot[i] = msg.acceleration[i] + self.xd_3dot[i] = msg.jerk[i] + self.xd_4dot[i] = msg.snap[i] + + self.b1d[i] = msg.b1[i] + self.b1d_dot[i] = msg.b1_dot[i] + self.b1d_2dot[i] = msg.b1_2dot[i] + + self.is_landed = msg.is_landed + + + def motors_on_callback(self, msg): + self.motor_on = msg.data + + if self.motor_on: + self.get_logger().info('Turning motors on') + else: + self.get_logger().info('Turning motors off') + + + def publish_desired(self): + """Publish the desired states.""" + + msg = DesiredData() + + for i in range(3): + msg.position[i] = self.xd[i] + msg.velocity[i] = self.xd_dot[i] + msg.acceleration[i] = self.xd_2dot[i] + msg.jerk[i] = self.xd_3dot[i] + msg.snap[i] = self.xd_4dot[i] + + msg.b1[i] = self.b1d[i] + msg.b1_dot[i] = self.b1d_dot[i] + msg.b1_2dot[i] = self.b1d_2dot[i] + + msg.angular_velocity[i] = self.Wd[i] + msg.angular_acceleration[i] = self.Wd_dot[i] + # msg.angular_jerk = self.Wd_2dot + + msg.b1c[i] = self.b1c[i] + + msg.b3[i] = self.b3d[i] + msg.b3_dot[i] = self.b3d_dot[i] + msg.b3_2dot[i] = self.b3d_2dot[i] + + for j in range(3): + msg.attitude[3*i + j] = self.Rd[i, j] + + msg.wc3 = self.wc3 + msg.wc3_dot = self.wc3_dot + + msg.is_landed = self.is_landed + + self.pub_desired.publish(msg) + + + def publish_errors(self): + + msg = ErrorData() + + for i in range(3): + msg.position[i] = self.ex[i] + msg.velocity[i] = self.ev[i] + + msg.attitude[i] = self.eR[i] + msg.angular_velocity[i] = self.eW[i] + + msg.position_integral[i] = self.eIX.error[i] + msg.attitude_integral[i] = self.eIR.error[i] + + + self.pub_errors.publish(msg) + + +def main(args=None): + print("Starting control node") + + rclpy.init(args=args) + + control = ControlNode() + + try: + rclpy.spin(control) + except KeyboardInterrupt: + pass + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + control.destroy_node() + + print("Terminating control node") diff --git a/scripts/estimator.py b/src/fdcl_uav/fdcl_uav/estimator.py similarity index 63% rename from scripts/estimator.py rename to src/fdcl_uav/fdcl_uav/estimator.py index 25ae911..ae59bf1 100644 --- a/scripts/estimator.py +++ b/src/fdcl_uav/fdcl_uav/estimator.py @@ -1,10 +1,21 @@ -from matrix_utils import hat, vee, expm_SO3 +from .matrix_utils import hat, vee, expm_SO3, q_to_R + +import rclpy +from rclpy.node import Node import datetime import numpy as np +import rclpy +from rclpy.node import Node + +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Imu + +from uav_gazebo.msg import StateData + -class Estimator: +class EstimatorNode(Node): """Estimates the states of the UAV. This uses the estimator defined in "Real-time Kinematics GPS Based @@ -36,7 +47,7 @@ class Estimator: ge3: (3x1 numpy array) gravitational acceleration direction [m/s^2] R_bi: (3x3 numpy array) transformation from IMU frame to the body frame - R_bi_T: (3x3 numpy array) transformation from IMU frame to the body frame + R_ib: (3x3 numpy array) transformation from IMU frame to the body frame e3 : (3x1 numpy array) direction of the e3 axis eye3: (3x3 numpy array) 3x3 identity matrix @@ -46,6 +57,8 @@ class Estimator: """ def __init__(self): + Node.__init__(self, 'estimator') + self.x = np.zeros(3) self.v = np.zeros(3) self.a = np.zeros(3) @@ -68,12 +81,13 @@ def __init__(self): 1.0 # accelerometer z bias ]) - self.t0 = datetime.datetime.now() + self.t0 = self.get_clock().now() self.t = 0.0 self.t_pre = 0.0 self.W_pre = np.zeros(3) self.a_imu_pre = np.zeros(3) + self.W_imu_pre = np.zeros(3) self.R_pre = np.eye(3) self.b_a_pre = 0.0 @@ -86,7 +100,7 @@ def __init__(self): [0.0, -1.0, 0.0], [0.0, 0.0, -1.0] ]) - self.R_bi_T = self.R_bi.T + self.R_ib = self.R_bi.T self.e3 = np.array([0.0, 0.0, 1.0]) self.eye3 = np.eye(3) @@ -94,6 +108,23 @@ def __init__(self): self.zero3 = np.zeros((3, 3)) + self.V_R_imu = np.diag([0.01, 0.01, 0.01]) + self.V_x_gps = np.diag([0.01, 0.01, 0.01]) + self.V_v_gps = np.diag([0.01, 0.01, 0.01]) + + # Gazebo uses ENU frame, but NED frame is used in FDCL. + # Note that ENU and the NED here refer to their direction order. + # ENU: E - axis 1, N - axis 2, U - axis 3 + # NED: N - axis 1, E - axis 2, D - axis 3 + self.R_fg = np.array([ + [1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0] + ]) + + self.init_subscribers() + self.init_publishers() + def prediction(self, a_imu, W_imu): """Prediction step of the estimator. @@ -109,7 +140,8 @@ def prediction(self, a_imu, W_imu): self.W_pre = np.copy(self.W) self.b_a_pre = self.b_a * 1.0 - self.W = self.R_bi.dot(W_imu) + # TODO: replace .dot with @ + self.W = self.R_bi @ W_imu self.R = self.R.dot(expm_SO3(h / 2.0 * (self.W + self.W_pre))) # This assumes IMU provide acceleration without g @@ -142,6 +174,13 @@ def prediction(self, a_imu, W_imu): self.P = A.dot(self.P).dot(A.T) + F.dot(self.Q).dot(F.T) self.a_imu_pre = a_imu + self.W_imu_pre = W_imu + + # rover.x = self.x + # rover.v = self.v + # rover.a = self.a + # rover.R = self.R + # rover.W = self.W def imu_correction(self, R_imu, V_R_imu): @@ -152,7 +191,7 @@ def imu_correction(self, R_imu, V_R_imu): V_R_imu: (3x3 numpy array) attitude measurement covariance """ - imu_R = self.R.T.dot(R_imu).dot(self.R_bi_T) + imu_R = self.R.T @ R_imu @ self.R_ib del_z = 0.5 * vee(imu_R - imu_R.T) H = np.block([self.zero3, self.zero3, self.eye3, np.zeros((3, 1))]) @@ -224,8 +263,8 @@ def get_dt(self): """ self.t_pre = self.t * 1.0 - t_now = datetime.datetime.now() - self.t = (t_now - self.t0).total_seconds() + t_now = self.get_clock().now() + self.t = float((t_now - self.t0).nanoseconds) * 1e-9 return self.t - self.t_pre @@ -241,3 +280,106 @@ def get_states(self): W: (3x1 numpy array) current angular velocity of the UAV [rad/s] """ return (self.x, self.v, self.a, self.R, self.W) + + + def init_subscribers(self): + self.sub_imu = self.create_subscription( \ + Imu, + '/uav/imu', + self.imu_callback, + 1) + + self.sub_gps = self.create_subscription( \ + Odometry, + '/uav/gps', + self.gps_callback, + 1) + + + def imu_callback(self, msg): + """Callback function for the IMU subscriber. + + Args: + msg: (Imu) IMU message + """ + + q_gazebo = msg.orientation + a_gazebo = msg.linear_acceleration + W_gazebo = msg.angular_velocity + + q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) + + R_gi = q_to_R(q) # IMU to Gazebo frame + R_fi = self.R_fg @ R_gi # IMU to FDCL frame (NED frame) + + # FDCL-UAV expects IMU accelerations without gravity. + a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) + a_imu = R_gi.T @ (R_gi @ a_i - self.ge3) + + W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) + + # TODO: get covariances from the message + self.prediction(a_imu, W_imu) + self.imu_correction(R_fi, self.V_R_imu) + + self.publish_states() + + + + def gps_callback(self, msg): + """Callback function for the GPS subscriber. + + Args: + msg: (GPS) GPS message + """ + + x_gazebo = msg.pose.pose.position + v_gazebo = msg.twist.twist.linear + + # Gazebo uses ENU frame, but NED frame is used in FDCL. + x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z]) + v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z]) + + self.gps_correction(x_gps, v_gps, self.V_x_gps, self.V_v_gps) + + self.publish_states() + + + + def init_publishers(self): + self.pub_state = self.create_publisher(StateData, '/uav/states', 1) + + + def publish_states(self): + states = StateData() + for i in range(3): + states.position[i] = self.x[i] + states.velocity[i] = self.v[i] + states.acceleration[i] = self.a[i] + states.angular_velocity[i] = self.W[i] + + for j in range(3): + states.attitude[3*i + j] = self.R[i, j] + + self.pub_state.publish(states) + + + +def main(args=None): + print("Starting estimator node") + + rclpy.init(args=args) + + estimator = EstimatorNode() + + try: + rclpy.spin(estimator) + except KeyboardInterrupt: + pass + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + estimator.destroy_node() + + print("Terminating estimator node") \ No newline at end of file diff --git a/src/fdcl_uav/fdcl_uav/freq_utils.py b/src/fdcl_uav/fdcl_uav/freq_utils.py new file mode 100644 index 0000000..9f90de2 --- /dev/null +++ b/src/fdcl_uav/fdcl_uav/freq_utils.py @@ -0,0 +1,29 @@ + + +class Freq(): + def __init__(self, name, freq): + self.name = name + self.t_prev = 0.0 + self.num_avg = 50 + self.freq_avg = freq + self.dt = 0 + + def update(self, t_now): + dt = t_now - self.t_prev + + if dt <= 0.0: + return + + self.dt = dt + freq = 1.0 / dt + self.freq_avg = (freq + (self.num_avg - 1) * self.freq_avg) / self.num_avg + + self.t_prev = t_now + + + def get_freq(self): + return self.freq_avg + + + def get_freq_str(self): + return f'{self.name}: {self.freq_avg:5.1f} Hz' \ No newline at end of file diff --git a/src/fdcl_uav/fdcl_uav/gui.py b/src/fdcl_uav/fdcl_uav/gui.py new file mode 100644 index 0000000..ec045f6 --- /dev/null +++ b/src/fdcl_uav/fdcl_uav/gui.py @@ -0,0 +1,591 @@ +from .matrix_utils import q_to_R +from .freq_utils import Freq + +import numpy as np +import os + +from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtWidgets import QApplication, QLabel, QWidget, QVBoxLayout, \ + QHBoxLayout, QPushButton, QRadioButton, QMainWindow + +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import WrenchStamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Imu +from std_msgs.msg import Bool + +from uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData + + +class GuiNode(Node, QMainWindow): + def __init__(self): + Node.__init__(self, 'gui') + QMainWindow.__init__(self) + + self.t0 = self.get_clock().now() + + self.motor_on = False + + # States + self.freq_estimator = Freq("EST", 100) + self.x = np.zeros(3) + self.v = np.zeros(3) + self.a = np.zeros(3) + self.R = np.identity(3) + self.W = np.zeros(3) + + # Desired states + self.xd = np.zeros(3) + self.vd = np.zeros(3) + self.b1d = np.zeros(3) + self.b1d[0] = 1.0 + self.Rd = np.identity(3) + self.Wd = np.zeros(3) + + # Errors + self.ex = np.zeros(3) + self.ev = np.zeros(3) + self.eR = np.identity(3) + self.eW = np.zeros(3) + self.eIX = np.zeros(3) + self.eIR = np.zeros(3) + + # Sensor measurements + self.freq_imu = Freq("IMU", 100) + self.R_imu = np.zeros([3, 3]) + self.a_imu = np.zeros([3, 1]) + self.W_imu = np.zeros([3, 1]) + + self.freq_gps = Freq("GPS", 10) + self.x_gps = np.zeros([3, 1]) + self.v_gps = np.zeros([3, 1]) + + # Motor forces + self.freq_control = Freq("CTRL", 100) + self.fM = np.zeros(4) + + # Manual mode + self.x_offset = np.zeros(3) + self.x_offset_delta = 0.1 + self.yaw_offset = 0.0 + self.yaw_offset_delta = 0.02 + + self.mode = 0 + + self.g = 9.81 + self.ge3 = np.array([0.0, 0.0, self.g]) + + self.init_gui() + self.init_subscribers() + self.init_publishers() + + self.timer = QTimer() + self.timer.timeout.connect(self.update) + self.timer.start(100) + + + def init_gui(self): + window = QWidget() + window.setWindowTitle("FDCL UAV Simulator") + + main_layout = QVBoxLayout() + main_layout.setSpacing(10) + + # Status box + self.label_time = QLabel("0.0") + self.label_time.setMinimumWidth(80) + self.label_freq_imu = QLabel("0.0") + self.label_freq_gps = QLabel("0.0") + self.label_freq_control = QLabel("0.0") + self.label_freq_estimator = QLabel("0.0") + + thread_status_box = QHBoxLayout() + thread_status_box.setAlignment(Qt.AlignLeft) + thread_status_box.addWidget(self.label_time) + thread_status_box.addWidget(self.label_freq_imu) + thread_status_box.addWidget(self.label_freq_gps) + thread_status_box.addWidget(self.label_freq_control) + thread_status_box.addWidget(self.label_freq_estimator) + + # Control box + self.button_on = QPushButton("Shutdown") + self.button_on.clicked.connect(self.on_btn_close_clicked) + + self.button_motor_on = QPushButton("Turn on motors") + self.button_motor_on.clicked.connect(self.on_btn_motor_on_clicked) + + controls_box = QHBoxLayout() + controls_box.setAlignment(Qt.AlignLeft) + controls_box.addWidget(self.button_on) + controls_box.addWidget(self.button_motor_on) + + # Flight mode box + label_flight_mode = QLabel("Mode: ") + self.radio_button_modes = \ + [ \ + QRadioButton("Idle"), \ + QRadioButton("Warmup"), \ + QRadioButton("Take-off"), \ + QRadioButton("Stay"), \ + QRadioButton("Circle"), \ + QRadioButton("Land") \ + ] + num_flight_modes = len(self.radio_button_modes) + + flight_mode_box = QHBoxLayout() + flight_mode_box.setAlignment(Qt.AlignLeft) + flight_mode_box.addWidget(label_flight_mode) + for i in range(num_flight_modes): + flight_mode_box.addWidget(self.radio_button_modes[i]) + self.radio_button_modes[i].toggled.connect(self.on_flight_mode_changed) + + # State box + state_box = QHBoxLayout() + state_box.setAlignment(Qt.AlignLeft) + state_box.addSpacing(5) + [state_box, self.label_x] = self.init_vbox(state_box, "x", 4) + [state_box, self.label_v] = self.init_vbox(state_box, "v", 4) + [state_box, self.label_a] = self.init_vbox(state_box, "a", 4) + [state_box, self.label_R] = self.init_attitude_vbox(state_box, "R") + [state_box, self.label_W] = self.init_vbox(state_box, "W", 4) + + # Desired box + desired_box = QHBoxLayout() + desired_box.setAlignment(Qt.AlignLeft) + desired_box.addSpacing(5) + [desired_box, self.label_xd] = self.init_vbox(desired_box, "xd", 4) + [desired_box, self.label_vd] = self.init_vbox(desired_box, "vd", 4) + [desired_box, self.label_b1d] = self.init_vbox(desired_box, "b1d", 4) + [desired_box, self.label_Rd] = self.init_attitude_vbox(desired_box, "Rd") + [desired_box, self.label_Wd] = self.init_vbox(desired_box, "Wd", 4) + + # Sensor box + sensor_box = QHBoxLayout() + sensor_box.setAlignment(Qt.AlignLeft) + sensor_box.addSpacing(5) + [sensor_box, self.label_imu_ypr] = self.init_vbox(sensor_box, "IMU YPR", 4) + [sensor_box, self.label_imu_a] = self.init_vbox(sensor_box, "IMU a", 4) + [sensor_box, self.label_imu_W] = self.init_vbox(sensor_box, "IMU W", 4) + [sensor_box, self.label_gps_x] = self.init_vbox(sensor_box, "GPS x", 4) + [sensor_box, self.label_gps_v] = self.init_vbox(sensor_box, "GPS v", 4) + + # Error box + error_box = QHBoxLayout() + error_box.setAlignment(Qt.AlignLeft) + error_box.addSpacing(5) + [error_box, self.label_ex] = self.init_vbox(error_box, "ex", 4) + [error_box, self.label_ev] = self.init_vbox(error_box, "ev", 4) + [error_box, self.label_fM] = self.init_vbox(error_box, "f-M", 5) + # [error_box, self.label_f] = self.init_vbox(error_box, "f", 5) + # [error_box, self.label_thr] = self.init_vbox(error_box, "Throttle", 5) + + # Left box + left_box = QVBoxLayout() + left_box.setAlignment(Qt.AlignTop) + left_box.addSpacing(5) + left_box.addLayout(state_box) + main_layout.addSpacing(10) + left_box.addLayout(desired_box) + + # Right box + right_box = QVBoxLayout() + right_box.setAlignment(Qt.AlignTop) + right_box.addSpacing(5) + right_box.addLayout(sensor_box) + main_layout.addSpacing(10) + right_box.addLayout(error_box) + + # Data box + data_box = QHBoxLayout() + data_box.setAlignment(Qt.AlignLeft) + data_box.addLayout(left_box) + data_box.addSpacing(5) + data_box.addLayout(right_box) + + main_layout.setAlignment(Qt.AlignTop) + main_layout.addLayout(thread_status_box) + main_layout.addSpacing(5) + main_layout.addLayout(controls_box) + main_layout.addSpacing(5) + main_layout.addLayout(flight_mode_box) + main_layout.addSpacing(5) + main_layout.addLayout(data_box) + + window.setLayout(main_layout) + self.setCentralWidget(window) + window.keyPressEvent = self.on_key_press + + def init_subscribers(self): + + self.sub_states = self.create_subscription( \ + StateData, + '/uav/states', + self.states_callback, + 1) + + self.sub_desired = self.create_subscription( \ + DesiredData, + '/uav/desired', + self.desired_callback, + 1) + + self.sub_errors = self.create_subscription( \ + ErrorData, + '/uav/errors', + self.errors_callback, + 1) + + self.sub_fM = self.create_subscription( \ + WrenchStamped, + '/uav/fm', + self.fM_callback, + 1) + + self.sub_imu = self.create_subscription( \ + Imu, + '/uav/imu', + self.imu_callback, + 1) + + self.sub_gps = self.create_subscription( \ + Odometry, + '/uav/gps', + self.gps_callback, + 1) + + + def init_publishers(self): + self.pub_mode = self.create_publisher(ModeData, '/uav/mode', 1) + self.pub_motors_on = self.create_publisher(Bool, '/uav/motors_on', 1) + + + def update(self): + t_sec = self.get_t_now() + + self.label_time.setText(f't: {t_sec:5.1f} s') + self.label_freq_imu.setText(self.freq_imu.get_freq_str()) + self.label_freq_gps.setText(self.freq_gps.get_freq_str()) + self.label_freq_control.setText(self.freq_control.get_freq_str()) + self.label_freq_estimator.setText(self.freq_estimator.get_freq_str()) + + self.update_vbox(self.label_x, self.x) + self.update_vbox(self.label_v, self.v) + self.update_vbox(self.label_a, self.a) + self.update_attitude_vbox(self.label_R, self.R) + self.update_vbox(self.label_W, self.W) + + self.update_vbox(self.label_xd, self.xd) + self.update_vbox(self.label_vd, self.vd) + self.update_vbox(self.label_b1d, self.b1d) + self.update_attitude_vbox(self.label_Rd, self.Rd) + self.update_vbox(self.label_Wd, self.Wd) + + # TODO + # self.update_vbox(self.label_imu_ypr, self.imu_ypr) + + self.update_vbox(self.label_imu_a, self.a_imu) + self.update_vbox(self.label_imu_W, self.W_imu) + self.update_vbox(self.label_gps_x, self.x_gps) + self.update_vbox(self.label_gps_v, self.v_gps) + + self.update_vbox(self.label_ex, self.ex) + self.update_vbox(self.label_ev, self.ev) + + self.update_vbox(self.label_fM, self.fM, data_size=4) + # self.update_vbox(self.label_f, self.f) + # self.update_vbox(self.label_thr, self.thr) + + msg = ModeData() + msg.mode = self.mode + msg.x_offset = self.x_offset + msg.yaw_offset = self.yaw_offset + self.pub_mode.publish(msg) + + + def init_vbox(self, box, name, num): + vbox = QVBoxLayout() + vbox.setAlignment(Qt.AlignTop) + + labels = [] + for _ in range(num): + label_i = QLabel("{:5.2}".format(0.0)) + + labels.append(label_i) + vbox.addWidget(label_i) + + labels[0].setText(name) + box.addLayout(vbox) + + return [box, labels] + + + def init_attitude_vbox(self, box, name): + vbox_array = [QVBoxLayout(), QVBoxLayout(), QVBoxLayout()] + labels = [] + + for vbox_index in range(len(vbox_array)): + vbox = vbox_array[vbox_index] + vbox.setAlignment(Qt.AlignTop) + + for _ in range(4): + label_i = QLabel("{:6.4}".format(0.0)) + + labels.append(label_i) + vbox.addWidget(label_i) + + box.addLayout(vbox) + + labels[0].setText(name) + labels[4].setText("") + labels[8].setText("") + + return [box, labels] + + + def update_vbox(self, labels, data, data_size=3): + for i in range(data_size): + try: + labels[i + 1].setText("{:>5.2f}".format(data[i])) + except TypeError: + labels[i + 1].setText("{:>5.2f}".format(-1)) + + + def update_attitude_vbox(self, labels, data): + for i in range(3): + for j in range(1, 4): + labels[i * 4 + j].setText("{:>5.2f}".format(data[i, j - 1])) + + + def on_btn_close_clicked(self): + self.get_logger().info("Shutdown button clicked") + + self.get_logger().info('Turning motors off') + self.motor_on = False + + msg = Bool() + msg.data = self.motor_on + self.pub_motors_on.publish(msg) + + self.destroy_node() + self.close() + rclpy.shutdown() + + + def on_btn_motor_on_clicked(self): + + if self.motor_on: + self.get_logger().info('Turning motors off') + self.motor_on = False + self.button_motor_on.setText("Turn on motors") + else: + self.get_logger().info('Turning motors on') + self.motor_on = True + self.button_motor_on.setText("Turn off motors") + + msg = Bool() + msg.data = self.motor_on + self.pub_motors_on.publish(msg) + + + def on_flight_mode_changed(self): + for i in range(len(self.radio_button_modes)): + if self.radio_button_modes[i].isChecked(): + self.get_logger().info('Mode switched to {}'.format(i)) + + self.mode = i + + msg = ModeData() + msg.mode = self.mode + self.pub_mode.publish(msg) + + self.x_offset = np.zeros(3) + self.yaw_offset = 0.0 + break + + + def on_key_press(self, event): + self.get_logger().info('Key presed {}'.format(event.key())) + if event.key() == Qt.Key_M: + self.on_btn_motor_on_clicked() + elif event.key() == Qt.Key_QuoteLeft: + self.radio_button_modes[0].setChecked(True) + elif event.key() == Qt.Key_1: + self.radio_button_modes[1].setChecked(True) + elif event.key() == Qt.Key_2: + self.radio_button_modes[2].setChecked(True) + elif event.key() == Qt.Key_3: + self.radio_button_modes[3].setChecked(True) + elif event.key() == Qt.Key_4: + self.radio_button_modes[4].setChecked(True) + elif event.key() == Qt.Key_5: + self.radio_button_modes[5].setChecked(True) + elif event.key() == Qt.Key_6: + self.radio_button_modes[6].setChecked(True) + elif event.key() == Qt.Key_W: + self.x_offset[0] += self.x_offset_delta + elif event.key() == Qt.Key_S: + self.x_offset[0] -= self.x_offset_delta + elif event.key() == Qt.Key_D: + self.x_offset[1] += self.x_offset_delta + elif event.key() == Qt.Key_A: + self.x_offset[1] -= self.x_offset_delta + elif event.key() == Qt.Key_L: + self.x_offset[2] += self.x_offset_delta + elif event.key() == Qt.Key_P: + self.x_offset[2] -= self.x_offset_delta + elif event.key() == Qt.Key_E: + self.yaw_offset += self.yaw_offset_delta + elif event.key() == Qt.Key_Q: + self.yaw_offset -= self.yaw_offset_delta + + + def get_t_now(self): + t_now = self.get_clock().now() + t_sec = float((t_now - self.t0).nanoseconds) * 1e-9 + return t_sec + + + def states_callback(self, msg): + """Callback function for the states subscriber. + + Args: + msg: (StateData) State data message + """ + + self.freq_estimator.update(self.get_t_now()) + + for i in range(3): + self.x[i] = msg.position[i] + self.v[i] = msg.velocity[i] + self.a[i] = msg.acceleration[i] + self.W[i] = msg.angular_velocity[i] + + for j in range(3): + self.R[i, j] = msg.attitude[3*i + j] + + + def desired_callback(self, msg): + """Callback function for the desired subscriber. + + Args: + msg: (DesiredData) Desired data message + """ + + for i in range(3): + self.xd[i] = msg.position[i] + self.vd[i] = msg.velocity[i] + self.b1d[i] = msg.b1[i] + self.Wd[i] = msg.angular_velocity[i] + + for j in range(3): + self.Rd[i, j] = msg.attitude[3*i + j] + + + def errors_callback(self, msg): + """Callback function for the error subscriber. + + Args: + msg: (ErrorData) Error data message + """ + + for i in range(3): + self.ex[i] = msg.position[i] + self.ev[i] = msg.velocity[i] + self.eR[i] = msg.attitude[i] + self.eW[i] = msg.angular_velocity[i] + self.eIX[i] = msg.position_integral[i] + self.eIR[i] = msg.attitude_integral[i] + + + def imu_callback(self, msg): + """Callback function for the IMU subscriber. + + Args: + msg: (Imu) IMU message + """ + + self.freq_imu.update(self.get_t_now()) + + # Gazebo uses ENU frame, but NED frame is used in FDCL. + # Note that ENU and the NED here refer to their direction order. + # ENU: E - axis 1, N - axis 2, U - axis 3 + # NED: N - axis 1, E - axis 2, D - axis 3 + self.R_fg = np.array([ + [1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [0.0, 0.0, -1.0] + ]) + + q_gazebo = msg.orientation + a_gazebo = msg.linear_acceleration + W_gazebo = msg.angular_velocity + + q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) + + R_gi = q_to_R(q) # IMU to Gazebo frame + R_fi = self.R_fg.dot(R_gi) # IMU to FDCL frame (NED freme) + + # FDCL-UAV expects IMU accelerations without gravity. + a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) + self.a_imu = R_gi.T.dot(R_gi.dot(a_i) - self.ge3) + + self.W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) + + + def gps_callback(self, msg): + """Callback function for the GPS subscriber. + + Args: + msg: (GPS) GPS message + """ + + self.freq_gps.update(self.get_t_now()) + + x_gazebo = msg.pose.pose.position + v_gazebo = msg.twist.twist.linear + + # Gazebo uses ENU frame, but NED frame is used in FDCL. + self.x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z]) + self.v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z]) + + + def fM_callback(self, msg): + + self.freq_control.update(self.get_t_now()) + + self.fM[0] = msg.wrench.force.z + self.fM[1] = msg.wrench.torque.x + self.fM[2] = msg.wrench.torque.y + self.fM[3] = msg.wrench.torque.z + + +def main(args=None): + print("Starting gui node") + + rclpy.init(args=args) + + app = QApplication([]) + app.processEvents() + + gui = GuiNode() + gui.show() + + executor = rclpy.executors.SingleThreadedExecutor() + executor.add_node(gui) + + while rclpy.ok(): + executor.spin_once() + app.processEvents() + + app.quit() + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + gui.destroy_node() + + # rclpy.shutdown() + + print("Terminating gui node") \ No newline at end of file diff --git a/scripts/integral_utils.py b/src/fdcl_uav/fdcl_uav/integral_utils.py similarity index 100% rename from scripts/integral_utils.py rename to src/fdcl_uav/fdcl_uav/integral_utils.py diff --git a/scripts/matrix_utils.py b/src/fdcl_uav/fdcl_uav/matrix_utils.py similarity index 100% rename from scripts/matrix_utils.py rename to src/fdcl_uav/fdcl_uav/matrix_utils.py diff --git a/scripts/trajectory.py b/src/fdcl_uav/fdcl_uav/trajectory.py similarity index 67% rename from scripts/trajectory.py rename to src/fdcl_uav/fdcl_uav/trajectory.py index 2b9654c..a0296e8 100644 --- a/scripts/trajectory.py +++ b/src/fdcl_uav/fdcl_uav/trajectory.py @@ -1,15 +1,21 @@ import datetime import numpy as np -import pdb +import rclpy +from rclpy.node import Node + +from uav_gazebo.msg import DesiredData, ModeData, StateData + +class TrajectoryNode(Node): -class Trajectory: def __init__(self): + Node.__init__(self, 'trajectory') + self.mode = 0 self.is_mode_changed = False self.is_landed = False - self.t0 = datetime.datetime.now() + self.t0 = self.get_clock().now() self.t = 0.0 self.t_traj = 0.0 @@ -65,27 +71,81 @@ def __init__(self): self.e1 = np.array([1.0, 0.0, 0.0]) + self.init_subscribers() + self.init_publishers() - def get_desired(self, mode, states, x_offset, yaw_offset): - self.x, self.v, self.a, self.R, self.W = states - self.x_offset = x_offset - self.yaw_offset = yaw_offset - if mode == self.mode: - self.is_mode_changed = False - else: + def init_subscribers(self): + + self.sub_states = self.create_subscription( \ + StateData, + '/uav/states', + self.states_callback, + 1) + + self.sub_mode = self.create_subscription( \ + ModeData, + '/uav/mode', + self.mode_callback, + 1) + + + def init_publishers(self): + + self.pub_trajectory = self.create_publisher( \ + DesiredData, + '/uav/trajectory', + 1) + + + def states_callback(self, msg): + + for i in range(3): + self.x[i] = msg.position[i] + self.v[i] = msg.velocity[i] + self.a[i] = msg.acceleration[i] + self.W[i] = msg.angular_velocity[i] + + for j in range(3): + self.R[i, j] = msg.attitude[3*i + j] + + self.calculate_trajectory() + self.publish_trajectory() + + + def mode_callback(self, msg): + + self.x_offset = msg.x_offset + self.yaw_offset = msg.yaw_offset + + if self.mode != msg.mode: + self.mode = msg.mode self.is_mode_changed = True - self.mode = mode self.mark_traj_start() + self.get_logger().info('Mode switched to {}'.format(self.mode)) + + + def publish_trajectory(self): + + msg = DesiredData() + + for i in range(3): + msg.position[i] = self.xd[i] + msg.velocity[i] = self.xd_dot[i] + msg.acceleration[i] = self.xd_2dot[i] + msg.jerk[i] = self.xd_3dot[i] + msg.snap[i] = self.xd_4dot[i] - self.calculate_desired() + msg.b1[i] = self.b1d[i] + msg.b1_dot[i] = self.b1d_dot[i] + msg.b1_2dot[i] = self.b1d_2dot[i] - desired = (self.xd, self.xd_dot, self.xd_2dot, self.xd_3dot, \ - self.xd_4dot, self.b1d, self.b1d_dot, self.b1d_2dot, self.is_landed) - return desired + msg.is_landed = self.is_landed + + self.pub_trajectory.publish(msg) - def calculate_desired(self): + def calculate_trajectory(self): if self.manual_mode: self.manual() return @@ -95,12 +155,14 @@ def calculate_desired(self): self.mark_traj_start() elif self.mode == 2: # take-off self.takeoff() - elif self.mode == 3: # land - self.land() - elif self.mode == 4: # stay + elif self.mode == 3: # stay self.stay() - elif self.mode == 5: # circle + elif self.mode == 4: # circle self.circle() + elif self.mode == 5: # land + self.land() + else: + self.get_logger().info('Undefined mode {}'.format(self.mode)) def mark_traj_start(self): @@ -113,7 +175,7 @@ def mark_traj_start(self): self.t = 0.0 self.t_traj = 0.0 - self.t0 = datetime.datetime.now() + self.t0 = self.get_clock().now() self.x_offset = np.zeros(3) self.yaw_offset = 0.0 @@ -179,8 +241,8 @@ def waypoint_reached(self, waypoint, current, radius): def update_current_time(self): - t_now = datetime.datetime.now() - self.t = (t_now - self.t0).total_seconds() + t_now = self.get_clock().now() + self.t = float((t_now - self.t0).nanoseconds) * 1e-9 def manual(self): @@ -206,33 +268,48 @@ def takeoff(self): self.set_desired_states_to_zero() # Take-off starts from the current horizontal position. - self.xd[0] = self.x[0] - self.xd[1] = self.x[1] - self.x_init = self.x + self.xd = np.copy(self.x) + + self.xd_dot = np.zeros(3) + self.xd_dot[2] = self.takeoff_velocity - self.t_traj = (self.takeoff_end_height - self.x[2]) / \ - self.takeoff_velocity + self.t_traj = abs((self.takeoff_end_height - self.x[2]) / self.takeoff_velocity) # Set the takeoff attitude to the current attitude. self.b1d = self.get_current_b1() self.trajectory_started = True + takeoff_end_position = np.copy(self.x_init) + takeoff_end_position[2] += self.takeoff_velocity * self.t_traj + + self.get_logger().info('Takeoff started') + self.get_logger().info('Starting from ' + str(self.x)) + self.get_logger().info('Takeoff time ' + str(self.t_traj)) + self.get_logger().info('Reaching ' + str(takeoff_end_position)) + self.get_logger().info('Takeoff velocity ' + str(self.takeoff_velocity) + ' m/s') + self.update_current_time() if self.t < self.t_traj: self.xd[2] = self.x_init[2] + self.takeoff_velocity * self.t - self.xd_2dot[2] = self.takeoff_velocity + self.xd_dot[2] = self.takeoff_velocity + + elif not self.waypoint_reached(self.xd, self.x, 0.04): + self.xd[2] = self.takeoff_end_height + xd_dot_gain = 0.1 + self.xd_dot = xd_dot_gain * (self.x - self.xd) + else: - if self.waypoint_reached(self.xd, self.x, 0.04): - self.xd[2] = self.takeoff_end_height - self.xd_dot[2] = 0.0 + self.xd[2] = self.takeoff_end_height + self.xd_dot[2] = 0.0 - if not self.trajectory_complete: - print('Takeoff complete\nSwitching to manual mode') - - self.mark_traj_end(True) + if not self.trajectory_complete: + self.get_logger().info('Takeoff complete') + self.get_logger().info('Switching to manual mode') + self.mark_traj_end(True) + def land(self): if not self.trajectory_started: @@ -256,7 +333,8 @@ def land(self): self.xd_dot[2] = 0.0 if not self.trajectory_complete: - print('Landing complete') + self.get_logger().info('Landing complete') + self.mark_traj_end(False) self.is_landed = True @@ -326,3 +404,23 @@ def circle(self): w_b1d * w_b1d * np.sin(th_b1d), 0.0]) else: self.mark_traj_end(True) + + +def main(args=None): + print("Starting trajectory node") + + rclpy.init(args=args) + + trajectory = TrajectoryNode() + + try: + rclpy.spin(trajectory) + except KeyboardInterrupt: + pass + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + trajectory.destroy_node() + + print("Terminating trajectory node") \ No newline at end of file diff --git a/src/fdcl_uav/launch/fdcl_uav_launch.py b/src/fdcl_uav/launch/fdcl_uav_launch.py new file mode 100644 index 0000000..d07b4fd --- /dev/null +++ b/src/fdcl_uav/launch/fdcl_uav_launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='fdcl_uav', + namespace='estimator', + executable='estimator', + name='sim' + ), + Node( + package='fdcl_uav', + namespace='control', + executable='control', + name='sim' + ), + Node( + package='fdcl_uav', + namespace='trajectory', + executable='trajectory', + name='sim' + ), + Node( + package='fdcl_uav', + namespace='gui', + executable='gui', + name='sim' + ) + ]) \ No newline at end of file diff --git a/src/fdcl_uav/package.xml b/src/fdcl_uav/package.xml new file mode 100644 index 0000000..9876227 --- /dev/null +++ b/src/fdcl_uav/package.xml @@ -0,0 +1,18 @@ + + + + fdcl_uav + 0.0.0 + TODO: Package description + kani + TODO: License declaration + + rclpy + std_msgs + + ros2launch + + + ament_python + + diff --git a/src/fdcl_uav/resource/fdcl_uav b/src/fdcl_uav/resource/fdcl_uav new file mode 100644 index 0000000..e69de29 diff --git a/src/fdcl_uav/setup.cfg b/src/fdcl_uav/setup.cfg new file mode 100644 index 0000000..ad3be24 --- /dev/null +++ b/src/fdcl_uav/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fdcl_uav +[install] +install_scripts=$base/lib/fdcl_uav diff --git a/src/fdcl_uav/setup.py b/src/fdcl_uav/setup.py new file mode 100644 index 0000000..f1fa850 --- /dev/null +++ b/src/fdcl_uav/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup +from glob import glob +import os + +package_name = 'fdcl_uav' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='kani', + maintainer_email='kanishkegb@gwu.edu', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'estimator = fdcl_uav.estimator:main', + 'control = fdcl_uav.control:main', + 'trajectory = fdcl_uav.trajectory:main', + 'gui = fdcl_uav.gui:main', + ], + }, +) diff --git a/src/uav_gazebo/CMakeLists.txt b/src/uav_gazebo/CMakeLists.txt index 4f58a87..15da9f1 100644 --- a/src/uav_gazebo/CMakeLists.txt +++ b/src/uav_gazebo/CMakeLists.txt @@ -1,27 +1,29 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.22.1) project(uav_gazebo) -find_package(catkin REQUIRED COMPONENTS - gazebo_ros -) +set(CMAKE_CXX_STANDARD 14) -# Depend on system install of Gazebo -find_package(gazebo REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(xacro REQUIRED) -include_directories( - #include - ${catkin_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} - ${SDFormat_INCLUDE_DIRS} -) +xacro_add_files("urdf/uav.urdf.xacro" INSTALL DESTINATION urdf/) -# Build whatever you need here -# add_library(...) # TODO +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/DesiredData.msg" + "msg/ErrorData.msg" + "msg/ModeData.msg" + "msg/StateData.msg" + DEPENDENCIES std_msgs + ) -catkin_package( - DEPENDS - gazebo_ros - CATKIN_DEPENDS - INCLUDE_DIRS - LIBRARIES +install(DIRECTORY launch meshes worlds msg + DESTINATION share/${PROJECT_NAME} ) + +install(FILES model.config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/src/uav_gazebo/launch/simple_world.launch b/src/uav_gazebo/launch/simple_world.launch deleted file mode 100644 index 36ceeee..0000000 --- a/src/uav_gazebo/launch/simple_world.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/uav_gazebo/launch/uav_gazebo.launch.py b/src/uav_gazebo/launch/uav_gazebo.launch.py new file mode 100644 index 0000000..4b1a138 --- /dev/null +++ b/src/uav_gazebo/launch/uav_gazebo.launch.py @@ -0,0 +1,60 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + +from scripts import GazeboRosPaths + +import os + +pkg_gazebo_ros = get_package_share_directory('gazebo_ros') +pkg_uav_gazebo = get_package_share_directory('uav_gazebo') + +def generate_launch_description(): + + model_path, plugin_path, media_path = GazeboRosPaths.get_paths() + + # print(plugin_path) + + env = { + "GAZEBO_MODEL_PATH": model_path, + "GAZEBO_PLUGIN_PATH": plugin_path, + "GAZEBO_RESOURCE_PATH": media_path, + } + + # World + world_path = os.path.join(pkg_uav_gazebo, 'worlds', 'simple.world') + world_file = DeclareLaunchArgument( + 'world', + default_value=[world_path], + description='SDF world file' + ) + + # Gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'), + ) + ) + + # UAV + urdf_file_name = 'uav.urdf' + urdf_file = os.path.join(pkg_uav_gazebo, 'urdf', urdf_file_name) + + spawn_uav = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', 'uav', '-file', urdf_file, \ + '-x', '0', '-y', '0', '-z', '0.2'], + output='screen', + additional_env=env + ) + + return LaunchDescription([ + world_file, + gazebo, + spawn_uav + ]) \ No newline at end of file diff --git a/src/uav_gazebo/model.config b/src/uav_gazebo/model.config new file mode 100644 index 0000000..510a098 --- /dev/null +++ b/src/uav_gazebo/model.config @@ -0,0 +1,16 @@ + + + + uav + 1.0 + urdf/uav.urdf + + + Kanish Gamagedara + kanishkegb@gwu.edu + + + + The UAV model + + diff --git a/src/uav_gazebo/msg/DesiredData.msg b/src/uav_gazebo/msg/DesiredData.msg new file mode 100644 index 0000000..aeb5edb --- /dev/null +++ b/src/uav_gazebo/msg/DesiredData.msg @@ -0,0 +1,28 @@ +std_msgs/Header header + +float64[3] position +float64[3] velocity +float64[3] acceleration +float64[3] jerk +float64[3] snap + +float64[9] attitude +float64[3] angular_velocity +float64[3] angular_acceleration +float64[3] angular_jerk + +float64[3] b1c + +float64[3] b1 +float64[3] b1_dot +float64[3] b1_2dot + +float64[3] b3 +float64[3] b3_dot +float64[3] b3_2dot + +# Decoupled-yaw controller +float64 wc3 +float64 wc3_dot + +bool is_landed \ No newline at end of file diff --git a/src/uav_gazebo/msg/ErrorData.msg b/src/uav_gazebo/msg/ErrorData.msg new file mode 100644 index 0000000..2d8da42 --- /dev/null +++ b/src/uav_gazebo/msg/ErrorData.msg @@ -0,0 +1,8 @@ +std_msgs/Header header + +float64[3] position +float64[3] velocity +float64[3] attitude +float64[3] angular_velocity +float64[3] position_integral +float64[3] attitude_integral \ No newline at end of file diff --git a/src/uav_gazebo/msg/ModeData.msg b/src/uav_gazebo/msg/ModeData.msg new file mode 100644 index 0000000..ca53baf --- /dev/null +++ b/src/uav_gazebo/msg/ModeData.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +int32 mode +float64[3] x_offset +float64 yaw_offset \ No newline at end of file diff --git a/src/uav_gazebo/msg/StateData.msg b/src/uav_gazebo/msg/StateData.msg new file mode 100644 index 0000000..6fb82b6 --- /dev/null +++ b/src/uav_gazebo/msg/StateData.msg @@ -0,0 +1,7 @@ +std_msgs/Header header + +float64[3] position +float64[3] velocity +float64[3] acceleration +float64[9] attitude +float64[3] angular_velocity \ No newline at end of file diff --git a/src/uav_gazebo/package.xml b/src/uav_gazebo/package.xml index 0ca1f36..759d659 100644 --- a/src/uav_gazebo/package.xml +++ b/src/uav_gazebo/package.xml @@ -1,58 +1,27 @@ - + + + uav_gazebo 0.0.0 FDCL-UAV Gazebo Simulation - - - - fdcl - + Kanish - - - MIT + ament_cmake - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - gazebo_ros - gazebo_ros - roscpp - rospy - std_msgs - xacro - - - - + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + xacro + + uav_control_plugin + + + ament_cmake + diff --git a/src/uav_gazebo/urdf/uav.gazebo b/src/uav_gazebo/urdf/uav.gazebo deleted file mode 100644 index 0193645..0000000 --- a/src/uav_gazebo/urdf/uav.gazebo +++ /dev/null @@ -1,40 +0,0 @@ - - - - Gazebo/Orange - true - - - true - 100 - true - - uav_imu - link - 100.0 - 0.0001 - 0 0 0 - 0 0 0 - uav_imu - - 0 0 0 0 0 0 - - - - - - link - uav_pos - world - 0.01 - 10 - - - - - - link - uav_fm - - - diff --git a/src/uav_gazebo/urdf/uav.urdf.xacro b/src/uav_gazebo/urdf/uav.urdf.xacro new file mode 100644 index 0000000..95d449d --- /dev/null +++ b/src/uav_gazebo/urdf/uav.urdf.xacro @@ -0,0 +1,72 @@ + + + + + + + 0 0 0.2 0 0 0 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + true + + + 0 0 0 0 0 0 + true + 100 + true + + + + ~/out:=uav/imu + + + 0 0 0 0 0 0 + link + 100.0 + 0.0001 + + + + + + + + + odom:=uav/gps + + + link + world + 0.01 + 10 + + + + + + link + /uav/fm + + + + diff --git a/src/uav_gazebo/urdf/uav.xacro b/src/uav_gazebo/urdf/uav.xacro deleted file mode 100644 index 49482af..0000000 --- a/src/uav_gazebo/urdf/uav.xacro +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - 0 0 0.2 0 0 0 - - - - - - - - - - - - - - - - - - - - diff --git a/src/uav_gazebo/worlds/simple.world b/src/uav_gazebo/worlds/simple.world index df53976..004f077 100644 --- a/src/uav_gazebo/worlds/simple.world +++ b/src/uav_gazebo/worlds/simple.world @@ -1,11 +1,22 @@ + + + + /gazebo + + 1.0 + + + model://ground_plane + model://sun + diff --git a/src/uav_plugins/CMakeLists.txt b/src/uav_plugins/CMakeLists.txt index bbde9de..682f328 100644 --- a/src/uav_plugins/CMakeLists.txt +++ b/src/uav_plugins/CMakeLists.txt @@ -1,200 +1,38 @@ -cmake_minimum_required(VERSION 2.8.3) -project(fdcl_uav_plugins) +cmake_minimum_required(VERSION 3.22.1) +project(uav_control_plugin) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 14) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs geometry_msgs message_generation) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(gazebo REQUIRED) +find_package(gazebo_ros REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES uav_control - CATKIN_DEPENDS message_runtime -# DEPENDS system_lib +add_library(uav_control_plugin SHARED + src/uav_control_plugin.cpp + src/fdcl_matrix_utils.cpp + src/fdcl_ros_utils.cpp ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include +target_include_directories(uav_control_plugin PUBLIC + # ./include ../../libraries/eigen - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} + $ + $ ) -add_library(control_plugin - SHARED - src/control_plugin.cpp - src/fdcl_matrix_utils.cpp - src/fdcl_ros_utils.cpp +ament_target_dependencies(uav_control_plugin + rclcpp + std_msgs + geometry_msgs ) -target_link_libraries(control_plugin ${GAZEBO_LIBRARIES}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(uav_control -# ${catkin_LIBRARIES} -# ${GAZEBO_LIBRARIES} -# ) - -############# -## Install ## -############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_fdcl_uav_ros.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(TARGETS + uav_control_plugin + DESTINATION share/${PROJECT_NAME} +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() \ No newline at end of file diff --git a/src/uav_plugins/package.xml b/src/uav_plugins/package.xml index aa6b0f7..8f13b89 100644 --- a/src/uav_plugins/package.xml +++ b/src/uav_plugins/package.xml @@ -1,13 +1,15 @@ + + - fdcl_uav_plugins + uav_control_plugin 0.0.0 - The fdcl_uav_plugins package + The control plugin package - Kani + Kanish @@ -16,45 +18,25 @@ MIT - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - message_generation + ament_cmake + - - + geometry_msgs + geometry_msgs --> + + launch + ros2run + + + geometry_msgs + gazebo_ros + rclcpp + + + ament_cmake + + diff --git a/src/uav_plugins/src/control_plugin.cpp b/src/uav_plugins/src/uav_control_plugin.cpp similarity index 50% rename from src/uav_plugins/src/control_plugin.cpp rename to src/uav_plugins/src/uav_control_plugin.cpp index 7eae05d..22c8b33 100644 --- a/src/uav_plugins/src/control_plugin.cpp +++ b/src/uav_plugins/src/uav_control_plugin.cpp @@ -1,21 +1,28 @@ -#include -#include -#include -#include +// #include +// #include +// #include +// #include -#include -#include -#include -#include +// #include +// #include +// // #include +// // #include #include #include -#include -#include +#include +#include + +// #include +// #include "std_msgs/String.h" + +#include +#include +#include + +#include -#include -#include "std_msgs/String.h" #include "fdcl/common_types.hpp" #include "fdcl/ros_utils.hpp" @@ -32,23 +39,29 @@ class UavControlPlugin : public ModelPlugin { public: - void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) { - this->world = _model->GetWorld(); - this->model = _model; + std::cout << "Loading UAV Control Plugin\n"; + + rn_ = gazebo_ros::Node::Get(sdf); + + world_ = model->GetWorld(); + model_ = model; - this->link_name = _sdf->GetElement("bodyName")->Get(); - this->link = _model->GetLink(this->link_name); + link_name_ = sdf->GetElement("body_name")->Get(); + link_ = model->GetLink(link_name_); // Listen to the update event. This event is broadcast every // simulation iteration. - this->update_connection = event::Events::ConnectWorldUpdateBegin( \ + update_connection_ = event::Events::ConnectWorldUpdateBegin( \ std::bind(&UavControlPlugin::update, this)); // Start the ROS subscriber. - this->topic_name = _sdf->GetElement("topicName")->Get(); - this->sub_fm = this->n.subscribe(this->topic_name, 1, \ - UavControlPlugin::update_fm); + topic_name_ = sdf->GetElement("topic_name")->Get(); + + sub_fm_ = rn_->create_subscription( \ + topic_name_, 1, std::bind(&UavControlPlugin::update_fm, this, \ + std::placeholders::_1)); } @@ -57,66 +70,64 @@ class UavControlPlugin : public ModelPlugin no_msg_counter++; if (no_msg_counter > 100) { - this->reset_uav(); + reset_uav(); if (!print_reset_message) { - std::cout << ros::Time::now() - << ": no new force messages, resetting UAV .." - << std::endl; + std::cout << "UAV Plugin: No new force messages" + << "\tresetting UAV\n"; print_reset_message = true; } return; } // Both must be in world frame. - this->calculate_force(); - this->link->SetForce(this->force); - this->link->SetTorque(this->M_out); + calculate_force(); + link_->SetForce(force); + link_->SetTorque(M_out); } void calculate_force(void) { - this->update_uav_rotation(); + update_uav_rotation(); fdcl::Vector3 force_body; - this->ignition_to_eigen(this->f, force_body); + ignition_to_eigen(this->f, force_body); fdcl::Vector3 force_world = this->R * force_body; - this->eigen_to_ignition(force_world, this->force); + eigen_to_ignition(force_world, this->force); fdcl::Vector3 M_body; - this->ignition_to_eigen(this->M, M_body); + ignition_to_eigen(this->M, M_body); - fdcl::Vector3 M_world = this->R * M_body; - this->eigen_to_ignition(M_world, this->M_out); + fdcl::Vector3 M_world = R * M_body; + eigen_to_ignition(M_world, this->M_out); } void update_uav_rotation(void) { - ignition::math::Pose3d pose = this->link->WorldPose(); + ignition::math::Pose3d pose = link_->WorldPose(); ignition::math::Quaterniond q = pose.Rot(); fdcl::Vector3 q13(q.X(), q.Y(), q.Z()); double q4 = q.W(); fdcl::Matrix3 hat_q = fdcl::hat(q13); - this->R = eye3 + 2 * q4 * hat_q + 2 * hat_q * hat_q; + R = eye3 + 2 * q4 * hat_q + 2 * hat_q * hat_q; } - static void update_fm(const geometry_msgs::Wrench::ConstPtr& msg) + void update_fm(const geometry_msgs::msg::WrenchStamped &msg) const { + f[0] = msg.wrench.force.x; + f[1] = msg.wrench.force.y; + f[2] = msg.wrench.force.z; - f[0] = msg->force.x; - f[1] = msg->force.y; - f[2] = msg->force.z; - - M[0] = msg->torque.x; - M[1] = msg->torque.y; - M[2] = msg->torque.z; + M[0] = msg.wrench.torque.x; + M[1] = msg.wrench.torque.y; + M[2] = msg.wrench.torque.z; no_msg_counter = 0; print_reset_message = false; @@ -125,8 +136,8 @@ class UavControlPlugin : public ModelPlugin void reset_uav(void) { - this->link->SetForce(zero_fM); - this->link->SetTorque(zero_fM); + link_->SetForce(zero_fM); + link_->SetTorque(zero_fM); } @@ -151,22 +162,22 @@ class UavControlPlugin : public ModelPlugin private: - ros::Time t0 = ros::Time::now(); + physics::ModelPtr model_; + physics::WorldPtr world_; + physics::LinkPtr link_; - physics::ModelPtr model; - physics::WorldPtr world; - physics::LinkPtr link; - - std::string link_name; - std::string topic_name; + std::string link_name_; + std::string topic_name_; - event::ConnectionPtr update_connection; - ros::Subscriber sub_fm; - ros::NodeHandle n; + event::ConnectionPtr update_connection_; + + rclcpp::Node::SharedPtr rn_; + rclcpp::Subscription::SharedPtr sub_fm_; + + static igm::Vector3d f; static igm::Vector3d M; igm::Vector3d M_out; - static igm::Vector3d f; fdcl::Matrix3 R = fdcl::Matrix3::Identity(); igm::Vector3d force = igm::Vector3d::Zero;