From c034d649b1a588fcb70c98b21466f64513b9fa48 Mon Sep 17 00:00:00 2001 From: Brennan Date: Tue, 3 May 2022 16:16:12 -0400 Subject: [PATCH 01/11] Added GUI --- CMakeLists.txt | 1 + msg/system_stats.msg | 2 + plugins/{sensors => camera}/camera0.py | 0 .../{sensors => camera}/camera_subscriber.py | 0 plugins/{sensors => imu}/imu_data.py | 0 plugins/{sensors => imu}/nxp-by-file.py | 0 plugins/{sensors => imu}/simulate_nineDof.py | 0 .../thruster}/thruster_sensor.py | 0 plugins/sensors/__init__.py | 0 scripts/vehicle_stats.py | 31 ++ surface/UI/surface_ui.py | 119 ++++++++ .../{ => joystick}/chaos_joystick_sender.py | 0 surface/joystick/joystick_sender.py | 267 ++++++++++++++++++ 13 files changed, 420 insertions(+) create mode 100644 msg/system_stats.msg rename plugins/{sensors => camera}/camera0.py (100%) rename plugins/{sensors => camera}/camera_subscriber.py (100%) rename plugins/{sensors => imu}/imu_data.py (100%) rename plugins/{sensors => imu}/nxp-by-file.py (100%) rename plugins/{sensors => imu}/simulate_nineDof.py (100%) rename plugins/{sensors => motors/thruster}/thruster_sensor.py (100%) delete mode 100644 plugins/sensors/__init__.py create mode 100644 scripts/vehicle_stats.py create mode 100644 surface/UI/surface_ui.py rename surface/{ => joystick}/chaos_joystick_sender.py (100%) create mode 100755 surface/joystick/joystick_sender.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ffe918..dcb0eff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,7 @@ add_message_files( trajectory.msg translation.msg magnetometer.msg + system_stats.msg ) ## Generate services in the 'srv' folder diff --git a/msg/system_stats.msg b/msg/system_stats.msg new file mode 100644 index 0000000..34f0063 --- /dev/null +++ b/msg/system_stats.msg @@ -0,0 +1,2 @@ +float32 cpu_usage +float32 temp diff --git a/plugins/sensors/camera0.py b/plugins/camera/camera0.py similarity index 100% rename from plugins/sensors/camera0.py rename to plugins/camera/camera0.py diff --git a/plugins/sensors/camera_subscriber.py b/plugins/camera/camera_subscriber.py similarity index 100% rename from plugins/sensors/camera_subscriber.py rename to plugins/camera/camera_subscriber.py diff --git a/plugins/sensors/imu_data.py b/plugins/imu/imu_data.py similarity index 100% rename from plugins/sensors/imu_data.py rename to plugins/imu/imu_data.py diff --git a/plugins/sensors/nxp-by-file.py b/plugins/imu/nxp-by-file.py similarity index 100% rename from plugins/sensors/nxp-by-file.py rename to plugins/imu/nxp-by-file.py diff --git a/plugins/sensors/simulate_nineDof.py b/plugins/imu/simulate_nineDof.py similarity index 100% rename from plugins/sensors/simulate_nineDof.py rename to plugins/imu/simulate_nineDof.py diff --git a/plugins/sensors/thruster_sensor.py b/plugins/motors/thruster/thruster_sensor.py similarity index 100% rename from plugins/sensors/thruster_sensor.py rename to plugins/motors/thruster/thruster_sensor.py diff --git a/plugins/sensors/__init__.py b/plugins/sensors/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/scripts/vehicle_stats.py b/scripts/vehicle_stats.py new file mode 100644 index 0000000..0cef814 --- /dev/null +++ b/scripts/vehicle_stats.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 + +import rospy +from scripts.trajectory_sender_test import publisher +from wurov.msg import system_stats +import psutil +from gpiozero import CPUTemperature + +class vehicle_stats: + def __init__(self): + rospy.init_node('vehicle_stats', anonymous=True) + + self.nineDof_current_pub = rospy.Publisher( + 'vehicle_stats', system_stats, queue_size=3) + + + rospy.Timer(rospy.Duration(4, self.publisher) + + rospy.spin() + + def publisher(self, data): + ret_msg = system_stats() + ret_msg.cpu_usage = psutil.cpu_percent() + + cpu = CPUTemperature() + + ret_msg.temp = cpu.temperature + + self.publisher.publish(ret_msg) +if __name__ == '__main__': + vehicle_stats() diff --git a/surface/UI/surface_ui.py b/surface/UI/surface_ui.py new file mode 100644 index 0000000..a417c0e --- /dev/null +++ b/surface/UI/surface_ui.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python + +from PyQt5 import QtWidgets, QtGui +from PyQt5.QtCore import QTimer +from PyQt5.QtWidgets import QApplication, QWidget +from PyQt5.QtGui import * +from PyQt5.QtWidgets import * +from PyQt5.QtCore import * +import os +import sys +import rospy + +from wurov.msg import system_stats +from wurov.msg import ninedof + +# Configurations +fullscreenMode = True +cameraIndex1 = 2 +cameraIndex2 = 1 +# Enter Screen Resolution for surface station(Only if fullscreenMode is disabled) +height = 1080 +width = 1920 +# Fonts +Header = QtGui.QFont("Roboto", 12, QtGui.QFont.Bold) +Body = QtGui.QFont("Roboto", 10) + + +class WUROV(QWidget): + def __init__(self): + rospy.init_node('gui', anonymous=True) + rospy.Subscriber('ninedof_values', ninedof, self.sensor) + + + # Style settings for GUI + super(WUROV, self).__init__() + self.setGeometry(0, 0, width, height) + self.setWindowTitle("WUROV Control") + self.setStyleSheet('background-color: ' + '#2C2F33' + ';color: ' + 'white') + + # Grid layout for GUI + self.nineDof() + grid = QtWidgets.QGridLayout() + grid.addWidget(self.cam1, 1, 0) + grid.addWidget(self.cam2, 1, 1) + grid.addWidget(self.accel_x, 2, 0) + grid.addWidget(self.accel_y, 3, 0) + grid.addWidget(self.accel_z, 4, 0) + grid.addWidget(self.roll, 5, 0) + grid.addWidget(self.pitch, 6, 0) + grid.addWidget(self.yaw, 7, 0) + self.setLayout(grid) + + # Refreshes Qlabel on a timer + timer = QTimer(self) + timer.start() + timer = QTimer(self) + timer.setInterval(10) + timer.timeout.connect(self.sensor) + timer.start() + + # NineDof labels to display sensor read outs + def nineDof(self): + self.accel_x = QtWidgets.QLabel(self) + self.accel_x.setFont(QtGui.QFont(Body)) + self.accel_x.adjustSize() + + self.accel_y = QtWidgets.QLabel(self) + self.accel_y.setFont(QtGui.QFont(Body)) + self.accel_y.adjustSize() + + self.accel_z = QtWidgets.QLabel(self) + self.accel_z.setFont(QtGui.QFont(Body)) + self.accel_z.adjustSize() + + self.yaw = QtWidgets.QLabel(self) + self.yaw.setFont(QtGui.QFont(Body)) + self.yaw.adjustSize() + + self.pitch = QtWidgets.QLabel(self) + self.pitch.setFont(QtGui.QFont(Body)) + self.pitch.adjustSize() + + self.roll = QtWidgets.QLabel(self) + self.roll.setFont(QtGui.QFont(Body)) + self.roll.adjustSize() + + # Sensor function updates labels from receive_nineDof.py + def sensor(self, data): + self.accel_x.setText("X: " + ndof.get_x()) + self.accel_y.setText("Y: " + ndof.get_y()) + self.accel_z.setText("Z: " + ndof.get_z()) + self.roll.setText("Roll: " + ndof.get_roll()) + self.pitch.setText("Pitch: " + ndof.get_pitch()) + self.yaw.setText("Yaw: " + ndof.get_yaw()) + + def alert(self, s): + """ + This handle errors and displaying alerts. + """ + err = QErrorMessage(self) + err.showMessage(s) + +# Code to be executed +if __name__ == "__main__": + import sys + + app = QApplication(sys.argv) + win = WUROV() + + + def window(): + if fullscreenMode: + win.showFullScreen() + else: + win.show() + sys.exit(app.exec_()) + + + window() \ No newline at end of file diff --git a/surface/chaos_joystick_sender.py b/surface/joystick/chaos_joystick_sender.py similarity index 100% rename from surface/chaos_joystick_sender.py rename to surface/joystick/chaos_joystick_sender.py diff --git a/surface/joystick/joystick_sender.py b/surface/joystick/joystick_sender.py new file mode 100755 index 0000000..e4c3889 --- /dev/null +++ b/surface/joystick/joystick_sender.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python3 + +""" + + This file is part of Enbarr. + + Enbarr is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Enbarr is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Enbarr. If not, see . + +""" + +import os +import rospy +import pygame +import sys +import argparse +import socket +import os + +from wurov.msg import surface_command, io_request, trajectory + +class joystick_sender: + def __init__(self): + rospy.init_node('joystick_sender_'+socket.gethostname(), anonymous=False) + + self.publisher = rospy.Publisher('surface_command', surface_command, queue_size=3) + + parser = argparse.ArgumentParser("Find a plugged in joystick and send it over /surface_command.") + parser.add_argument('--config_name', type=str, help='Set the name of the file we should use as a config (from ' + 'within the config directory)', required=True) + self.args = parser.parse_args(rospy.myargv()[1:]) + namespace = f"/{rospy.get_name()}/Controllers/{self.args.config_name}" + + self.controllerConfig = rospy.get_param(namespace) + + self.already_sent_zero = True # Set to true so that we aren't trying to set anything to zero on startup + self.last_sent = "" + self.servo_in_position = True + self.stepper_already_moving = True + self.thruster_already_killed = True + self.thruster_already_unkilled = True + self.joystick = None + + # We'll use this to try not to connect to the joystick too quickly. + rate = rospy.Rate(5) + + #Set pygame to headless + os.environ["SDL_VIDEODRIVER"] = "dummy" + pygame.display.init() + + try: #Try to init joystick + pygame.joystick.init() + while pygame.joystick.get_count() == 0: + rospy.logerr("No joystick connected!") + #pygame.joystick.quit() + pygame.joystick.init() + rate.sleep() + rospy.loginfo("Found a joystick to use.") + pygame.init() + self.joystick = pygame.joystick.Joystick(0) + self.joystick.init() + except pygame.error: + rospy.logerr("Failed to initialize joystick!") + sys.exit(1) + + # Now that we're not using the rate to slow down our joystick connection, let's bring it to something we'll use. + rospy.Timer(rospy.Duration(0.1), self.update) + rospy.spin() + + def update(self, data): + lastmsg = surface_command() + + pygame.event.get() + horizontal_axis = self.joystick.get_axis(self.controllerConfig["translationX_axis"]) # Horizontal: -1 is full left, 1 is full right + vertical_axis = self.joystick.get_axis(self.controllerConfig["translationY_axis"]) # Vertical: -1 is full forward, 1 is full back + twist_axis = self.joystick.get_axis(self.controllerConfig["translationZ_axis"]) # Twist: -1 is full counter-clockwise, 1 is clockwise + + if self.controllerConfig["depth_axis"]["inputCount"] == 1: + depth_axis = self.joystick.get_axis(self.controllerConfig["depth_axis"]["inputOne_Axis"]) # Lever: 1 is full down, -1 is full up + elif self.controllerConfig["depth_axis"]["inputCount"] == 2: + depthAxisOne = -1 * self.joystick.get_axis((self.controllerConfig["depth_axis"]["inputOne_Axis"])) + depthAxisTwo = self.joystick.get_axis((self.controllerConfig["depth_axis"]["inputTwo_Axis"])) + + if depthAxisOne > 0: + depthAxisOne = 0 + if depthAxisTwo < 0: + depthAxisTwo = 0 + + depth_axis = depthAxisOne + (depthAxisTwo) + else: + rospy.loginfo("Depth config set wrong") + quit() + + self.msg = surface_command() + self.msg.desired_trajectory.translation.x = -1 * horizontal_axis + self.msg.desired_trajectory.translation.y = vertical_axis + self.msg.desired_trajectory.translation.z = depth_axis # Flipped: forward is negative, that's dumb + self.msg.desired_trajectory.orientation.yaw = -1 * twist_axis + + msg = self.handle_peripherals(self.joystick, self.msg) + if self.different_msg(lastmsg, msg): + self.publisher.publish(msg) + lastmsg = msg + + def hat_to_val(self, a, b): + if a == 0: + if b == 0: + return None + if b == 1: + return "top_front" + if b == -1: + return "top_left" + if a == 1: + if b == 0: + return "top_right" + if b == 1: + return "front_right" + if b == -1: + return "front_left" + if a == -1: + if b == 0: + return "top_back" + if b == 1: + return "back_left" + if b == -1: + return "back_right" + + + def handle_peripherals(self, joystick_, msg_): + hat = joystick_.get_hat(0) + hat = self.hat_to_val(hat[0], hat[1]) + + io_request_ = io_request() + io_request_.executor = "individual_thruster_control" + + if hat is not None: + io_request_.float = 0.75 + io_request_.string = hat + else: + io_request_.float = 0.5 + io_request_.string = "all" + self.msg.io_requests += (io_request_,) + + return msg_ # If we wanted to do something with button presses, we could mess around with that sort of thing here. + + + def different_msg(self, msg1, msg2): + if msg1 is None or msg2 is None: + return True + + return msg1.desired_trajectory.orientation != msg2.desired_trajectory.orientation or \ + msg1.desired_trajectory.translation != msg2.desired_trajectory.translation or \ + msg1.io_requests != msg2.io_requests + + def hat_to_val(self, a, b): + if a == 0: + if b == 0: + return None + if b == 1: + return "top_front" + if b == -1: + return "top_back" + if a == 1: + if b == 0: + return "top_right" + if b == 1: + return "front_right" + if b == -1: + return "back_right" + if a == -1: + if b == 0: + return "top_left" + if b == 1: + return "front_left" + if b == -1: + return "back_left" + + + def handle_peripherals(self, joystick, msg): + hat = joystick.get_hat(0) + hat = self.hat_to_val(hat[0], hat[1]) + io_request_ = io_request() + + if hat is None: + if not self.already_sent_zero: + io_request_.executor = "individual_thruster_control" + io_request_.string = self.last_sent + self.already_sent_zero = True + msg.io_requests += (io_request_,) + else: + io_request_.executor = "individual_thruster_control" + io_request_.string = hat + io_request_.float = 0.75 + self.last_sent = hat + self.already_sent_zero = False + msg.io_requests += (io_request_,) + + if joystick.get_button(self.controllerConfig["safetyButton"]): # Safety trigger: Do not Send trajectory data if this trigger is held. + msg.desired_trajectory = trajectory() + + if not joystick.get_button(self.controllerConfig["boostMode"]): # 'Boost mode': If this button is pressed, multiply trajectory by 2 + # We implement this by always cutting by 2, and then when the button is pressed, not cutting in half. + msg.desired_trajectory.translation.x = msg.desired_trajectory.translation.x / 2 + msg.desired_trajectory.translation.y = msg.desired_trajectory.translation.y / 2 + msg.desired_trajectory.translation.z = msg.desired_trajectory.translation.z / 2 + msg.desired_trajectory.orientation.roll = msg.desired_trajectory.orientation.roll / 2 + msg.desired_trajectory.orientation.pitch = msg.desired_trajectory.orientation.pitch / 2 + msg.desired_trajectory.orientation.yaw = msg.desired_trajectory.orientation.yaw / 2 + + if joystick.get_button(self.controllerConfig["killThrusters"]): # Kill thrusters button + if not self.thruster_already_killed: + io_request_ = io_request() + io_request_.executor = "kill_thruster" + io_request_.string = "front_left" + msg.io_requests += (io_request_,) + + io_request_ = io_request() + io_request_.executor = "kill_thruster" + io_request_.string = "front_right" + msg.io_requests += (io_request_,) + + io_request_ = io_request() + io_request_.executor = "kill_thruster" + io_request_.string = "top_front" + msg.io_requests += (io_request_,) + + self.thruster_already_killed = True + elif self.thruster_already_killed: # Make sure the button is released before we send another stream of kill stuff + self.thruster_already_killed = False + + if joystick.get_button(self.controllerConfig["unkillThrusters"]): # Un-kill thrusters button + if not self.thruster_already_unkilled: + io_request_ = io_request() + io_request_.executor = "unkill_thruster" + io_request_.string = "front_left" + msg.io_requests += (io_request_,) + + io_request_ = io_request() + io_request_.executor = "unkill_thruster" + io_request_.string = "front_right" + msg.io_requests += (io_request_,) + + io_request_ = io_request() + io_request_.executor = "unkill_thruster" + io_request_.string = "top_front" + msg.io_requests += (io_request_,) + + self.thruster_already_unkilled = True + elif self.thruster_already_unkilled: # Make sure the button is released before we send another stream of un-kill stuff + self.thruster_already_unkilled = False + + return msg # If we wanted to do something with button presses, we could mess around with that sort of thing here. + + + +if __name__ == '__main__': + joystick_sender() From a2551ab649415f479671c0afaf57a1bd2f503946 Mon Sep 17 00:00:00 2001 From: Brennan Date: Fri, 6 May 2022 14:58:33 -0400 Subject: [PATCH 02/11] Updated GUI --- CMakeLists.txt | 4 - msg/magnetometer.msg | 5 - plugins/imu/imu_data.py | 42 --- .../imu_data_fxas21002c_fxos8700.py | 0 plugins/{sensors => imu}/simulate_imu_data.py | 0 plugins/imu/simulate_nineDof.py | 59 ---- surface/UI/debug_ui.py | 181 ++++++++++++ surface/UI/default.jpg | Bin 0 -> 8196 bytes surface/UI/surface_ui.py | 100 +++---- surface/joystick_sender.py | 267 ------------------ 10 files changed, 221 insertions(+), 437 deletions(-) delete mode 100644 msg/magnetometer.msg delete mode 100755 plugins/imu/imu_data.py rename plugins/{sensors => imu}/imu_data_fxas21002c_fxos8700.py (100%) rename plugins/{sensors => imu}/simulate_imu_data.py (100%) delete mode 100755 plugins/imu/simulate_nineDof.py create mode 100755 surface/UI/debug_ui.py create mode 100644 surface/UI/default.jpg mode change 100644 => 100755 surface/UI/surface_ui.py delete mode 100755 surface/joystick_sender.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f05f526..40eb70c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,11 +59,7 @@ add_message_files( thrustermove.msg trajectory.msg translation.msg -<<<<<<< HEAD - magnetometer.msg system_stats.msg -======= ->>>>>>> 45fba0cac002b99a5008027c1ef0ee9dac242243 ) ## Generate services in the 'srv' folder diff --git a/msg/magnetometer.msg b/msg/magnetometer.msg deleted file mode 100644 index bbe8e51..0000000 --- a/msg/magnetometer.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header - -float32 x -float32 y -float32 z \ No newline at end of file diff --git a/plugins/imu/imu_data.py b/plugins/imu/imu_data.py deleted file mode 100755 index 3427e96..0000000 --- a/plugins/imu/imu_data.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python3 - -import board; -import busio; -import adafruit_fxas21002c; -import adafruit_fxos8700; -import rospy -from wurov.msg import ninedof - - - -class imu_data: - def __init__(self): - rospy.init_node('imu_data', anonymous=True) - - i2c = busio.I2C(board.SCL, board.SDA) - self.gyroSensor = adafruit_fxas21002c.FXAS21002C(i2c) - self.sensor = adafruit_fxos8700.FXOS8700(i2c) - - self.nineDof_current_pub = rospy.Publisher('ninedof_values', ninedof, queue_size=3) - - rospy.Timer(rospy.Duration(0.1), self.imuRead) - - rospy.spin() - - def imuRead(self, data): - sendval_ninedof = ninedof() - - sendval_ninedof.orientation.roll = self.gyroSensor.gyroscope[0] - sendval_ninedof.orientation.pitch = self.gyroSensor.gyroscope[1] - sendval_ninedof.orientation.yaw = self.gyroSensor.gyroscope[2] - sendval_ninedof.translation.x = self.sensor.accelerometer[0] - sendval_ninedof.translation.y = self.sensor.accelerometer[1] - sendval_ninedof.translation.z = self.sensor.accelerometer[2] - sendval_ninedof.magnetometer.x = self.sensor.magnetometer[0] - sendval_ninedof.magnetometer.y = self.sensor.magnetometer[1] - sendval_ninedof.magnetometer.z = self.sensor.magnetometer[2] - - self.nineDof_current_pub.publish(sendval_ninedof) - -if __name__ == '__main__': - imu_data() \ No newline at end of file diff --git a/plugins/sensors/imu_data_fxas21002c_fxos8700.py b/plugins/imu/imu_data_fxas21002c_fxos8700.py similarity index 100% rename from plugins/sensors/imu_data_fxas21002c_fxos8700.py rename to plugins/imu/imu_data_fxas21002c_fxos8700.py diff --git a/plugins/sensors/simulate_imu_data.py b/plugins/imu/simulate_imu_data.py similarity index 100% rename from plugins/sensors/simulate_imu_data.py rename to plugins/imu/simulate_imu_data.py diff --git a/plugins/imu/simulate_nineDof.py b/plugins/imu/simulate_nineDof.py deleted file mode 100755 index cedb529..0000000 --- a/plugins/imu/simulate_nineDof.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 - -""" - - This file is part of Enbarr. - - Enbarr is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Enbarr is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Enbarr. If not, see . - -""" - -import rospy -import numpy as np -from wurov.msg import ninedof - - -class simulate_nineDof: - def __init__(self): - rospy.init_node('simulate_nineDof', anonymous=True) - - self.nineDof_current_pub = rospy.Publisher( - 'ninedof_values', ninedof, queue_size=3) - - rospy.Timer(rospy.Duration(0.1), self.publisher) - - rospy.spin() - - def publisher(self, data): - sendval_ninedof = ninedof() - # While this node is still running, keep getting sensor values - # In this case, it's simulated, so we're making up values - # In your case, replace this block with however you're getting values - # A real sensor is likley providing accelerometer, gyroscope, and magnetometer values - # Combine them to produce more accure roll/pitch/yaw/x/y/z values - sendval_ninedof.orientation.roll = np.random.normal() - sendval_ninedof.orientation.pitch = np.random.normal() - sendval_ninedof.orientation.yaw = np.random.normal() - sendval_ninedof.translation.x = np.random.normal() - sendval_ninedof.translation.y = np.random.normal() - sendval_ninedof.translation.z = np.random.normal() - sendval_ninedof.magnetometer.x = np.random.normal() - sendval_ninedof.magnetometer.y = np.random.normal() - sendval_ninedof.magnetometer.z = np.random.normal() - - self.nineDof_current_pub.publish(sendval_ninedof) - - -if __name__ == '__main__': - simulate_nineDof() diff --git a/surface/UI/debug_ui.py b/surface/UI/debug_ui.py new file mode 100755 index 0000000..0048918 --- /dev/null +++ b/surface/UI/debug_ui.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 + +from PySide2 import QtCore, QtGui, QtWidgets +from PySide2.QtCore import QTimer +from PySide2.QtWidgets import QApplication, QWidget +from PySide2.QtGui import * +from PySide2.QtWidgets import * +from PySide2.QtCore import * +import os +import sys +import rospy +import math +from PySide2extn.RoundProgressBar import roundProgressBar + +from wurov.msg import system_stats +from sensor_msgs.msg import Imu, MagneticField + +# Configurations +fullscreenMode = True +cameraIndex1 = 2 +cameraIndex2 = 1 +# Enter Screen Resolution for surface station(Only if fullscreenMode is disabled) +height = 1080 +width = 1920 +# Fonts +Header = QtGui.QFont("Roboto", 12, QtGui.QFont.Bold) +Body = QtGui.QFont("Roboto", 10) + + +class WUROV(QWidget): + def __init__(self): + rospy.init_node('gui', anonymous=True) + rospy.Subscriber('imu/data_raw', Imu, self.update_raw_imu) + rospy.Subscriber('imu/data', Imu, self.update_filtered_imu) + + # Style settings for GUI + super(WUROV, self).__init__() + self.setGeometry(0, 0, width, height) + self.setWindowTitle("WUROV Control") + self.setStyleSheet('background-color: ' + + '#2C2F33' + ';color: ' + 'white') + + # Grid layout for GUI + self.nineDof() + grid = QtWidgets.QGridLayout() + + grid.addWidget(self.raw_title, 1, 0) + grid.addWidget(self.raw_accel_x, 2, 0) + grid.addWidget(self.raw_accel_y, 3, 0) + grid.addWidget(self.raw_accel_z, 4, 0) + grid.addWidget(self.raw_ang_x, 5, 0) + grid.addWidget(self.raw_ang_y, 6, 0) + grid.addWidget(self.raw_ang_z, 7, 0) + + grid.addWidget(self.filtered_title, 1, 1) + grid.addWidget(self.filtered_accel_x, 2, 1) + grid.addWidget(self.filtered_accel_y, 3, 1) + grid.addWidget(self.filtered_accel_z, 4, 1) + grid.addWidget(self.filtered_ang_x, 5, 1) + grid.addWidget(self.filtered_ang_y, 6, 1) + grid.addWidget(self.filtered_ang_z, 7, 1) + + grid.addWidget(self.speed_title, 1, 2) + grid.addWidget(self.speed, 2, 2) + self.setLayout(grid) + + # NineDof labels to display sensor read outs + + def nineDof(self): + # raw ninedof + self.raw_title = QtWidgets.QLabel(self) + self.raw_title.setFont(QtGui.QFont(Header)) + self.raw_title.adjustSize() + self.raw_title.setText("Raw IMU: ") + + self.raw_accel_x = QtWidgets.QLabel(self) + self.raw_accel_x.setFont(QtGui.QFont(Body)) + self.raw_accel_x.adjustSize() + + self.raw_accel_y = QtWidgets.QLabel(self) + self.raw_accel_y.setFont(QtGui.QFont(Body)) + self.raw_accel_y.adjustSize() + + self.raw_accel_z = QtWidgets.QLabel(self) + self.raw_accel_z.setFont(QtGui.QFont(Body)) + self.raw_accel_z.adjustSize() + + self.raw_ang_x = QtWidgets.QLabel(self) + self.raw_ang_x.setFont(QtGui.QFont(Body)) + self.raw_ang_x.adjustSize() + + self.raw_ang_y = QtWidgets.QLabel(self) + self.raw_ang_y.setFont(QtGui.QFont(Body)) + self.raw_ang_y.adjustSize() + + self.raw_ang_z = QtWidgets.QLabel(self) + self.raw_ang_z.setFont(QtGui.QFont(Body)) + self.raw_ang_z.adjustSize() + # filtered ninedof + self.filtered_title = QtWidgets.QLabel(self) + self.filtered_title.setFont(QtGui.QFont(Header)) + self.filtered_title.adjustSize() + self.filtered_title.setText("Filtered IMU: ") + + self.filtered_accel_x = QtWidgets.QLabel(self) + self.filtered_accel_x.setFont(QtGui.QFont(Body)) + self.filtered_accel_x.adjustSize() + + self.filtered_accel_y = QtWidgets.QLabel(self) + self.filtered_accel_y.setFont(QtGui.QFont(Body)) + self.filtered_accel_y.adjustSize() + + self.filtered_accel_z = QtWidgets.QLabel(self) + self.filtered_accel_z.setFont(QtGui.QFont(Body)) + self.filtered_accel_z.adjustSize() + + self.filtered_ang_x = QtWidgets.QLabel(self) + self.filtered_ang_x.setFont(QtGui.QFont(Body)) + self.filtered_ang_x.adjustSize() + + self.filtered_ang_y = QtWidgets.QLabel(self) + self.filtered_ang_y.setFont(QtGui.QFont(Body)) + self.filtered_ang_y.adjustSize() + + self.filtered_ang_z = QtWidgets.QLabel(self) + self.filtered_ang_z.setFont(QtGui.QFont(Body)) + self.filtered_ang_z.adjustSize() + # speedometer + self.speed_title = QtWidgets.QLabel(self) + self.speed_title.setFont(QtGui.QFont(Header)) + self.speed_title.adjustSize() + self.speed_title.setText("Speed Data: ") + + self.speed = roundProgressBar() + self.speed.rpb_setRange(0, 1.5) + self.speed.rpb_setTextFormat('Value') + + # Update functions change labels based on callback + def update_raw_imu(self, data): + self.raw_accel_x.setText(f"Accel X: {data.linear_acceleration.x}") + self.raw_accel_y.setText(f"Accel Y: {data.linear_acceleration.y}") + self.raw_accel_z.setText(f"Accel Z: {data.linear_acceleration.z}") + self.raw_ang_x.setText(f"Angular X: {data.angular_velocity.x}") + self.raw_ang_y.setText(f"Angular Y: {data.angular_velocity.y}") + self.raw_ang_z.setText(f"Angular Z: {data.angular_velocity.z}") + + def update_filtered_imu(self, data): + self.filtered_accel_x.setText(f"Accel X: {data.linear_acceleration.x}") + self.filtered_accel_y.setText(f"Accel Y: {data.linear_acceleration.y}") + self.filtered_accel_z.setText(f"Accel Z: {data.linear_acceleration.z}") + self.filtered_ang_x.setText(f"Angular X: {data.angular_velocity.x}") + self.filtered_ang_y.setText(f"Angular Y: {data.angular_velocity.y}") + self.filtered_ang_z.setText(f"Angular Z: {data.angular_velocity.z}") + + acel_mag = math.sqrt(data.linear_acceleration.x**2 + + data.linear_acceleration.y**2 + data.linear_acceleration.z**2) + self.speed.rpb_setValue(acel_mag) + + def alert(self, s): + """ + This handle errors and displaying alerts. + """ + err = QErrorMessage(self) + err.showMessage(s) + + +# Code to be executed +if __name__ == "__main__": + import sys + + app = QApplication(sys.argv) + win = WUROV() + + def window(): + if fullscreenMode: + win.showFullScreen() + else: + win.show() + sys.exit(app.exec_()) + + window() diff --git a/surface/UI/default.jpg b/surface/UI/default.jpg new file mode 100644 index 0000000000000000000000000000000000000000..48d8e7d282df1e7ed2a3e0f329e702ce7d2dc5d5 GIT binary patch literal 8196 zcmYjUWmr_v)}E0rNeQL9TSDoS?(UREDH*!E8)=Xfl&+x#Boz=u7`kf!>E=7&z4!V4 z&CHp7_S)}SZ)|lr8JQaj5J*Q_LRDK;fbtOdoPhWw2h1FRB7i6&N1G^9SV&bsi3(Ch zL$h^SwmZN(;4-=OtGm;K-^1gp{jY!8_ut>2jM~0#8}h#dxj7#th*luoUx7N$OejG2 z*(TDrIx)@x=rhisa~K0PT1eaU>|Ylbb+dy;Rmmk5slv_nvkjBdMrFxGhwSio-g9|A-NirBl?B(_=(kPaDVjUZL^DL_2&V)0x&PL6``RHT z!M%Ts7)4R)u}?=QCjgrPZ?KdV3(Xxw5O?9<8sLr2kW}r|T?_YYDMQy_&<*J=X7lr>DFzu*Xk2OMU4H5IeS-tu z@Z^N6=aC(U&z@{EnfN^%iW=o%U&g8pZS!O6Pa0uQ(9&_^wd`<40;LL((u#4|U|lcH zSTGSDPdX!ry9B~&X;@iX1L_*^LD}|bGt7<%T9o`*vLq&a_FLYE{Km+Su{xNT{6@Eu z>+kT@11tg&Ew7ld-Z}pFgvydzCJK$h;2Kx&r3N(L_Y24|w?JjrTCh^alxJ_2r5y|x z@x;@&;mL{jLiriE;aQK!KBCfOEc*k)keAp93f>iMJldPPrsH` z(0{1b`Tf~`)}>mYCDfSxAKaaUIRDy2zJANT>XjUo>DStEe{*J@8ad;j_HA@_QkHMT zsubujHpiYv@jtL0&^^?T`WDRb!t&wB`f#bSzZZC;@N&)?77CC;|LG3GR7@W9(e)*F8@Q#-ZS; z_C0Z>8Zm9N)re~8=~Z(*S8SjuE!Hn z)NPhFDk}8Xt2cV9iYfpnSdMbLGeXFn30fy%JU%ZCKg3F(U7 z*MKrI>Ho(wN&koWAV=fg8Et9E)W(c-*Tstj6XfH&iZ7ZW^OjW=%*IU5OU@#tmB*#p z%stHd19yZko`4S;N8?gOGPku`9mHx4KWXMp$NOT7Qi&>Td%_r`xv4qXY)KNcwiU7X zN5ukwAqRjWvQ=e$pwRa#p9x|1Y1fEcfUtOsU5owp&$1VDi+r7{jhY;z9Ma#nt+$Ye zG0LdnUTi=EfDX;p%lapJ35yN|c%SBA* zudeBX(EP8dz^`70#@UC7^)N}G41J}IIaoW?FF{=0!8E+$1r<6%mD85KpQUkyl_ z3-oV*?zaEcJM4B>B5g(+mO-eP+56GMg3VnA#!dhFY4+u?C5gP*g30e<90?IXw2nz^ z0XT6)uOA^MY+hRqbis-yd;P;8;p&Yu5rg!MZ^(e}^N+{kdO$@V&;a^JP}CCH%Hy&M zh;Mz=P_rx4qpgHsUfWyERf?fu<@hzCVRrb~s(-_;V$`>|ruSdXqYnVl>;y@b z@i3;aNrc<6aepES?0e7?pmdUY;4FmjvZn5XMhmuw(Z9a_YtfvFsJt16h_1A_jX!7F zOUK4M4v5e?0te9yMw8t?f^!tgA`>n1Dl@6!I`&`yFL@Z_O%HB*{JLEMI*uw)wtH+jIVi*-mVu++i|7-+WH#+AUO(w0} zc~?v7CZ6$f43+P|E#kl%o}%xp`tk{JF)2E-F}uO!qtZMKWy1OV0!>BJRvP$h!HPMx zr8T^s_w+xcb^;i3e!f)rOO5R*k*XsZE8 zerODPuuh_yQ?IQRquZ1;kEHljh|1(CbfDzUh>f?)wP4S5syJjvT*P-A%j~~$3!e!P z!H^v~H2*WiV$8vP_wBzj#_v)XJCNrN$yMbswYWS)M0|89U9xY{RS`gMsOwQ7MNiZR7=B*XxASAAVNlx3NktE(_g#UH-tv;V_P7 zK9r7?e25@+Ad_+<)e*pKK(+w3xeY%V{Gih~>O!sbn&DMqL8#e}iF~8$d!;kY?{ud? zd)lM)H3>-6G3r0dIb`9EVz^lX%Cj2i12{@}$K3FlyEP?5kU+JNZ$qy)4(IK-|DVqY zi|(Tt-$>D;)SV<311N!(Zs5;)TXSC?6!`KQBhSGIW&mkVA-_VNo4tB6auir;WGRYR zD5s!Uz6EFYuoieQ*d}g3Qr+I7ky1hxKi5_uwKe8mi9HU-{af^e41`4&QK9S}e;cCU z^OVuGWuNc|^OQ#x0)57#YT|-XHdnksa`J(N{Q zsn_~b1(j|_3ddVf0s9}ly*t_APUjpaZDfqtDyom(Ga}yx201XjB>6!;`$Fs!GEJ*R zl-~Je@SzP{>r0=s`yf5$5JnQujsar_dCtOMfI@*q^R+JGIi4-j_=#lq#}5;c)gs7V zTPDwx%PdYrdR+%qd;h0wTZck+uCo}%%-SnGIC_%eP1J#j0Dw$QoeXftmE-at;MRd( z{E(6nu>t0A?0WT1RHK+(OCA(BZdzrbTJ%Zyg7h(Bq^NGO%UJ4bVsN?fwHf<1Ju3a8 zL%!9v%^q<#7DXS~RV3;D0H0M15`^U`0KESsYAQz9vRi#flzf%`5ne`3;9;LVmgL%2 ziL1?j@(o~iNf!iODA>Pku=GSYxcB9@x6Ky-CAYN1nb;R z4N6E%{Uy<8z_sN=(HCrQ>~$2H7o4OD(b0X9S=`PhiJTKIL|*a=;9&bU#u+0-1hkK! ztHV88VqZ`y1q$N+rSM&oR(QxjMLM4>AB42uM1&HeCNrO~CQxoZh1%tja!H*P8W(>z zUEDNgUQlI!2UuRL)YZ&A60>EyvrMwRisjj>&*Gl^k zJ9n4(A#JcF(+CcQW=;kZ-+Vo8h$0*5LEXA1!q)WXCobH?t&Q;ozOtJMiQHmMUI%M2 z71Sv#jdzDJv2o6tBL=9Hg0W71E!h5>d|z@toLq(T)l&XsXiB;~1rnJQ;We|7N~x<4 zDoV#e7x|5edX)VDb&vnbQkR~7%5MMyLCPtFR>|e4U$E36qYPQ8_mm?C0R(6?x5q-+ z;avfso)l$;jwaU$Se9G+#f`S&FX6AkSm{klo8xcg zh`V`HSpb!h5B$s>I5AN#{n_TxhrYaT>)VK47Tn zbZne`yWf{Qhtu`IyfO(F8Tfj!mdJ0wU>u?Bgtw*<3nGrAkDf{bTW5vGpD% zA2_M6EOZ?1U|zBHx@2$K2&BsKwB|0F3g#r&!DM7~~hLBslqw(bsN zVN+&K!?xq9lGrvO86gP0BG+kJdb#+d-E427Yv-QKi;>V^8j$6EBK(F2Dvk8xRx?mt z**f!J)l1kls_6oD;R`hgVaH2Ln8O#YUsg0;z+Ov>=EZU7x>%xZJ56i@b*Za%V`>6 z+^;(-&N!b;gpGD+J>aegK~WM6Pryfg`j1IaTMY?|Kv^d4FBP&CTeb9tNpnN9cq|hj z2E}48osE2JRBWIos;dce$FESfhkOVNkohB9>%Ozm9aIND2jsVM#`_^qTJNhq+{!2< z9xgiE-Ue>JkE=_zL%cR;#q!QO7BhXno<&@nA%(wIoX*JI-BTLy1~M}YceJH>u^fmR znK+XcY$AwpKp+BFyhzN}@VddIVec>lc^{G&=|iRU)?r`f(==wiE!Shv>zy3E-u>E! z{!eK~`s#4)knmo)P#}4zl|7^H*K!!M=Jg9m{IJol1cVCD1g(wc3kpD{ynq?=R+ZOq z@@Z!IAY?c;&K9gCf4*RIq#wdg8riE3%W^=so+t>5(HZObXsZFNjluS9DRD3i)0lO~ z#Da?QSyl_+cg$*Xeu3RFRT!*4-T)u(z_K?-E}wgz_UxYw&|xRXp8%L}|AJ-4fg-}y zh{)zq%OlCnQx?OfwS32BwOZ1@{BZ-~;*(+Z2%;9aBAJFhAIt|+c2@PA=>w)wY| z)T@XV(NQ0zep(tQC;lwgr0#>JS}N;l2C5VEZp(JF9!yj3gZ&EDtX6qNoso6JOmLl| zc`VC)ih6dj!14Tvpp*1A3f5@)>0Bm2KDUGqZs8p>F}*eYGthfayKNBY?rz5o8>LXm zsF7pnEH+fwmJvfgiEnoPU9EL__Xw!8Iv`y>gD<3887&z83H9u>PleWXhbqTae8u$Q z0zogN3bV@zGOL1%*QF*!0a1`Jw+T8tK;G3Pd}q~QQeXv@8;fBKrnvmgq8r82Ow9Vx z3>saJ0|Gf%g+#{B-<{XCl1r%K2=3%+J9Dd+JtZID$CwG)d>$&DKl+AstQ{SXY)3rg z@7i3dNkyN#_oBr7hiZ`7P6w8y=Dt8abH0@+en=D!31i7`8qoQ6_J=>XKi-WW+~60l z5V+x5@OWH;r;eSUSx^Ysp(3_$+tCfT4VN44Z!=8VJMYT})9Z?Xrv9*U9e9<&vgY}h z`wix21()nwfzRSLxRh1E$ZcVD}DT zy4*rBL2;gEDi{#Yx^unC=+!9FHZd2Cw=Z)!gBkLuYmGLH6y!NCl;i7Cf;y8wEc$0< zVvtZEAn;kTlD9Dj_0Ls#>fNnALvNv-7s>OQq&d-b^kb;o*ve!5)REoURK(4Jc=oXn zG+{k7L5ep;%0BYui~$_X zbv8~H_)OB!0r8^_pGhL*?7q|s(2p-5eU24x<_Eq3Vrz2FBq1(jf_+Jg- z#?s+cd^G4hDvp68m3V^$O6a{*!_@s*twF+D+^O6(dWSI-*m9yZHTcJ&1*OB%-wy{^1x+NgdmSZ)+gjd7R*xJG1@gv8o+@1+sqEkeB>!ndF@^)G*GM(=H zF?=T0!f({=YsN={NU^SZS(Ph;g2k}0q}#=%=OD1iLVUxzIUTgR#Wjq2utv7y3nd7| zdha46-b5!nAYLYol){QF&SwEZbvCLew*0{H`lrq0#P7|_&Le7*_ZNip?QI~ChSz5> zeqGT^8V2NSm+8nB$g9t|w}$R{XYiuE0cuH2f~2|-K16>(qB+(S-@KR@o|MADWHY?y zPY+uhDUC(cLfl5?Q4%ss=&Tu3D#*wa(_;z7JVVxHBq zY~Sf8@ThL=cv}?5ehJ`8?!KK0-O;;S^FyY}v!-`r@(~sjfY^)<%(T))Xz4dgoe6wt z1+5&ry$rUrSZ=Ap5PQLS4q_Fx6W3q@ZIs-41rEvpn>aSPid8yuW_%lwEg#YbBljAL z7wb%cyyBGOTpR8HZw9ycnb8anA`DmOcQ0xLw2R?Op5}2FLzl;qf27R1kCgWH;yc1{ zSyz954*&_PRBy}9@bodKRYHH`%8i4*PwH!Ri7rcu>-qPfZix`b^4%+uhs{NF@o&o0 zfHP5U!?N9rr>T>qb66q z>*gCSv9b8ZXh23$t@q*^Q=tk;dEY$8V{V6RrKSVe08-&*;-WsmVABsO4uf7U!wzA<& zpeZ1^z_lXfSWRofpsw1BPkDdkn|`O$Oi4+h-1pKnXo(4bHJSdaaPIpu1GVhlCw0y= zQ+26fsGE=*G}1$#7nO zuL||RC*gJ%tVEDs)UAy**u_d1qD?p7SB9>kauWG0jG9W2Fk&xW zm6`H=eAUClw~=C4HZju+5~piGS*(_Zj`^;22mmc{2z(KB)|XAXK%t z*<&wGvDoCb+vy)IW^lJ}fJ|39(XO^)>u&izkKPY;V14V)>DwXcs~8*ZM({?Z(JV>$ zMzW*f7y8ih?7tVDo zdw(r~#xZDuc7>|c%TPQvbcl&m&kT!RBbu>%F$zviEG4{+% zX;`K4Dyd75>ss(Ao4;1!L6dmSxwtbs?a4~_$<@s!#L%lOwZ*W!XH~r4>L!|>1<8bV zCD?zv)Rf0%bpm(Yu43A~`Ukj3(~Pi4-^Ra}(MKoU){hmPGA-%m<2fY1?TQ`SxF4kp z{8vh2+qeMIp6D~?V`n2~K!Ag<@e;j6G6U6`>cIN8rx%~4^O&-LSz2e#uNk|ht&O|- z1&QBafodSKSho$?u@G)y$m=kCzJks8`bMd9_DM9khAmH(=We(uU6QoX>_%GH0==hT zn;Q2IyRi#pA#emrA@j0>kls=@hOD7mnp{vcmpuE%{bgvwm)owFKd;Tyy4;}EW`(QwOiBd(JwlE)qtm#i=z`&+dYI49s*EZ$*!e*AdCtvFw zM)2~ee@)4-k5-^_Ppitp5LG@(pD%iv;g~OM0jH8)9hsc>_v{YN_&n`L>JT-TAL?}9 z$i=qb+cM&-{7es83TT?E_oQyegxGlXyM@veaNC8e`=e5h7R<%A*Boi;=Komz2iWD_$$8B+UgoQqak$2GbKtv>an?MC_@vRQ0}UKk|_S*c78q|y z4oka>%YENTPZ>BCy{)Qj&1;l?|0g}_O^B-`b{~J5pScG(^}NjCeQE~nD?OBrL__z`Fx-=U zTS4NnnhR=uVn7zCk;h+F*1hwQyx%yC^mVdPj;7$kPx{;qeOlDro z{&MJasOr8QB}nuis(#Kfae@0TQrl<_8{@QNWj8BUdI6H;j@6;P6}8u!6b_nDZ#8q2 z7B9=myt9s@o)4gGvYmk68n;WhYr+-@N43EcQu)%V&kx0LvQVOId%mzCAVE>Hs|eOy z52}3|iduuBFe^Zah)hz8Yiwa!S2a|z$wPtbHn+&JRX44Imuiy_z83EzIgApBZi_tk>}8Yoq8Vy<)&k|P`%wmJiXB3}m#hv(u+Zys ze;ME0EUtQjRDEAjTp?Dmj90fPnpec=)xv9q^6ibWSNTc?QGYC*%u1hBdp#Y;(2(($ z@atc?Re}e#_x^XAJBiZQkixhhUI(wA_pB&1#)zVvTnC94ff|p%)xm?Hij5I1#Br0{ zs3eIF15i?a0X`wVa0TXJgE5YhY<-xiOIm-${N-o%lPsQ{<&ywo0p|)I5A+htm|Y#LR*Gcs zKms>6`;^#a%FJJ3SAQPV*``GMn{d{41C*#G+dI)DZ=GOY4sK$WMy<{?d zNd;D{V}1bWXpYP0*&DK-hUl+hU5xZYoZP;f-*@iQt?cs;12m<(IRfObOVz}ox@P=S z>56k+-ReM2y^9M#e>wVA!RFR8so}) zQoUJO4Y{(HP!6WW-JhRU|JKtJPlXBce6QL5{p9;Pox33G%X*u=FcoUO8D}S*ASkc% zn_I7x55L{43-oqAQum#%`Tf+lL6DQ_+a_(^h2J>a<|?5#9kalrpb}5<1n06&cixz6 zUo=1aV7;%jTT9t~uTiheGF_(f-3S#fooCiVq&=i$>lGkYId2{T>s15HHm34M7( k_=Hh4qWkZe7#@vyEoTJ3qJseoNJVnB)=H. - -""" - -import os -import rospy -import pygame -import sys -import argparse -import socket -import os - -from wurov.msg import surface_command, io_request, trajectory - -class joystick_sender: - def __init__(self): - rospy.init_node('joystick_sender_'+socket.gethostname(), anonymous=False) - - self.publisher = rospy.Publisher('surface_command', surface_command, queue_size=3) - - parser = argparse.ArgumentParser("Find a plugged in joystick and send it over /surface_command.") - parser.add_argument('--config_name', type=str, help='Set the name of the file we should use as a config (from ' - 'within the config directory)', required=True) - self.args = parser.parse_args(rospy.myargv()[1:]) - namespace = f"/{rospy.get_name()}/Controllers/{self.args.config_name}" - - self.controllerConfig = rospy.get_param(namespace) - - self.already_sent_zero = True # Set to true so that we aren't trying to set anything to zero on startup - self.last_sent = "" - self.servo_in_position = True - self.stepper_already_moving = True - self.thruster_already_killed = True - self.thruster_already_unkilled = True - self.joystick = None - - # We'll use this to try not to connect to the joystick too quickly. - rate = rospy.Rate(5) - - #Set pygame to headless - os.environ["SDL_VIDEODRIVER"] = "dummy" - pygame.display.init() - - try: #Try to init joystick - pygame.joystick.init() - while pygame.joystick.get_count() == 0: - rospy.logerr("No joystick connected!") - #pygame.joystick.quit() - pygame.joystick.init() - rate.sleep() - rospy.loginfo("Found a joystick to use.") - pygame.init() - self.joystick = pygame.joystick.Joystick(0) - self.joystick.init() - except pygame.error: - rospy.logerr("Failed to initialize joystick!") - sys.exit(1) - - # Now that we're not using the rate to slow down our joystick connection, let's bring it to something we'll use. - rospy.Timer(rospy.Duration(0.1), self.update) - rospy.spin() - - def update(self, data): - lastmsg = surface_command() - - pygame.event.get() - horizontal_axis = self.joystick.get_axis(self.controllerConfig["translationX_axis"]) # Horizontal: -1 is full left, 1 is full right - vertical_axis = self.joystick.get_axis(self.controllerConfig["translationY_axis"]) # Vertical: -1 is full forward, 1 is full back - twist_axis = self.joystick.get_axis(self.controllerConfig["translationZ_axis"]) # Twist: -1 is full counter-clockwise, 1 is clockwise - - if self.controllerConfig["depth_axis"]["inputCount"] == 1: - depth_axis = self.joystick.get_axis(self.controllerConfig["depth_axis"]["inputOne_Axis"]) # Lever: 1 is full down, -1 is full up - elif self.controllerConfig["depth_axis"]["inputCount"] == 2: - depthAxisOne = -1 * self.joystick.get_axis((self.controllerConfig["depth_axis"]["inputOne_Axis"])) - depthAxisTwo = self.joystick.get_axis((self.controllerConfig["depth_axis"]["inputTwo_Axis"])) - - if depthAxisOne > 0: - depthAxisOne = 0 - if depthAxisTwo < 0: - depthAxisTwo = 0 - - depth_axis = depthAxisOne + (depthAxisTwo) - else: - rospy.loginfo("Depth config set wrong") - quit() - - self.msg = surface_command() - self.msg.desired_trajectory.translation.x = -1 * horizontal_axis - self.msg.desired_trajectory.translation.y = vertical_axis - self.msg.desired_trajectory.translation.z = depth_axis # Flipped: forward is negative, that's dumb - self.msg.desired_trajectory.orientation.yaw = -1 * twist_axis - - msg = self.handle_peripherals(self.joystick, self.msg) - if self.different_msg(lastmsg, msg): - self.publisher.publish(msg) - lastmsg = msg - - def hat_to_val(self, a, b): - if a == 0: - if b == 0: - return None - if b == 1: - return "top_front" - if b == -1: - return "top_left" - if a == 1: - if b == 0: - return "top_right" - if b == 1: - return "front_right" - if b == -1: - return "front_left" - if a == -1: - if b == 0: - return "top_back" - if b == 1: - return "back_left" - if b == -1: - return "back_right" - - - def handle_peripherals(self, joystick_, msg_): - hat = joystick_.get_hat(0) - hat = self.hat_to_val(hat[0], hat[1]) - - io_request_ = io_request() - io_request_.executor = "individual_thruster_control" - - if hat is not None: - io_request_.float = 0.75 - io_request_.string = hat - else: - io_request_.float = 0.5 - io_request_.string = "all" - self.msg.io_requests += (io_request_,) - - return msg_ # If we wanted to do something with button presses, we could mess around with that sort of thing here. - - - def different_msg(self, msg1, msg2): - if msg1 is None or msg2 is None: - return True - - return msg1.desired_trajectory.orientation != msg2.desired_trajectory.orientation or \ - msg1.desired_trajectory.translation != msg2.desired_trajectory.translation or \ - msg1.io_requests != msg2.io_requests - - def hat_to_val(self, a, b): - if a == 0: - if b == 0: - return None - if b == 1: - return "top_front" - if b == -1: - return "top_back" - if a == 1: - if b == 0: - return "top_right" - if b == 1: - return "front_right" - if b == -1: - return "back_right" - if a == -1: - if b == 0: - return "top_left" - if b == 1: - return "front_left" - if b == -1: - return "back_left" - - - def handle_peripherals(self, joystick, msg): - hat = joystick.get_hat(0) - hat = self.hat_to_val(hat[0], hat[1]) - io_request_ = io_request() - - if hat is None: - if not self.already_sent_zero: - io_request_.executor = "individual_thruster_control" - io_request_.string = self.last_sent - self.already_sent_zero = True - msg.io_requests += (io_request_,) - else: - io_request_.executor = "individual_thruster_control" - io_request_.string = hat - io_request_.float = 0.75 - self.last_sent = hat - self.already_sent_zero = False - msg.io_requests += (io_request_,) - - if joystick.get_button(self.controllerConfig["safetyButton"]): # Safety trigger: Do not Send trajectory data if this trigger is held. - msg.desired_trajectory = trajectory() - - if not joystick.get_button(self.controllerConfig["boostMode"]): # 'Boost mode': If this button is pressed, multiply trajectory by 2 - # We implement this by always cutting by 2, and then when the button is pressed, not cutting in half. - msg.desired_trajectory.translation.x = msg.desired_trajectory.translation.x / 2 - msg.desired_trajectory.translation.y = msg.desired_trajectory.translation.y / 2 - msg.desired_trajectory.translation.z = msg.desired_trajectory.translation.z / 2 - msg.desired_trajectory.orientation.roll = msg.desired_trajectory.orientation.roll / 2 - msg.desired_trajectory.orientation.pitch = msg.desired_trajectory.orientation.pitch / 2 - msg.desired_trajectory.orientation.yaw = msg.desired_trajectory.orientation.yaw / 2 - - if joystick.get_button(self.controllerConfig["killThrusters"]): # Kill thrusters button - if not self.thruster_already_killed: - io_request_ = io_request() - io_request_.executor = "kill_thruster" - io_request_.string = "front_left" - msg.io_requests += (io_request_,) - - io_request_ = io_request() - io_request_.executor = "kill_thruster" - io_request_.string = "front_right" - msg.io_requests += (io_request_,) - - io_request_ = io_request() - io_request_.executor = "kill_thruster" - io_request_.string = "top_front" - msg.io_requests += (io_request_,) - - self.thruster_already_killed = True - elif self.thruster_already_killed: # Make sure the button is released before we send another stream of kill stuff - self.thruster_already_killed = False - - if joystick.get_button(self.controllerConfig["unkillThrusters"]): # Un-kill thrusters button - if not self.thruster_already_unkilled: - io_request_ = io_request() - io_request_.executor = "unkill_thruster" - io_request_.string = "front_left" - msg.io_requests += (io_request_,) - - io_request_ = io_request() - io_request_.executor = "unkill_thruster" - io_request_.string = "front_right" - msg.io_requests += (io_request_,) - - io_request_ = io_request() - io_request_.executor = "unkill_thruster" - io_request_.string = "top_front" - msg.io_requests += (io_request_,) - - self.thruster_already_unkilled = True - elif self.thruster_already_unkilled: # Make sure the button is released before we send another stream of un-kill stuff - self.thruster_already_unkilled = False - - return msg # If we wanted to do something with button presses, we could mess around with that sort of thing here. - - - -if __name__ == '__main__': - joystick_sender() From 05dcbc44e2a4c1812ad09515fbda4d6419aaedad Mon Sep 17 00:00:00 2001 From: Brennan Date: Fri, 6 May 2022 15:37:08 -0400 Subject: [PATCH 03/11] Repaired vehicle stats --- scripts/vehicle_stats.py | 2 +- surface/UI/debug_ui.py | 2 -- surface/UI/surface_ui.py | 2 -- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/scripts/vehicle_stats.py b/scripts/vehicle_stats.py index 0cef814..b571a10 100644 --- a/scripts/vehicle_stats.py +++ b/scripts/vehicle_stats.py @@ -10,7 +10,7 @@ class vehicle_stats: def __init__(self): rospy.init_node('vehicle_stats', anonymous=True) - self.nineDof_current_pub = rospy.Publisher( + self.publisher = rospy.Publisher( 'vehicle_stats', system_stats, queue_size=3) diff --git a/surface/UI/debug_ui.py b/surface/UI/debug_ui.py index 0048918..3085001 100755 --- a/surface/UI/debug_ui.py +++ b/surface/UI/debug_ui.py @@ -17,8 +17,6 @@ # Configurations fullscreenMode = True -cameraIndex1 = 2 -cameraIndex2 = 1 # Enter Screen Resolution for surface station(Only if fullscreenMode is disabled) height = 1080 width = 1920 diff --git a/surface/UI/surface_ui.py b/surface/UI/surface_ui.py index c4747f4..a1da4db 100755 --- a/surface/UI/surface_ui.py +++ b/surface/UI/surface_ui.py @@ -20,8 +20,6 @@ # Configurations fullscreenMode = True -cameraIndex1 = 2 -cameraIndex2 = 1 # Enter Screen Resolution for surface station(Only if fullscreenMode is disabled) height = 1080 width = 1920 From 2ee159c36c7d98f797985454e19563f3e5c2df2c Mon Sep 17 00:00:00 2001 From: Chris Thierauf Date: Fri, 6 May 2022 15:47:35 -0400 Subject: [PATCH 04/11] fix vehicle stats formatting --- scripts/vehicle_stats.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/scripts/vehicle_stats.py b/scripts/vehicle_stats.py index b571a10..2f95ad8 100644 --- a/scripts/vehicle_stats.py +++ b/scripts/vehicle_stats.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import rospy -from scripts.trajectory_sender_test import publisher from wurov.msg import system_stats import psutil from gpiozero import CPUTemperature @@ -10,22 +9,21 @@ class vehicle_stats: def __init__(self): rospy.init_node('vehicle_stats', anonymous=True) - self.publisher = rospy.Publisher( + self._publisher = rospy.Publisher( 'vehicle_stats', system_stats, queue_size=3) - - rospy.Timer(rospy.Duration(4, self.publisher) + rospy.Timer(rospy.Duration((1/4)), self.publisher) rospy.spin() - def publisher(self, data): + def publisher(self, event): + del event ret_msg = system_stats() ret_msg.cpu_usage = psutil.cpu_percent() - - cpu = CPUTemperature() - ret_msg.temp = cpu.temperature + ret_msg.temp = CPUTemperature().temperature self.publisher.publish(ret_msg) + if __name__ == '__main__': vehicle_stats() From b364819729ab2a0f443d780208964bef30b05712 Mon Sep 17 00:00:00 2001 From: Chris Thierauf Date: Fri, 6 May 2022 15:48:03 -0400 Subject: [PATCH 05/11] vehicle stats made executable for ruinning as node --- scripts/vehicle_stats.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 scripts/vehicle_stats.py diff --git a/scripts/vehicle_stats.py b/scripts/vehicle_stats.py old mode 100644 new mode 100755 From 2f64efc8ac0b1d60461ede2d0335630342293a9d Mon Sep 17 00:00:00 2001 From: Chris Thierauf Date: Fri, 6 May 2022 15:50:03 -0400 Subject: [PATCH 06/11] fix masked function --- scripts/vehicle_stats.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/vehicle_stats.py b/scripts/vehicle_stats.py index 2f95ad8..dd8caaa 100755 --- a/scripts/vehicle_stats.py +++ b/scripts/vehicle_stats.py @@ -23,7 +23,7 @@ def publisher(self, event): ret_msg.temp = CPUTemperature().temperature - self.publisher.publish(ret_msg) + self._publisher.publish(ret_msg) if __name__ == '__main__': vehicle_stats() From c4ac80912b3336490b8af77499bb5d3bbd3ae15a Mon Sep 17 00:00:00 2001 From: Brennan Date: Fri, 6 May 2022 15:52:58 -0400 Subject: [PATCH 07/11] Set default speed value to 0 --- surface/UI/debug_ui.py | 8 +++++--- surface/UI/surface_ui.py | 7 ++++--- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/surface/UI/debug_ui.py b/surface/UI/debug_ui.py index 3085001..059caaa 100755 --- a/surface/UI/debug_ui.py +++ b/surface/UI/debug_ui.py @@ -25,14 +25,14 @@ Body = QtGui.QFont("Roboto", 10) -class WUROV(QWidget): +class debug_ui(QWidget): def __init__(self): rospy.init_node('gui', anonymous=True) rospy.Subscriber('imu/data_raw', Imu, self.update_raw_imu) rospy.Subscriber('imu/data', Imu, self.update_filtered_imu) # Style settings for GUI - super(WUROV, self).__init__() + super(debug_ui, self).__init__() self.setGeometry(0, 0, width, height) self.setWindowTitle("WUROV Control") self.setStyleSheet('background-color: ' + @@ -132,6 +132,8 @@ def nineDof(self): self.speed = roundProgressBar() self.speed.rpb_setRange(0, 1.5) self.speed.rpb_setTextFormat('Value') + self.speed.rpb_setValue(0) + # Update functions change labels based on callback def update_raw_imu(self, data): @@ -167,7 +169,7 @@ def alert(self, s): import sys app = QApplication(sys.argv) - win = WUROV() + win = debug_ui() def window(): if fullscreenMode: diff --git a/surface/UI/surface_ui.py b/surface/UI/surface_ui.py index a1da4db..734f17f 100755 --- a/surface/UI/surface_ui.py +++ b/surface/UI/surface_ui.py @@ -28,7 +28,7 @@ Body = QtGui.QFont("Roboto", 10) -class WUROV(QWidget): +class surface_ui(QWidget): def __init__(self): rospy.init_node('gui', anonymous=True) rospy.Subscriber('camera', CameraInfo, self.update_camera) @@ -37,7 +37,7 @@ def __init__(self): self.path = rospack.get_path('wurov') # Style settings for GUI - super(WUROV, self).__init__() + super(surface_ui, self).__init__() self.setGeometry(0, 0, width, height) self.setWindowTitle("WUROV Control") self.setStyleSheet('background-color: ' + '#2C2F33' + ';color: ' + 'white') @@ -62,6 +62,7 @@ def widgets(self): self.speed = roundProgressBar() self.speed.rpb_setRange(0, 1.5) self.speed.rpb_setTextFormat('Value') + self.speed.rpb_setValue(0) def update_filtered_imu(self, data): acel_mag = math.sqrt(data.linear_acceleration.x**2 + data.linear_acceleration.y**2 + data.linear_acceleration.z**2) @@ -83,7 +84,7 @@ def alert(self, s): import sys app = QApplication(sys.argv) - win = WUROV() + win = surface_ui() def window(): From 396740c065867cff32dcdf83a13e288b152bb99d Mon Sep 17 00:00:00 2001 From: Hank Pham <46695607+HkPPP@users.noreply.github.com> Date: Wed, 8 Jun 2022 19:52:43 -0400 Subject: [PATCH 08/11] Dev/madgwick filter (#21) * Fixed mag orientation --- plugins/sensors/imu_data_fxas21002c_fxos8700.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/plugins/sensors/imu_data_fxas21002c_fxos8700.py b/plugins/sensors/imu_data_fxas21002c_fxos8700.py index 943618f..b9bf02b 100755 --- a/plugins/sensors/imu_data_fxas21002c_fxos8700.py +++ b/plugins/sensors/imu_data_fxas21002c_fxos8700.py @@ -42,15 +42,14 @@ def read_imu(self, data): ang_y, ang_z, ang_x = self.gyroSensor.gyroscope # in Radians/s # TODO: mag needs calibrations - # TODO: correct mag components? - mag_x, mag_y, mag_z = [k/1000000 for k in self.sensor.magnetometer] # in Tesla + mag_y, mag_z, mag_x = [k/1000000 for k in self.sensor.magnetometer] # in Tesla current_time = rospy.Time.now() # Imu msg self.imu_msg.header.stamp = current_time - self.imu_msg.header.frame_id = 'base_link' # Need to do URDF + self.imu_msg.header.frame_id = 'base_link' self.imu_msg.linear_acceleration.x = accel_x self.imu_msg.linear_acceleration.y = accel_y self.imu_msg.linear_acceleration.z = -accel_z @@ -60,7 +59,7 @@ def read_imu(self, data): # Mag msg self.mag_msg.header.stamp = current_time - self.mag_msg.header.frame_id = 'base_link' # Need to do URDF + self.mag_msg.header.frame_id = 'base_link' self.mag_msg.magnetic_field.x = mag_x self.mag_msg.magnetic_field.y = mag_y self.mag_msg.magnetic_field.z = mag_z From 83939becd63dcf934803caf773aa3e4860188561 Mon Sep 17 00:00:00 2001 From: Brennan <55165406+brennanmk@users.noreply.github.com> Date: Thu, 9 Jun 2022 17:41:37 -0400 Subject: [PATCH 09/11] Dev/sensors (#20) * Added bar30 and ping360 nodes * Added sonar code * updated launchfile for bar30 * Updated with dos2unix * added param for fluid den * Fixed temp/depth reversal Co-authored-by: Hank --- launch/sensor.launch | 8 +++ launch/sonar.launch | 9 +++ plugins/{sensors => camera}/camera0.py | 0 .../{sensors => camera}/camera_subscriber.py | 0 .../imu_data_fxas21002c_fxos8700.py | 0 plugins/{sensors => imu}/simulate_imu_data.py | 0 .../thruster}/thruster_sensor.py | 0 plugins/sensors/__init__.py | 0 plugins/sensors/bar30.py | 43 ++++++++++++++ plugins/sonar/ping360.py | 56 +++++++++++++++++++ requirements.txt | 3 + 11 files changed, 119 insertions(+) create mode 100644 launch/sensor.launch create mode 100644 launch/sonar.launch rename plugins/{sensors => camera}/camera0.py (100%) mode change 100755 => 100644 rename plugins/{sensors => camera}/camera_subscriber.py (100%) mode change 100755 => 100644 rename plugins/{sensors => imu}/imu_data_fxas21002c_fxos8700.py (100%) rename plugins/{sensors => imu}/simulate_imu_data.py (100%) rename plugins/{sensors => motors/thruster}/thruster_sensor.py (100%) delete mode 100644 plugins/sensors/__init__.py create mode 100755 plugins/sensors/bar30.py create mode 100755 plugins/sonar/ping360.py diff --git a/launch/sensor.launch b/launch/sensor.launch new file mode 100644 index 0000000..46d2160 --- /dev/null +++ b/launch/sensor.launch @@ -0,0 +1,8 @@ + + + + rate: 5 + fluidDensity: 997 + + + diff --git a/launch/sonar.launch b/launch/sonar.launch new file mode 100644 index 0000000..e37a7d7 --- /dev/null +++ b/launch/sonar.launch @@ -0,0 +1,9 @@ + + + + rate: 5 + interface: "/dev/ttyUSB0" + baudrate: 115200 + + + diff --git a/plugins/sensors/camera0.py b/plugins/camera/camera0.py old mode 100755 new mode 100644 similarity index 100% rename from plugins/sensors/camera0.py rename to plugins/camera/camera0.py diff --git a/plugins/sensors/camera_subscriber.py b/plugins/camera/camera_subscriber.py old mode 100755 new mode 100644 similarity index 100% rename from plugins/sensors/camera_subscriber.py rename to plugins/camera/camera_subscriber.py diff --git a/plugins/sensors/imu_data_fxas21002c_fxos8700.py b/plugins/imu/imu_data_fxas21002c_fxos8700.py similarity index 100% rename from plugins/sensors/imu_data_fxas21002c_fxos8700.py rename to plugins/imu/imu_data_fxas21002c_fxos8700.py diff --git a/plugins/sensors/simulate_imu_data.py b/plugins/imu/simulate_imu_data.py similarity index 100% rename from plugins/sensors/simulate_imu_data.py rename to plugins/imu/simulate_imu_data.py diff --git a/plugins/sensors/thruster_sensor.py b/plugins/motors/thruster/thruster_sensor.py similarity index 100% rename from plugins/sensors/thruster_sensor.py rename to plugins/motors/thruster/thruster_sensor.py diff --git a/plugins/sensors/__init__.py b/plugins/sensors/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/plugins/sensors/bar30.py b/plugins/sensors/bar30.py new file mode 100755 index 0000000..0d19161 --- /dev/null +++ b/plugins/sensors/bar30.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +import ms5837 +from std_msgs.msg import Float32 + + +class bar30: + def __init__(self): + """ + bar30 node is used to get temperature and depth readings from a bar30 sensor + duration is taken from the launch file param server + """ + + rospy.init_node('temperature', anonymous=True) + + self.depth_publisher = rospy.Publisher('vehicle/temperature', Float32, queue_size=3) + self.temp_publisher = rospy.Publisher('vehicle/depth', Float32, queue_size=3) + + publishDuration = rospy.get_param(f"/{rospy.get_name()}/rate") + + self.sensor = ms5837.MS5837_30BA() + self.sensor.init() + + try: + fluidDensity = rospy.get_param(f"/{rospy.get_name()}/fluidDensity") + self.sensor.setFluidDensity(fluidDensity) + except Exception as e: + rospy.loginfo("No fluid density specified, using default") + + rospy.Timer(rospy.Duration(publishDuration), self.publisher) + + rospy.spin() + + def publisher(self, data): + self.sensor.read() + + self.temp_publisher.publish(self.sensor.depth()) + self.depth_publisher.publish(self.sensor.temperature()) + +if __name__ == '__main__': + bar30() diff --git a/plugins/sonar/ping360.py b/plugins/sonar/ping360.py new file mode 100755 index 0000000..9f21f7c --- /dev/null +++ b/plugins/sonar/ping360.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +import rospy +import numpy as np +from brping import Ping360 +from sensor_msgs.msg import LaserScan +import math + +class ping360: + def __init__(self): + """ + ping360 node is used to get temperature and depth readings from a ping360 sensor + duration is taken from the launch file param server + """ + + rospy.init_node('temperature', anonymous=True) + + self._publisher = rospy.Publisher('vehicle/ping360', LaserScan, queue_size=3) + + publishDuration = rospy.get_param(f"/{rospy.get_name()}/rate") + interface = rospy.get_param(f"/{rospy.get_name()}/interface") + baudrate = rospy.get_param(f"/{rospy.get_name()}/baudrate") + + self.sensor = Ping360() + self.sensor.connect_serial(interface, baudrate) + self.sensor.initialize() + + rospy.Timer(rospy.Duration(publishDuration), self.publisher) + + rospy.spin() + + def publisher(self, data): + msg = LaserScan() + msg.angle_min = 0 + msg.angle_max = 6.28319 #360 degrees + msg.angle_increment = 0.314159 #20 gradians + + msg.ranges = [] + data = [] + for val in range(20): + scan = self.sensor.transmitAngle(val * 20) #angle in gradians + data = scan.data + msg.ranges.append(-1) + for detectedIntensity in data: + if detectedIntensity >= 200: + detectedIndex = data.index(detectedIntensity) + rangeVal = (1+detectedIndex) * 1481 * 25e-9 * 80/ 2 + if rangeVal > 0.75: + msg.ranges.pop() + msg.ranges.append(rangeVal) + break + + self._publisher.publish(msg) + +if __name__ == '__main__': + ping360() diff --git a/requirements.txt b/requirements.txt index 9c5c86b..3434b39 100755 --- a/requirements.txt +++ b/requirements.txt @@ -15,3 +15,6 @@ adafruit-circuitpython-servokit opencv-python numpy rospy +bluerobotics-tsys01 +python-smbus +bluerobotics-ping From a68f5ea2458d117ca83823604fdb09132bc31b2f Mon Sep 17 00:00:00 2001 From: Brennan <55165406+brennanmk@users.noreply.github.com> Date: Thu, 9 Jun 2022 17:44:06 -0400 Subject: [PATCH 10/11] Dev/pid (#19) * Added PID * Added dynamic reconfig * Repaired PID on vehicle * Updated duty cycle values in launch file for vehicle * added time delay in PID for init cycle * Updated setpoint functon * Rearranged launch files * Fixed pitch and yaw sources * Repaired PID * Updated msgs Co-authored-by: HkPPP Co-authored-by: Hank Pham <46695607+HkPPP@users.noreply.github.com> --- CMakeLists.txt | 3 - config/imu_filter_madgwick.yaml | 2 +- core/control/command_receiver.py | 11 +- core/control/control_aggregator.py | 10 +- core/control/socket_receiver.py | 25 ++-- core/control_loop/pid.py | 124 ++++++++++++++++++ .../vector_trajectory_converter.py | 18 +-- launch/core.launch | 12 -- launch/imu_filter_madgwick.launch | 3 - launch/pid.launch | 3 + launch/surface.launch | 6 + launch/vehicle.launch | 10 +- msg/magnetometer.msg | 5 - msg/multistep_trajectory.msg | 2 +- msg/orientation.msg | 6 - msg/pid_params.msg | 7 - msg/surface_command.msg | 2 +- msg/trajectory.msg | 5 - msg/translation.msg | 7 - plugins/motors/thruster/pca9685.py | 5 + requirements.txt | 2 + scripts/random_trajectory_requests.py | 12 +- scripts/trajectory_sender_test.py | 12 +- surface/chaos_joystick_sender.py | 13 +- surface/joystick_sender.py | 30 +++-- 25 files changed, 219 insertions(+), 116 deletions(-) create mode 100755 core/control_loop/pid.py delete mode 100644 launch/core.launch create mode 100644 launch/pid.launch delete mode 100644 msg/magnetometer.msg delete mode 100644 msg/orientation.msg delete mode 100644 msg/pid_params.msg delete mode 100644 msg/trajectory.msg delete mode 100644 msg/translation.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index ec9fd5e..c5d8d35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,12 +53,9 @@ add_message_files( arbitrary_pca_commands.msg io_request.msg joystick_chaos.msg - orientation.msg surface_command.msg thruster_sensor.msg thrustermove.msg - trajectory.msg - translation.msg ) ## Generate services in the 'srv' folder diff --git a/config/imu_filter_madgwick.yaml b/config/imu_filter_madgwick.yaml index 204f8e8..8354790 100644 --- a/config/imu_filter_madgwick.yaml +++ b/config/imu_filter_madgwick.yaml @@ -3,7 +3,7 @@ orientation_stddev: 0.001 gain: 0.1 zeta: 0.001 -publish_tf: false +publish_tf: true # REP103: For ENU type IMUs, the orientation of the world frame is x-east, y-north, z-up, relative to magnetic north. world_frame: enu diff --git a/core/control/command_receiver.py b/core/control/command_receiver.py index dd016bd..68f7831 100755 --- a/core/control/command_receiver.py +++ b/core/control/command_receiver.py @@ -20,17 +20,18 @@ """ import rospy -from wurov.msg import trajectory, io_request, surface_command +from wurov.msg import io_request, surface_command +from geometry_msgs.msg import Accel -trajectory_requester = rospy.Publisher('trajectory_request', trajectory, queue_size=3) + +trajectory_requester = rospy.Publisher('trajectory_request', Accel, queue_size=3) io_requester = rospy.Publisher('io_request', io_request, queue_size=3) -last_trajectory = trajectory() +last_trajectory = Accel() last_io_requests = surface_command().io_requests def trajectory_content_match(msg1, msg2): - return msg1.orientation == msg2.orientation and msg1.translation == msg2.translation - + return msg1.linear == msg2.linear and msg1.angular == msg2.angular def io_content_match(msg1, msg_list): for msg in msg_list: diff --git a/core/control/control_aggregator.py b/core/control/control_aggregator.py index 70d2fa3..9b2dcc9 100755 --- a/core/control/control_aggregator.py +++ b/core/control/control_aggregator.py @@ -31,16 +31,18 @@ def joystick_callback(data): pubmsg = trajectory() - pubmsg.translation = data.translation - pubmsg.orientation = data.orientation + pubmsg.header.stamp = rospy.Time.now() + pubmsg.linear = data.linear + pubmsg.angular = data.angular if ROVMODE: pub.publish(data) def trajectory_callback(data): pubmsg = trajectory() - pubmsg.translation = data.translation - pubmsg.orientation = data.orientation + pubmsg.header.stamp = rospy.Time.now() + pubmsg.linear = data.linear + pubmsg.angular = data.angular if AUVMODE: pub.publish(data) diff --git a/core/control/socket_receiver.py b/core/control/socket_receiver.py index 949dfec..c3c3682 100755 --- a/core/control/socket_receiver.py +++ b/core/control/socket_receiver.py @@ -43,26 +43,27 @@ def onMessage(self, payload, isBinary): global trajectory_publisher, mode_publisher sendtraj = trajectory() sendmode = mode() + sendtraj.header.stamp = rospy.Time.now() try: datadict = json.loads(payload.decode('utf-8')) - sendtraj.orientation.roll = float(datadict['r']) - sendtraj.orientation.pitch = float(datadict['p']) - sendtraj.orientation.yaw = float(datadict['c']) - sendtraj.translation.x = float(datadict['x']) - sendtraj.translation.y = float(datadict['y']) - sendtraj.translation.z = float(datadict['z']) + sendtraj.angular.x = float(datadict['r']) + sendtraj.angular.y = float(datadict['p']) + sendtraj.angular.z = float(datadict['c']) + sendtraj.linear.x = float(datadict['x']) + sendtraj.linear.y = float(datadict['y']) + sendtraj.linear.z = float(datadict['z']) trajectory_publisher.publish(sendtraj) self.sendMessage(b"Joystick executor message consumed!") except Exception as e: # If anything messes up, make sure the thrusters aren't spinning anymore - sendtraj.orientation.roll = 0 - sendtraj.orientation.pitch = 0 - sendtraj.orientation.yaw = 0 - sendtraj.translation.x = 0 - sendtraj.translation.y = 0 - sendtraj.translation.z = 0 + sendtraj.angular.x = 0 + sendtraj.angular.y = 0 + sendtraj.angular.z = 0 + sendtraj.linear.x = 0 + sendtraj.linear.y = 0 + sendtraj.linear.z = 0 sendmode.auvmode = True sendmode.rovmode = False trajectory_publisher.publish(sendtraj) diff --git a/core/control_loop/pid.py b/core/control_loop/pid.py new file mode 100755 index 0000000..49aa93e --- /dev/null +++ b/core/control_loop/pid.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +from simple_pid import PID +import rospy +from sensor_msgs.msg import Imu +from geometry_msgs.msg import Accel +import argparse +from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure +from std_msgs.msg import Bool + +class simulate_imu_data: + def __init__(self): + rospy.init_node('pid', anonymous=True) + + rospy.wait_for_message("/thruster_init", Bool) #Do not start until init verification is recieved + + parser = argparse.ArgumentParser("Dynamic Reconfig") + parser.add_argument('--dynamic_reconfig', type=bool, help='set to true if dynamic reconfig should be enabled') + self.args = parser.parse_args(rospy.myargv()[1:]) + + if self.args.dynamic_reconfig: + self.dynamic_rec() + + #set initial current values to 0 + self.currentPitch = 0 + self.currentYaw = 0 + self.currentAccel_x = 0 + self.currentAccel_y = 0 + self.currentAccel_z = 0 + + rospy.Subscriber('imu/data', Imu, self.updateCurrent) + rospy.Subscriber('trajectory_request', Accel, self.updateSetpoint) + + self._publisher = rospy.Publisher('trajectory_corrected', Accel, queue_size=3) + + rospy.Timer(rospy.Duration(0.1), self.pid) + + self.yawPID = PID(0.3, 0, 0) + self.pitchPID = PID(0.3, 0, 0) + self.xAcelPID = PID(0.3, 0, 0) + self.yAcelPID = PID(0.3, 0, 0) + self.zAcelPID = PID(0.3, 0, 0) + + #Set initial trajectory_request to 0 + self.yaw_value = 0 + self.pitch_value = 0 + self.xAcelPID_value = 0 + self.yAcelPID_value = 0 + self.zAcelPID_value = 0 + + rospy.spin() + + def updateCurrent(self, data): + self.currentPitch = data.angular_velocity.y + self.currentYaw = data.angular_velocity.z + self.currentAccel_x = data.linear_acceleration.x + self.currentAccel_y = data.linear_acceleration.y + self.currentAccel_z = data.linear_acceleration.z + + + def updateSetpoint(self, data): + self.pitchPID.setpoint = data.angular.y + self.yawPID.setpoint = data.angular.z + self.xAcelPID.setpoint = data.linear.x + self.yAcelPID.setpoint = data.linear.y + self.zAcelPID.setpoint = data.linear.z + + def pid(self, data): + #TODO: Replace with geometry_msg + msg = Accel() + + #add step + self.yaw_value += self.yawPID(self.currentYaw) + self.pitch_value += self.pitchPID(self.currentYaw) + self.xAcelPID_value += self.xAcelPID(self.currentYaw) + self.yAcelPID_value += self.yAcelPID(self.currentYaw) + self.zAcelPID_value += self.zAcelPID(self.currentYaw) + + msg.angular.y = self.yaw_value + msg.angular.z = self.pitch_value + msg.linear.x = self.xAcelPID_value + msg.linear.y = self.yAcelPID_value + msg.linear.z = self.zAcelPID_value + + self._publisher.publish(msg) + + def dynamic_rec(self): + self.ddynrec = DDynamicReconfigure("pid_rec") + + self.ddynrec.add_variable("yawP", "yaw proportion scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("yawI", "yaw integral scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("yawD", "yaw derrivative scale", 0.0, -1.0, 1.0) + + self.ddynrec.add_variable("rollP", "rollP proportion scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("rollI", "rollP integral scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("rollD", "rollP derrivative scale", 0.0, -1.0, 1.0) + + self.ddynrec.add_variable("xAcelP", "xAcelP proportion scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("xAcelI", "xAcelP integral scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("xAcelD", "xAcelP derrivative scale", 0.0, -1.0, 1.0) + + self.ddynrec.add_variable("yAcelP", "yAcelP proportion scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("yAcelI", "yAcelP integral scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("yAcelD", "yAcelP derrivative scale", 0.0, -1.0, 1.0) + + self.ddynrec.add_variable("zAcelP", "zAcelP proportion scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("zAcelI", "zAcelP integral scale", 0.0, -1.0, 1.0) + self.ddynrec.add_variable("zAcelD", "zAcelP derrivative scale", 0.0, -1.0, 1.0) + + self.ddynrec.start(self.dyn_rec_callback) + + def dyn_rec_callback(self, config, data): + + self.yawPID = PID(config["yawP"], config["yawI"], config["yawD"]) + self.pitchPID = PID(config["rollP"], config["rollI"], config["rollD"]) + self.xAcelPID = PID(config["xAcelP"], config["xAcelI"], config["xAcelD"]) + self.yAcelPID = PID(config["yAcelP"], config["yAcelI"], config["yAcelD"]) + self.zAcelPID = PID(config["zAcelP"], config["zAcelI"], config["zAcelD"]) + + return config + +if __name__ == '__main__': + simulate_imu_data() + diff --git a/core/trajectory_converter/vector_trajectory_converter.py b/core/trajectory_converter/vector_trajectory_converter.py index a4ec863..fa194bb 100755 --- a/core/trajectory_converter/vector_trajectory_converter.py +++ b/core/trajectory_converter/vector_trajectory_converter.py @@ -22,7 +22,9 @@ import rospy import argparse from math import floor -from wurov.msg import thrustermove, trajectory +from wurov.msg import thrustermove +from geometry_msgs.msg import Accel + ESC_IS_INIT = False Publisher = rospy.Publisher('thruster_commands', thrustermove, queue_size=3) @@ -205,12 +207,12 @@ def callback(data): movematrix = [[0, 0, 0, 0], [0, 0, 0, 0]] # Take the roll, pitch, etc, data and turn it into corresponding matrix values - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.orientation.roll, const_array_roll)) - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.orientation.pitch, const_array_pitch)) - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.orientation.yaw, const_array_cut)) - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.translation.x, const_array_x)) - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.translation.y, const_array_y)) - movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.translation.z, const_array_z)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.angular.x, const_array_roll)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.angular.y, const_array_pitch)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.angular.z, const_array_cut)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.linear.x, const_array_x)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.linear.y, const_array_y)) + movematrix = add_array_elements(movematrix, multiply_array_by_constant(data.linear.z, const_array_z)) # Now that the matrix exists, it needs to be corrected for ESC/thruster # weirdness, normalized, and converted to 0-1 range that the thrusters need. @@ -235,7 +237,7 @@ def listener(): rospy.init_node('trajectory_converter') # Run listener nodes, with the option of happening simultaneously. - rospy.Subscriber('trajectory_corrected', trajectory, callback) + rospy.Subscriber('trajectory_corrected', Accel, callback) # Run forever rospy.spin() diff --git a/launch/core.launch b/launch/core.launch deleted file mode 100644 index 0d54316..0000000 --- a/launch/core.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/imu_filter_madgwick.launch b/launch/imu_filter_madgwick.launch index 57d464f..4de25fd 100644 --- a/launch/imu_filter_madgwick.launch +++ b/launch/imu_filter_madgwick.launch @@ -1,8 +1,5 @@ - - - diff --git a/launch/pid.launch b/launch/pid.launch new file mode 100644 index 0000000..3fd7e6f --- /dev/null +++ b/launch/pid.launch @@ -0,0 +1,3 @@ + + + diff --git a/launch/surface.launch b/launch/surface.launch index f33d539..04fd6b5 100644 --- a/launch/surface.launch +++ b/launch/surface.launch @@ -1,5 +1,11 @@ + + + + + + diff --git a/launch/vehicle.launch b/launch/vehicle.launch index e811944..ca96613 100644 --- a/launch/vehicle.launch +++ b/launch/vehicle.launch @@ -1,8 +1,10 @@ - - + + - - + + + + diff --git a/msg/magnetometer.msg b/msg/magnetometer.msg deleted file mode 100644 index bbe8e51..0000000 --- a/msg/magnetometer.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header - -float32 x -float32 y -float32 z \ No newline at end of file diff --git a/msg/multistep_trajectory.msg b/msg/multistep_trajectory.msg index 1f4f7ba..bc08c56 100644 --- a/msg/multistep_trajectory.msg +++ b/msg/multistep_trajectory.msg @@ -2,4 +2,4 @@ Header header uint8 length -trajectory[] trajectories +geometry_msgs/Accel[] trajectories diff --git a/msg/orientation.msg b/msg/orientation.msg deleted file mode 100644 index 059be18..0000000 --- a/msg/orientation.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Vector to describe rotation -Header header - -float32 roll -float32 pitch -float32 yaw diff --git a/msg/pid_params.msg b/msg/pid_params.msg deleted file mode 100644 index 97fa72a..0000000 --- a/msg/pid_params.msg +++ /dev/null @@ -1,7 +0,0 @@ -# Allow for easy tuning of the control loop by passing a message -# instead of needing to shut everything down each time. -Header header - -float32 P -float32 I -float32 D diff --git a/msg/surface_command.msg b/msg/surface_command.msg index 85969eb..9c0a2e9 100644 --- a/msg/surface_command.msg +++ b/msg/surface_command.msg @@ -1,5 +1,5 @@ # Message for sending a command down from the surface. Header header -trajectory desired_trajectory # The orientation and translation we'd like to be at in this moment +geometry_msgs/Accel desired_trajectory # The orientation and translation we'd like to be at in this moment io_request[] io_requests # Any additional io requests to take place here diff --git a/msg/trajectory.msg b/msg/trajectory.msg deleted file mode 100644 index b5ed1ef..0000000 --- a/msg/trajectory.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Message for generic trajectory, from which orientation and translation data can be obtained. -Header header - -orientation orientation -translation translation diff --git a/msg/translation.msg b/msg/translation.msg deleted file mode 100644 index 086b3a6..0000000 --- a/msg/translation.msg +++ /dev/null @@ -1,7 +0,0 @@ -# Translation described as a normalized vector (values range from -1 to 1) -# x is side-to-side, y is front-to-back, z is up and down -Header header - -float32 x -float32 y -float32 z diff --git a/plugins/motors/thruster/pca9685.py b/plugins/motors/thruster/pca9685.py index b91bf9e..54c75c7 100755 --- a/plugins/motors/thruster/pca9685.py +++ b/plugins/motors/thruster/pca9685.py @@ -26,6 +26,8 @@ from threading import Thread from board import SCL, SDA import busio +from std_msgs.msg import Bool + # The thruster_dictionary takes sensible thruster names and turns them into PCA channels. # We default to -1 as a flag value. thruster_dictionary = { @@ -130,6 +132,9 @@ def init_thrusters(init_sequence): rospy.logerr("Error cause: " + str(e)) rospy.loginfo("Initialized thrusters!") + initPub = rospy.Publisher("thruster_init", Bool, queue_size=10, latch=True) + initPub.publish(True) #Send message that thrusters are init + def move_callback(data): diff --git a/requirements.txt b/requirements.txt index 3434b39..045129d 100755 --- a/requirements.txt +++ b/requirements.txt @@ -15,6 +15,8 @@ adafruit-circuitpython-servokit opencv-python numpy rospy +simple-pid bluerobotics-tsys01 python-smbus bluerobotics-ping + diff --git a/scripts/random_trajectory_requests.py b/scripts/random_trajectory_requests.py index 08082c7..2975e3b 100755 --- a/scripts/random_trajectory_requests.py +++ b/scripts/random_trajectory_requests.py @@ -32,13 +32,13 @@ surface_command_msg = surface_command() rate = rospy.Rate(2) while not rospy.is_shutdown(): - surface_command_msg.desired_trajectory.translation.x = np.random.normal() - surface_command_msg.desired_trajectory.translation.y = np.random.normal() - surface_command_msg.desired_trajectory.translation.z = np.random.normal() + surface_command_msg.desired_trajectory.linear.x = np.random.normal() + surface_command_msg.desired_trajectory.linear.y = np.random.normal() + surface_command_msg.desired_trajectory.linear.z = np.random.normal() - surface_command_msg.desired_trajectory.orientation.roll = np.random.normal() - surface_command_msg.desired_trajectory.orientation.pitch = np.random.normal() - surface_command_msg.desired_trajectory.orientation.yaw = np.random.normal() + surface_command_msg.desired_trajectory.angular.x = np.random.normal() + surface_command_msg.desired_trajectory.angular.y = np.random.normal() + surface_command_msg.desired_trajectory.angular.z = np.random.normal() command_publisher.publish(surface_command_msg) diff --git a/scripts/trajectory_sender_test.py b/scripts/trajectory_sender_test.py index 3a70b05..39917b8 100755 --- a/scripts/trajectory_sender_test.py +++ b/scripts/trajectory_sender_test.py @@ -11,12 +11,12 @@ def publisher(): while not rospy.is_shutdown(): msg = trajectory() - msg.orientation.roll = .1 - msg.orientation.pitch = -.2 - msg.orientation.yaw = .3 - msg.translation.x = -.4 - msg.translation.y = .5 - msg.translation.z = -.6 + msg.angular.x = .1 + msg.angular.y = -.2 + msg.angular.z = .3 + msg.linear.x = -.4 + msg.linear.y = .5 + msg.linear.z = -.6 pub.publish(msg) rate.sleep() diff --git a/surface/chaos_joystick_sender.py b/surface/chaos_joystick_sender.py index 3e5bb88..f3767da 100755 --- a/surface/chaos_joystick_sender.py +++ b/surface/chaos_joystick_sender.py @@ -89,8 +89,8 @@ def different_msg(msg1, msg2): if msg1 is None or msg2 is None: return True - return msg1.desired_trajectory.orientation != msg2.desired_trajectory.orientation or \ - msg1.desired_trajectory.translation != msg2.desired_trajectory.translation or \ + return msg1.desired_trajectory.angular != msg2.desired_trajectory.angular or \ + msg1.desired_trajectory.linear != msg2.desired_trajectory.linear or \ msg1.io_requests != msg2.io_requests @@ -146,10 +146,11 @@ def different_msg(msg1, msg2): lever_axis = joystick.get_axis(3) + shift_lever_correction # Lever: 1 is full down, -1 is full up msg = surface_command() - msg.desired_trajectory.translation.x = -1 * horizontal_axis * magnitude_horizontal_correction - msg.desired_trajectory.translation.y = vertical_axis * magnitude_vertical_correction - msg.desired_trajectory.translation.z = -1 * lever_axis * magnitude_lever_correction - msg.desired_trajectory.orientation.yaw = -1 * twist_axis * magnitude_twist_correction + msg.header.stamp = rospy.Time.now() + msg.desired_trajectory.linear.x = -1 * horizontal_axis * magnitude_horizontal_correction + msg.desired_trajectory.linear.y = vertical_axis * magnitude_vertical_correction + msg.desired_trajectory.linear.z = -1 * lever_axis * magnitude_lever_correction + msg.desired_trajectory.angular.z = -1 * twist_axis * magnitude_twist_correction msg = config.simulate_peripherals.handle_peripherals(joystick, msg) if different_msg(lastmsg, msg): diff --git a/surface/joystick_sender.py b/surface/joystick_sender.py index e4c3889..de6fae8 100755 --- a/surface/joystick_sender.py +++ b/surface/joystick_sender.py @@ -27,7 +27,8 @@ import socket import os -from wurov.msg import surface_command, io_request, trajectory +from wurov.msg import surface_command, io_request +from geometry_msgs.msg import Accel class joystick_sender: def __init__(self): @@ -102,10 +103,11 @@ def update(self, data): quit() self.msg = surface_command() - self.msg.desired_trajectory.translation.x = -1 * horizontal_axis - self.msg.desired_trajectory.translation.y = vertical_axis - self.msg.desired_trajectory.translation.z = depth_axis # Flipped: forward is negative, that's dumb - self.msg.desired_trajectory.orientation.yaw = -1 * twist_axis + self.msg .header.stamp = rospy.Time.now() + self.msg.desired_trajectory.linear.x = -1 * horizontal_axis + self.msg.desired_trajectory.linear.y = vertical_axis + self.msg.desired_trajectory.linear.z = depth_axis # Flipped: forward is negative, that's dumb + self.msg.desired_trajectory.angular.z = -1 * twist_axis msg = self.handle_peripherals(self.joystick, self.msg) if self.different_msg(lastmsg, msg): @@ -158,8 +160,8 @@ def different_msg(self, msg1, msg2): if msg1 is None or msg2 is None: return True - return msg1.desired_trajectory.orientation != msg2.desired_trajectory.orientation or \ - msg1.desired_trajectory.translation != msg2.desired_trajectory.translation or \ + return msg1.desired_trajectory.angular != msg2.desired_trajectory.angular or \ + msg1.desired_trajectory.linear != msg2.desired_trajectory.linear or \ msg1.io_requests != msg2.io_requests def hat_to_val(self, a, b): @@ -206,16 +208,16 @@ def handle_peripherals(self, joystick, msg): msg.io_requests += (io_request_,) if joystick.get_button(self.controllerConfig["safetyButton"]): # Safety trigger: Do not Send trajectory data if this trigger is held. - msg.desired_trajectory = trajectory() + msg.desired_trajectory = Accel() if not joystick.get_button(self.controllerConfig["boostMode"]): # 'Boost mode': If this button is pressed, multiply trajectory by 2 # We implement this by always cutting by 2, and then when the button is pressed, not cutting in half. - msg.desired_trajectory.translation.x = msg.desired_trajectory.translation.x / 2 - msg.desired_trajectory.translation.y = msg.desired_trajectory.translation.y / 2 - msg.desired_trajectory.translation.z = msg.desired_trajectory.translation.z / 2 - msg.desired_trajectory.orientation.roll = msg.desired_trajectory.orientation.roll / 2 - msg.desired_trajectory.orientation.pitch = msg.desired_trajectory.orientation.pitch / 2 - msg.desired_trajectory.orientation.yaw = msg.desired_trajectory.orientation.yaw / 2 + msg.desired_trajectory.linear.x = msg.desired_trajectory.linear.x / 2 + msg.desired_trajectory.linear.y = msg.desired_trajectory.linear.y / 2 + msg.desired_trajectory.linear.z = msg.desired_trajectory.linear.z / 2 + msg.desired_trajectory.angular.x = msg.desired_trajectory.angular.x / 2 + msg.desired_trajectory.angular.y = msg.desired_trajectory.angular.y / 2 + msg.desired_trajectory.angular.z = msg.desired_trajectory.angular.z / 2 if joystick.get_button(self.controllerConfig["killThrusters"]): # Kill thrusters button if not self.thruster_already_killed: From 609cb8499bff87acbbbda9736c024fd8d090b4e0 Mon Sep 17 00:00:00 2001 From: Hank Pham <46695607+HkPPP@users.noreply.github.com> Date: Fri, 10 Jun 2022 10:18:40 -0400 Subject: [PATCH 11/11] Dev/imu calibration (#23) * Added accel offset --- launch/vehicle.launch | 2 +- plugins/imu/imu_data_fxas21002c_fxos8700.py | 91 +++++++++++++++++---- 2 files changed, 76 insertions(+), 17 deletions(-) diff --git a/launch/vehicle.launch b/launch/vehicle.launch index ca96613..e654de2 100644 --- a/launch/vehicle.launch +++ b/launch/vehicle.launch @@ -1,5 +1,5 @@ - + diff --git a/plugins/imu/imu_data_fxas21002c_fxos8700.py b/plugins/imu/imu_data_fxas21002c_fxos8700.py index b9bf02b..36bdbe6 100755 --- a/plugins/imu/imu_data_fxas21002c_fxos8700.py +++ b/plugins/imu/imu_data_fxas21002c_fxos8700.py @@ -1,22 +1,59 @@ #!/usr/bin/env python3 +import time +import argparse import board; import busio; import adafruit_fxas21002c; import adafruit_fxos8700; import rospy from sensor_msgs.msg import Imu, MagneticField +from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure + class imu_data: def __init__(self): + parser = argparse.ArgumentParser("Dynamic Reconfig") + parser.add_argument('--accel_calibration', type=bool, help='set to true if dynamic reconfig should be enabled') + self.args = parser.parse_args(rospy.myargv()[1:]) + # init hardwares i2c = busio.I2C(board.SCL, board.SDA) self.gyroSensor = adafruit_fxas21002c.FXAS21002C(i2c) self.sensor = adafruit_fxos8700.FXOS8700(i2c) + # imu_filter_madgwick input topics + rospy.init_node('imu_raw_data', anonymous=True) + self.imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=3) + self.mag_pub = rospy.Publisher('imu/mag', MagneticField, queue_size=3) + self.imu_no_offset_pub = rospy.Publisher('imu/data_no_offset', Imu, queue_size=3) + + # init messages self.imu_msg = Imu() self.mag_msg = MagneticField() + self.no_offset_msg = Imu() + + # init offset values + self.linear_accel_offset = { + 'x': 0.25, + 'y': -0.03, + 'z': -0.47 + } + self.angular_vel_offset = { + 'x': 0, + 'y': 0, + 'z': 0 + } + self.magnetic_field_offset = { + 'x': 0, + 'y': 0, + 'z': 0 + } + + # Calculate accel offset + # if self.args.accel_calibration: + # self.calculate_accel_offset() # zeros matrix for unknow covariance according to sensor_msgs/Imu doc zeros_mat = [0]*9 @@ -24,11 +61,6 @@ def __init__(self): self.imu_msg.angular_velocity_covariance = zeros_mat self.imu_msg.linear_acceleration_covariance = zeros_mat - # imu_filter_madgwick input topics - rospy.init_node('imu_raw_data', anonymous=True) - self.imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=3) - self.mag_pub = rospy.Publisher('imu/mag', MagneticField, queue_size=3) - rospy.Timer(rospy.Duration(0.1), self.read_imu) rospy.spin() @@ -50,24 +82,51 @@ def read_imu(self, data): # Imu msg self.imu_msg.header.stamp = current_time self.imu_msg.header.frame_id = 'base_link' - self.imu_msg.linear_acceleration.x = accel_x - self.imu_msg.linear_acceleration.y = accel_y - self.imu_msg.linear_acceleration.z = -accel_z - self.imu_msg.angular_velocity.x = ang_x - self.imu_msg.angular_velocity.y = ang_y - self.imu_msg.angular_velocity.z = ang_z + self.imu_msg.linear_acceleration.x = accel_x - self.linear_accel_offset['x'] + self.imu_msg.linear_acceleration.y = accel_y - self.linear_accel_offset['y'] + self.imu_msg.linear_acceleration.z = -abs(accel_z - self.linear_accel_offset['z']) # negative abs is to ensure it's always -9.8 m/s initally + self.imu_msg.angular_velocity.x = ang_x - self.angular_vel_offset['x'] + self.imu_msg.angular_velocity.y = ang_y - self.angular_vel_offset['y'] + self.imu_msg.angular_velocity.z = ang_z - self.angular_vel_offset['z'] + + # Imu no offset msg + self.no_offset_msg.header.stamp = current_time + self.no_offset_msg.header.frame_id = 'base_link' + self.no_offset_msg.linear_acceleration.x = accel_x + self.no_offset_msg.linear_acceleration.y = accel_y + self.no_offset_msg.linear_acceleration.z = -accel_z + self.no_offset_msg.angular_velocity.x = ang_x + self.no_offset_msg.angular_velocity.y = ang_y + self.no_offset_msg.angular_velocity.z = ang_z # Mag msg self.mag_msg.header.stamp = current_time self.mag_msg.header.frame_id = 'base_link' - self.mag_msg.magnetic_field.x = mag_x - self.mag_msg.magnetic_field.y = mag_y - self.mag_msg.magnetic_field.z = mag_z + self.mag_msg.magnetic_field.x = mag_x - self.magnetic_field_offset['x'] + self.mag_msg.magnetic_field.y = mag_y - self.magnetic_field_offset['y'] + self.mag_msg.magnetic_field.z = mag_z - self.magnetic_field_offset['z'] # publish msgs self.imu_pub.publish(self.imu_msg) self.mag_pub.publish(self.mag_msg) - - + self.imu_no_offset_pub.publish(self.no_offset_msg) + + def calculate_accel_offset(self, duration=2, sampling_rate=10): + duration = duration + time.time() + period = 1/sampling_rate + x = [] + y = [] + z = [] + while time.time() < duration: + accel_y, accel_z, accel_x = self.sensor.accelerometer # in m/s^2 + x.append(accel_x) + y.append(accel_y) + z.append(accel_z) + time.sleep(period) + self.linear_accel_offset['x'] = sum(x)/len(x) + self.linear_accel_offset['y'] = sum(y)/len(y) + self.linear_accel_offset['z'] = sum(z)/len(z) - 9.8 + + if __name__ == '__main__': imu_data() \ No newline at end of file