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/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/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..e654de2 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/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/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/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/imu/imu_data_fxas21002c_fxos8700.py b/plugins/imu/imu_data_fxas21002c_fxos8700.py
new file mode 100755
index 0000000..406ef74
--- /dev/null
+++ b/plugins/imu/imu_data_fxas21002c_fxos8700.py
@@ -0,0 +1,132 @@
+#!/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
+ self.imu_msg.orientation_covariance = zeros_mat
+ self.imu_msg.angular_velocity_covariance = zeros_mat
+ self.imu_msg.linear_acceleration_covariance = zeros_mat
+
+ rospy.Timer(rospy.Duration(0.1), self.read_imu)
+
+ rospy.spin()
+
+ def read_imu(self, data):
+ # REP103:
+ # +x: forward
+ # +y: left
+ # +z: up
+ accel_y, accel_z, accel_x = self.sensor.accelerometer # in m/s^2
+ ang_y, ang_z, ang_x = self.gyroSensor.gyroscope # in Radians/s
+
+ # TODO: mag needs calibrations
+ 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'
+ 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.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
diff --git a/plugins/imu/nxp-by-file.py b/plugins/imu/nxp-by-file.py
new file mode 100755
index 0000000..1484288
--- /dev/null
+++ b/plugins/imu/nxp-by-file.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+"""
+
+ 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
+from auv.msg import ninedof, gyroscope
+# from auv.msg import gyroscope
+
+import csv
+import subprocess
+import os
+
+nineDof_current_pub = rospy.Publisher('ninedof_current', ninedof, queue_size=3)
+nineDof_integrated_pub = rospy.Publisher('ninedof_integrated', gyroscope, queue_size=3)
+nineDof_gyro_pub = rospy.Publisher('ninedof_gyroscope', gyroscope, queue_size=3)
+
+
+def listener():
+ rospy.init_node('nxp-by-file_nineDof', anonymous=True)
+ rate = rospy.Rate(5)
+ proc = subprocess.Popen('rosrun auv read_nxpval.py', shell=True)
+
+ sendval_ninedof = ninedof()
+ sendval_integrated = gyroscope()
+ sendval_gyro = gyroscope()
+ current_directory = os.path.dirname(__file__)
+ csv_filename = os.path.join(current_directory, '../scripts/output/nxpval.csv')
+ while not rospy.is_shutdown():
+
+ with open(csv_filename, "r") as f:
+ reader = csv.DictReader(f)
+
+ for row in reader:
+ sendval_ninedof.orientation.roll = row['roll']
+ sendval_ninedof.orientation.pitch = row['pitch']
+ sendval_ninedof.orientation.yaw = row['yaw']
+ sendval_ninedof.translation.x = row['accl_x']
+ sendval_ninedof.translation.y = row['accl_y']
+ sendval_ninedof.translation.z = row['accl_z']
+
+ sendval_integrated.x = row['pos_x']
+ sendval_integrated.y = row['pos_y']
+ sendval_integrated.z = row['pos_z']
+
+ sendval_gyro.x = row['gyro_x']
+ sendval_gyro.y = row['gyro_y']
+ sendval_gyro.z = row['gyro_z']
+
+ nineDof_current_pub.publish(sendval_ninedof)
+ nineDof_integrated_pub.publish(sendval_integrated)
+ nineDof_gyro_pub.publish(sendval_gyro)
+
+ rate.sleep()
+
+ proc.kill()
+
+
+if __name__ == '__main__':
+ listener()
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/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/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/sensors/imu_data_fxas21002c_fxos8700.py b/plugins/sensors/imu_data_fxas21002c_fxos8700.py
deleted file mode 100755
index 943618f..0000000
--- a/plugins/sensors/imu_data_fxas21002c_fxos8700.py
+++ /dev/null
@@ -1,74 +0,0 @@
-#!/usr/bin/env python3
-
-import board;
-import busio;
-import adafruit_fxas21002c;
-import adafruit_fxos8700;
-import rospy
-from sensor_msgs.msg import Imu, MagneticField
-
-
-class imu_data:
- def __init__(self):
- # init hardwares
- i2c = busio.I2C(board.SCL, board.SDA)
- self.gyroSensor = adafruit_fxas21002c.FXAS21002C(i2c)
- self.sensor = adafruit_fxos8700.FXOS8700(i2c)
-
- self.imu_msg = Imu()
- self.mag_msg = MagneticField()
-
- # zeros matrix for unknow covariance according to sensor_msgs/Imu doc
- zeros_mat = [0]*9
- self.imu_msg.orientation_covariance = zeros_mat
- 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()
-
- def read_imu(self, data):
- # REP103:
- # +x: forward
- # +y: left
- # +z: up
- accel_y, accel_z, accel_x = self.sensor.accelerometer # in m/s^2
- 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
-
-
- 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.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
-
- # Mag msg
- self.mag_msg.header.stamp = current_time
- self.mag_msg.header.frame_id = 'base_link' # Need to do URDF
- self.mag_msg.magnetic_field.x = mag_x
- self.mag_msg.magnetic_field.y = mag_y
- self.mag_msg.magnetic_field.z = mag_z
-
- # publish msgs
- self.imu_pub.publish(self.imu_msg)
- self.mag_pub.publish(self.mag_msg)
-
-
-if __name__ == '__main__':
- imu_data()
\ No newline at end of file
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..045129d 100755
--- a/requirements.txt
+++ b/requirements.txt
@@ -15,3 +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/scripts/vehicle_stats.py b/scripts/vehicle_stats.py
new file mode 100755
index 0000000..dd8caaa
--- /dev/null
+++ b/scripts/vehicle_stats.py
@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+
+import rospy
+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._publisher = rospy.Publisher(
+ 'vehicle_stats', system_stats, queue_size=3)
+
+ rospy.Timer(rospy.Duration((1/4)), self.publisher)
+
+ rospy.spin()
+
+ def publisher(self, event):
+ del event
+ ret_msg = system_stats()
+ ret_msg.cpu_usage = psutil.cpu_percent()
+
+ ret_msg.temp = CPUTemperature().temperature
+
+ self._publisher.publish(ret_msg)
+
+if __name__ == '__main__':
+ vehicle_stats()
diff --git a/surface/UI/debug_ui.py b/surface/UI/debug_ui.py
new file mode 100755
index 0000000..059caaa
--- /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
+# 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 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(debug_ui, 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')
+ self.speed.rpb_setValue(0)
+
+
+ # 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 = debug_ui()
+
+ 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 0000000..48d8e7d
Binary files /dev/null and b/surface/UI/default.jpg differ
diff --git a/surface/UI/surface_ui.py b/surface/UI/surface_ui.py
new file mode 100755
index 0000000..734f17f
--- /dev/null
+++ b/surface/UI/surface_ui.py
@@ -0,0 +1,98 @@
+#!/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, CameraInfo
+
+import rospkg
+
+
+# Configurations
+fullscreenMode = True
+# 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 surface_ui(QWidget):
+ def __init__(self):
+ rospy.init_node('gui', anonymous=True)
+ rospy.Subscriber('camera', CameraInfo, self.update_camera)
+ rospy.Subscriber('imu/data', Imu, self.update_filtered_imu)
+ rospack = rospkg.RosPack()
+ self.path = rospack.get_path('wurov')
+
+ # Style settings for GUI
+ super(surface_ui, self).__init__()
+ self.setGeometry(0, 0, width, height)
+ self.setWindowTitle("WUROV Control")
+ self.setStyleSheet('background-color: ' + '#2C2F33' + ';color: ' + 'white')
+
+ # Grid layout for GUI
+ self.widgets()
+ grid = QtWidgets.QGridLayout()
+
+ grid.addWidget(self.camera, 0,0)
+
+ grid.addWidget(self.speed, 1, 0)
+ self.setLayout(grid)
+
+
+ # NineDof labels to display sensor read outs
+ def widgets(self):
+ #Camera widget
+ self.camera = QtWidgets.QLabel(self)
+ pixmap = QPixmap(f'{self.path}/surface/UI/default.jpg')
+ self.camera.setPixmap(pixmap)
+ #Speedometer widget
+ 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)
+ self.speed.rpb_setValue(acel_mag)
+
+ def update_camera(self, data): #update camera based on ROS message
+ qimage = QtGui.QImage.fromData(data.data)
+ self.camera.setPixmap(QtGui.QPixmap.fromImage(qimage))
+
+ 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 = surface_ui()
+
+
+ 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 90%
rename from surface/chaos_joystick_sender.py
rename to surface/joystick/chaos_joystick_sender.py
index 3e5bb88..9448cb7 100755
--- a/surface/chaos_joystick_sender.py
+++ b/surface/joystick/chaos_joystick_sender.py
@@ -34,6 +34,7 @@
import sys
import argparse
import socket
+import time
from auv.msg import surface_command, io_request, joystick_chaos
@@ -89,8 +90,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 +147,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/joystick_sender.py
similarity index 88%
rename from surface/joystick_sender.py
rename to surface/joystick/joystick_sender.py
index e4c3889..de6fae8 100755
--- a/surface/joystick_sender.py
+++ b/surface/joystick/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: