Dev/gui

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

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

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:

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)
 

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)

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() 
+

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()

launch/imu_filter_madgwick.launch

@@ -1,8 +1,5 @@
 <launch>
     <arg name="simulate" default="false" doc="'simulate:=true' will publish randomized data to /imu/data_raw and /imu/mag"/>
-    <group if="$(eval arg('simulate') == false)">
-        <node pkg="wurov" type="imu_data_fxas21002c_fxos8700.py" name="imu_data_raw"/>
-    </group>
     <group if="$(eval arg('simulate') == true)">
         <node pkg="wurov" type="simulate_imu_data.py" name="imu_data_raw"/>
     </group>

launch/pid.launch

@@ -0,0 +1,3 @@
+<launch>
+    <node name="pid" pkg="wurov" type="pid.py" args="--dynamic_reconfig True"/>
+</launch>

launch/sensor.launch

@@ -0,0 +1,8 @@
+<launch>
+    <node name="bar30" pkg="wurov" type="bar30.py">
+        <rosparam>
+            rate: 5
+            fluidDensity: 997
+        </rosparam>
+    </node>
+</launch>

launch/sonar.launch

@@ -0,0 +1,9 @@
+<launch>
+    <node name="ping360" pkg="wurov" type="ping360.py">
+        <rosparam>
+            rate: 5
+            interface: "/dev/ttyUSB0"
+            baudrate: 115200
+        </rosparam>
+    </node>
+</launch>

launch/surface.launch

@@ -1,5 +1,11 @@
 <launch>
+    <!-- Start collecting IMU data and filter it -->
     <node name="joystick_sender" pkg="wurov" type="joystick_sender.py" args="--config_name Xbox">
         <rosparam file="$(find wurov)/config/controller.yaml" />
     </node>
+
+    <node name="control_loop" pkg="wurov" type="pid.py" args="--dynamic_reconfig True"/>
+
+    <include file="$(find wurov)/launch/imu_filter_madgwick.launch" />
+
 </launch>

launch/vehicle.launch

@@ -1,8 +1,10 @@
 <launch>
-    <node name="pca9685" pkg="wurov" type="pca9685.py" args="1850 3050 --frequency 390 --top_front 5 --top_back 6 --front_right 1 --back_right 2 --back_left 3 --front_left 4" />
-    <node name="imu/data_raw" pkg="wurov" type="imu_data.py" />
+    <node pkg="wurov" type="imu_data_fxas21002c_fxos8700.py" name="imu_data_raw" args="--accel_calibration False"/>
+    <node name="pca9685" pkg="wurov" type="pca9685.py" args="46650 27000 --frequency 390 --top_front 5 --top_back 6 --front_right 1 --back_right 2 --back_left 3 --front_left 4" />
     <node name="thruster_tester" pkg="wurov" type="individual_thruster_control_pca.py" args="" />
 
-    <!-- It's usually easier to tack this on last (to keep error message line numbers accurate) -->
-    <include file="$(find wurov)/launch/core.launch" />
+    <node name="command_receiver" pkg="wurov" type="command_receiver.py" args="" />
+    <node name="thruster_converter" pkg="wurov" type="vector_trajectory_converter.py" args="" />
+
+
 </launch>

msg/multistep_trajectory.msg

@@ -2,4 +2,4 @@
 Header header 
 
 uint8 length
-trajectory[] trajectories
+geometry_msgs/Accel[] trajectories

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

msg/system_stats.msg

@@ -0,0 +1,2 @@
+float32 cpu_usage
+float32 temp