diff --git a/gnc/sub8_thruster_mapper/CMakeLists.txt b/gnc/sub8_thruster_mapper/CMakeLists.txt
index 32b8469..1142b21 100644
--- a/gnc/sub8_thruster_mapper/CMakeLists.txt
+++ b/gnc/sub8_thruster_mapper/CMakeLists.txt
@@ -7,4 +7,11 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
-)
\ No newline at end of file
+)
+
+if(CATKIN_ENABLE_TESTING)
+ foreach(T
+ test/test_sub8_mapper.test)
+ add_rostest(${T})
+ endforeach()
+endif()
diff --git a/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch b/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch
new file mode 100644
index 0000000..2499135
--- /dev/null
+++ b/gnc/sub8_thruster_mapper/launch/thruster_mapper.launch
@@ -0,0 +1,62 @@
+
+
+ busses:
+ - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0
+ thrusters:
+ FLV: {
+ node_id: 10,
+ frame_id: /base_link,
+ position: [0.1583, 0.169, 0.0142],
+ direction: [0, 0, -1]
+ }
+ FLL: {
+ node_id: 11,
+ frame_id: /base_link,
+ position: [0.2678, 0.2795, 0],
+ direction: [-0.866, 0.5, 0]
+ }
+ - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMCI-if00-port0
+ thrusters:
+ FRV: {
+ node_id: 12,
+ frame_id: /base_link,
+ position: [0.1583, -0.169, 0.0142],
+ direction: [0, 0, -1]
+ }
+ FRL: {
+ node_id: 13,
+ frame_id: /base_link,
+ position: [0.2678, -0.2795, 0],
+ direction: [-0.866, -0.5, 0]
+ }
+ - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IN02-if00-port0
+ thrusters:
+ BLV: {
+ node_id: 14,
+ frame_id: /base_link,
+ position: [-0.1583, 0.169, 0.0142],
+ direction: [0, 0, 1]
+ }
+ BLL: {
+ node_id: 15,
+ frame_id: /base_link,
+ position: [-0.2678, 0.2795, 0],
+ direction: [0.866, 0.5, 0]
+ }
+ - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IN03-if00-port0
+ thrusters:
+ BRV: {
+ node_id: 16,
+ frame_id: /base_link,
+ position: [-0.1583, -0.169, 0.0142],
+ direction: [0, 0, 1]
+ }
+ BRL: {
+ node_id: 17,
+ frame_id: /base_link,
+ position: [-0.2678, -0.2795, 0],
+ direction: [0.866, -0.5, 0]
+ }
+
+
+
\ No newline at end of file
diff --git a/gnc/sub8_thruster_mapper/nodes/mapper.py b/gnc/sub8_thruster_mapper/nodes/mapper.py
new file mode 100755
index 0000000..372d0c0
--- /dev/null
+++ b/gnc/sub8_thruster_mapper/nodes/mapper.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+import rospy
+from scipy.optimize import minimize
+import numpy as np
+from sub8_ros_tools import wait_for_param, thread_lock, rosmsg_to_numpy
+import threading
+from sub8_msgs.msg import Thrust, ThrusterCmd
+from geometry_msgs.msg import WrenchStamped
+
+
+lock = threading.Lock()
+class ThrusterMapper(object):
+ def __init__(self, layout):
+ '''The layout should be a dictionary of the form used in thruster_mapper.launch
+ an excerpt is quoted here for posterity
+
+ busses:
+ - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0
+ thrusters:
+ FLV: {
+ node_id: 10,
+ frame_id: /base_link,
+ position: [0.1583, 0.169, 0.0142],
+ direction: [0, 0, -1]
+ }
+ FLL: {
+ node_id: 11,
+ frame_id: /base_link,
+ position: [0.2678, 0.2795, 0],
+ direction: [-0.866, 0.5, 0]
+ }
+
+ - port: ....
+ thrusters: ....
+
+ '''
+ self.min_thrust, self.max_thrust = self.get_ranges()
+ # Make thruster layout
+ self.B = self.generate_B(layout)
+ self.wrench_sub = rospy.Subscriber('/wrench', WrenchStamped, self.request_wrench_cb, queue_size=1)
+ self.thruster_pub = rospy.Publisher('/thrust', Thrust, queue_size=1)
+
+ @thread_lock(lock)
+ def update_layout(self, srv):
+ '''Update the physical thruster layout.
+ This should only be done in a thruster-out event
+ '''
+ raise(NotImplementedError("Layout Updates Not Implemented"))
+ self.layout = '...'
+ self.B = self.generate_B(self.layout)
+
+ def get_ranges(self):
+ '''Get upper and lower thrust limits for each thruster
+ Todo: Actually do something
+ --> Wait for a service in the thruster driver to say something
+ - We have this information in the form of calibration.json
+ '''
+ rospy.logwarn("Thruster mapper not querying thrusters for range data")
+ return np.array([-90] * 8), np.array([90] * 8)
+
+ def get_thruster_wrench(self, position, direction):
+ '''Compute a single column of B, or the wrench created by a particular thruster'''
+ forces = direction
+ torques = np.cross(position, forces)
+ wrench_column = np.hstack([forces, torques])
+ return np.transpose(wrench_column)
+
+ def generate_B(self, layout):
+ '''Construct the control-input matrix
+ Each column represents the wrench generated by a single thruster
+
+ The single letter "B" is conventionally used to refer to a matrix which converts
+ a vector of control inputs to a wrench
+
+ Meaning where u = [thrust_1, ... thrust_n],
+ B * u = [Fx, Fy, Fz, Tx, Ty, Tz]
+ '''
+ # Maintain an ordered list, tracking which column corresponds to which thruster
+ self.thruster_name_map = []
+ B = []
+ for port in layout:
+ for thruster_name, thruster_info in port['thrusters'].items():
+ # Assemble the B matrix by columns
+ self.thruster_name_map.append(thruster_name)
+ wrench_column = self.get_thruster_wrench(
+ thruster_info['position'],
+ thruster_info['direction']
+ )
+ B.append(wrench_column)
+
+ return np.transpose(np.array(B))
+
+ @thread_lock(lock)
+ def map(self, wrench):
+ '''TODO:
+ - Account for variable thrusters
+ '''
+ thrust_cost = np.diag([1.0] * 8)
+
+ def objective(u):
+ '''Compute a cost resembling analytical least squares
+ Minimize
+ norm((B * u) - wrench) + (u.T * Q * u)
+ Subject to
+ min_u < u < max_u
+ Where
+ Q defines the cost of firing the thrusters
+ u is a vector where u[n] is the thrust output by thruster_n
+
+ i.e., least squares with bounds
+ '''
+ error_cost = np.linalg.norm(self.B.dot(u) - wrench) ** 2
+ effort_cost = np.transpose(u).dot(thrust_cost).dot(u)
+ return error_cost + effort_cost
+
+ def obj_jacobian(u):
+ '''Compute the jacobian of the objective function
+
+ [1] Scalar-By-Matrix derivative identities [Online]
+ Available: https://en.wikipedia.org/wiki/Matrix_calculus#Scalar-by-vector_identities
+ '''
+ error_jacobian = 2 * self.B.T.dot(self.B.dot(u) - wrench)
+ effort_jacobian = np.transpose(u).dot(2 * thrust_cost)
+ return error_jacobian + effort_jacobian
+
+ minimization = minimize(
+ method='slsqp',
+ fun=objective,
+ jac=obj_jacobian,
+ x0=(self.min_thrust + self.max_thrust) / 2,
+ bounds=zip(self.min_thrust, self.max_thrust),
+ tol=0.1
+ )
+ return minimization.x, minimization.success
+
+ def request_wrench_cb(self, msg):
+ '''Callback for requesting a wrench'''
+ force = rosmsg_to_numpy(msg.wrench.force)
+ torque = rosmsg_to_numpy(msg.wrench.torque)
+ wrench = np.hstack([force, torque])
+
+ u, success = self.map(wrench)
+ if success:
+ thrust_cmds = []
+ # Assemble the list of thrust commands to send
+ for name, thrust in zip(self.thruster_name_map, u):
+ # > Can speed this up by avoiding appends
+ thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
+ self.thruster_pub.publish(thrust_cmds)
+ else:
+ # I expect this to happen frequently enough that we should not raise
+ rospy.logwarn("Could not achieve wrench of {}".format(wrench))
+
+
+if __name__ == '__main__':
+ '''
+ --> Deal with ranges
+ --> Deal with getting B
+ --> Figure out how to communicate with the thrusters
+ --> Do these things in TF
+ - Decided not to do this to avoid needless traffic for a lot of static transforms
+ '''
+ rospy.init_node('thruster_mapper')
+ busses = wait_for_param('busses')
+ mapper = ThrusterMapper(busses)
+ rospy.spin()
\ No newline at end of file
diff --git a/gnc/sub8_thruster_mapper/package.xml b/gnc/sub8_thruster_mapper/package.xml
index f483a1d..9203586 100644
--- a/gnc/sub8_thruster_mapper/package.xml
+++ b/gnc/sub8_thruster_mapper/package.xml
@@ -5,7 +5,7 @@
The sub8_thruster_mapper package
Jacob Panikulam
- TODO
+ MIT
catkin
rospy
rospy
diff --git a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py
new file mode 100755
index 0000000..cdbc175
--- /dev/null
+++ b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.py
@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+
+PKG = 'sub8_thruster_mapper'
+NAME = 'test_map'
+
+import sys
+import unittest
+import numpy as np
+from sub8_msgs.msg import Thrust, ThrusterCmd
+from geometry_msgs.msg import WrenchStamped, Wrench, Vector3
+import rospy
+import rostest
+import time
+
+
+class TestMapThrusters(unittest.TestCase):
+ def setUp(self, *args):
+ '''TODO:
+ - Assert that wrenches within bounds are close to the unbounded least-squares estimate
+ - Make waiting for node functionality into a test_helpers util
+ - Test random wrenches and assert valid behavior
+ '''
+ self.got_msg = False
+ self.test_data = []
+
+ def thrust_callback(self, msg):
+ self.got_msg = True
+ self.test_data.append(msg)
+
+ def test_map_good(self):
+ '''Test desired wrenches that are known to be achievable
+ '''
+ # wait at most 5 seconds for the thruster_mapper to be registered
+ timeout_t = time.time() + 5.0
+ while not (rostest.is_subscriber(
+ rospy.resolve_name('/wrench'),
+ rospy.resolve_name('thruster_mapper')) and time.time() < timeout_t):
+ time.sleep(0.1)
+
+ self.assert_(
+ rostest.is_subscriber(
+ rospy.resolve_name('/wrench'),
+ rospy.resolve_name('thruster_mapper')
+ ),
+ "{} is not up".format('thruster_mapper')
+ )
+
+ thrust_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1, latch=True)
+ wrenches = [
+ np.zeros(6),
+ np.arange(6) * 5,
+ np.arange(6)[::-1] * 5,
+ np.arange(6) * -5,
+ np.arange(6)[::-1] * -5,
+ ]
+ rospy.Subscriber("/thrust", Thrust, self.thrust_callback)
+
+ for num, wrench in enumerate(wrenches):
+ wrench_msg = WrenchStamped(
+ wrench=Wrench(
+ force=Vector3(*wrench[:3]),
+ torque=Vector3(*wrench[3:])
+ )
+ )
+
+ thrust_pub.publish(wrench_msg)
+ timeout_t = time.time() + 0.2
+ while not rospy.is_shutdown() and time.time() < timeout_t and not self.got_msg:
+ time.sleep(0.01)
+
+ self.assertEqual(len(self.test_data) - 1, num, msg="Could not compute wrench for " + str(wrench) + " within timeout")
+ self.got_msg = False
+
+
+if __name__ == '__main__':
+ rospy.init_node(NAME, anonymous=True)
+ rostest.rosrun(PKG, NAME, TestMapThrusters, sys.argv)
\ No newline at end of file
diff --git a/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test
new file mode 100644
index 0000000..d6f1deb
--- /dev/null
+++ b/gnc/sub8_thruster_mapper/test/test_sub8_mapper.test
@@ -0,0 +1,5 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/utils/sub8_msgs/CMakeLists.txt b/utils/sub8_msgs/CMakeLists.txt
index 199fec7..41ebf26 100644
--- a/utils/sub8_msgs/CMakeLists.txt
+++ b/utils/sub8_msgs/CMakeLists.txt
@@ -9,26 +9,26 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
)
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
+add_message_files(
+ FILES
+ Alarm.msg
+ Thrust.msg
+ ThrusterCmd.msg
+)
+add_service_files(
+ FILES
+ ThrusterInfo.srv
+)
+
# add_action_files(
# FILES
# Action1.action
-# Action2.action
-# )
-# generate_messages(
-# DEPENDENCIES
-# geometry_msgs
-# std_msgs
# )
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+ std_msgs
+)
catkin_package(
LIBRARIES sub8_msgs
diff --git a/utils/sub8_msgs/msg/Thrust.msg b/utils/sub8_msgs/msg/Thrust.msg
new file mode 100644
index 0000000..65248fb
--- /dev/null
+++ b/utils/sub8_msgs/msg/Thrust.msg
@@ -0,0 +1,3 @@
+# Control thrust
+
+sub8_msgs/ThrusterCmd[] thruster_commands
\ No newline at end of file
diff --git a/utils/sub8_msgs/msg/ThrusterCmd.msg b/utils/sub8_msgs/msg/ThrusterCmd.msg
new file mode 100644
index 0000000..21373d1
--- /dev/null
+++ b/utils/sub8_msgs/msg/ThrusterCmd.msg
@@ -0,0 +1,4 @@
+# Thruster Command
+
+string name
+float32 thrust
\ No newline at end of file
diff --git a/utils/sub8_msgs/srv/ThrusterInfo.srv b/utils/sub8_msgs/srv/ThrusterInfo.srv
new file mode 100644
index 0000000..57fd39a
--- /dev/null
+++ b/utils/sub8_msgs/srv/ThrusterInfo.srv
@@ -0,0 +1,4 @@
+uint8 thruster_id
+---
+float32 min_force
+float32 max_force
\ No newline at end of file
diff --git a/utils/sub8_ros_tools/sub8_ros_tools/math_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/math_helpers.py
index aad5951..1c7e4a5 100644
--- a/utils/sub8_ros_tools/sub8_ros_tools/math_helpers.py
+++ b/utils/sub8_ros_tools/sub8_ros_tools/math_helpers.py
@@ -41,7 +41,6 @@ def rosmsg_to_numpy(rosmsg, keys=None):
else:
output_array = np.zeros(len(keys), np.float32)
for n, key in enumerate(keys):
- print key
output_array[n] = getattr(rosmsg, key)
return output_array
\ No newline at end of file
diff --git a/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py b/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py
index ef852f6..5d51e76 100644
--- a/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py
+++ b/utils/sub8_ros_tools/sub8_ros_tools/threading_helpers.py
@@ -27,4 +27,4 @@ def locked_function(*args, **kwargs):
# Return the function with locking added
return locked_function
- return thread_lock
\ No newline at end of file
+ return lock_thread
diff --git a/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py b/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py
index 6aee5a7..1c4cc65 100644
--- a/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py
+++ b/utils/sub8_ros_tools/test_ros_tools/test_ros_tools.py
@@ -5,12 +5,13 @@
from sensor_msgs.msg import Image
from sub8_ros_tools import make_image_msg, get_image_msg
from sub8_ros_tools import rosmsg_to_numpy
+from sub8_ros_tools import thread_lock
class TestROSTools(unittest.TestCase):
def test_rosmsg_to_numpy_quaternion(self):
'''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
- # I know this is not unit quaternion
+ # I know this is not a unit quaternion
q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
numpy_array = rosmsg_to_numpy(q)
@@ -21,7 +22,7 @@ def test_rosmsg_to_numpy_quaternion(self):
def test_rosmsg_to_numpy_vector(self):
'''Test a rosmsg conversion for a geometry_msgs/Vector'''
- v = Vector3(0.1, 99., 21.)
+ v = Vector3(x=0.1, y=99., z=21.)
numpy_array = rosmsg_to_numpy(v)
np.testing.assert_allclose(
@@ -53,6 +54,31 @@ def test_get_image_msg(self):
cv_im = get_image_msg(im_msg)
np.testing.assert_array_equal(im, cv_im)
+ def test_thread_lock(self):
+ ran = False
+
+ class FakeLock(object):
+ def __init__(self):
+ self.entry = False
+ self.exit = False
+
+ def __enter__(self):
+ self.entry = True
+
+ def __exit__(self, *args):
+ self.exit = True
+
+ fake_lock = FakeLock()
+ @thread_lock(fake_lock)
+ def lock_test(a):
+ return (fake_lock.entry is True) and (fake_lock.exit is False)
+
+ result = lock_test(1)
+
+ self.assertTrue(fake_lock.entry, msg='Thread was never locked')
+ self.assertTrue(fake_lock.exit, msg='Thread was never released')
+ self.assertTrue(result, msg='Thread was not locked while the function was executed')
+
if __name__ == '__main__':
unittest.main()
\ No newline at end of file