THRUSTERS: adapt thruster driver to unified config

* make thruster driver load from port layout and thruster definition params
* make thruster spin test utility load from new params

command/sub8_launch/launch/subsystems/thrusters.launch

@@ -1,4 +1,5 @@
 <launch>
+        <rosparam file="$(find sub8_thruster_mapper)/config/thruster_layout.yaml" param="thruster_layout"/>
         <include file="$(find sub8_thruster_mapper)/launch/thruster_mapper.launch"/>	
         <include file="$(find sub8_videoray_m5_thruster)/launch/thruster_driver.launch"/>
 </launch>

drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py

@@ -18,21 +18,21 @@
 class ThrusterDriver(object):
     _dropped_timeout = 1 # s
 
-    def __init__(self, config_path, bus_layout):
+    def __init__(self, config_path, ports, thruster_definitions):
         '''Thruster driver, an object for commanding all of the sub's thrusters
             - Gather configuration data and make it available to other nodes
             - Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
             - Track a thrust_dict, which maps thruster names to the appropriate port
             - Given a command message, route that command to the appropriate port/thruster
             - Send a thruster status message describing the status of the particular thruster
         '''
+        self.thruster_heartbeats = {}
+        self.failed_thrusters = []
+
         self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
         AlarmListener("thruster-out", self.check_alarm_status, call_when_raised=False)
         self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
 
-        self.thruster_heartbeats = {}
-        self.failed_thrusters = []
-
         self.make_fake = rospy.get_param('simulate', False)
         if self.make_fake:
             rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'")
@@ -45,7 +45,7 @@ def __init__(self, config_path, bus_layout):
         self.status_pub = rospy.Publisher('thrusters/thruster_status', ThrusterStatus, queue_size=8)
 
         # Bus configuration
-        self.port_dict = self.load_bus_layout(bus_layout)
+        self.port_dict = self.load_thruster_layout(ports, thruster_definitions)
         self.bus_voltage = None
         self.last_bus_voltage_time = rospy.Time.now()
         self.bus_voltage_pub = rospy.Publisher('bus_voltage', Float64, queue_size=1)
@@ -117,7 +117,8 @@ def load_config(self, path):
         return newtons, thruster_input
 
     def get_thruster_info(self, srv):
-        '''Get the thruster info for a particular thruster ID
+        '''
+        Get the thruster info for a particular thruster ID
         Right now, this is only the min and max thrust data
         '''
         # Unused right now
@@ -132,19 +133,19 @@ def get_thruster_info(self, srv):
         return thruster_info
 
     @thread_lock(lock)
-    def load_bus_layout(self, layout):
+    def load_thruster_layout(self, ports, thruster_definitions):
         '''Load and handle the thruster bus layout'''
         port_dict = {}
 
         # These alarms require this service to be available before things will work
         rospy.wait_for_service("update_thruster_layout")
         self.thruster_out_alarm.clear_alarm(parameters={'clear_all': True})
 
-        for port in layout:
-            thruster_port = thruster_comm_factory(port, fake=self.make_fake)
+        for port_info in ports:
+            thruster_port = thruster_comm_factory(port_info, thruster_definitions, fake=self.make_fake)
 
             # Add the thrusters to the thruster dict
-            for thruster_name, thruster_info in port['thrusters'].items():
+            for thruster_name in port_info['thruster_names']:
                 if thruster_name in thruster_port.missing_thrusters:
                     rospy.logerr("{} IS MISSING!".format(thruster_name))
                     self.alert_thruster_loss(thruster_name, "Motor ID was not found on it's port.")
@@ -298,11 +299,12 @@ def fail_thruster(self, srv):
 
     rospy.init_node('videoray_m5_thruster_driver')
 
-    layout_parameter = '/busses'
+    layout_parameter = '/thruster_layout'
     rospy.loginfo("Thruster Driver waiting for parameter, {}".format(layout_parameter))
-    busses = wait_for_param(layout_parameter)
-    if busses is None:
+    thruster_layout = wait_for_param(layout_parameter)
+    if thruster_layout is None:
         raise(rospy.exceptions.ROSException("Failed to find parameter '{}'".format(layout_parameter)))
 
-    thruster_driver = ThrusterDriver(config_path, busses)
+    thruster_driver = ThrusterDriver(config_path, thruster_layout['thruster_ports'],
+                                     thruster_layout['thrusters'])
     rospy.spin()

drivers/sub8_videoray_m5_thruster/nodes/thruster_spin_test.py

@@ -15,8 +15,8 @@ def ports_from_layout(layout):
             '''Load and handle the thruster bus layout'''
             port_dict = {}
 
-            for port in layout:
-                thruster_port = thruster_comm_factory(port, fake=True)
+            for port in layout['thruster_ports']:
+                thruster_port = thruster_comm_factory(port, layout['thrusters'], fake=True)
 
                 # Add the thrusters to the thruster dict
                 for thruster_name, thruster_info in port['thrusters'].items():
@@ -27,8 +27,8 @@ def ports_from_layout(layout):
     rospy.init_node('thruster_spin_test')
 
     sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper')
-    busses = rosparam.load_file(sub8_thruster_mapper + '/config/busses.yaml')[0][0]
-    thruster_ports = ports_from_layout(busses)
+    thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0]
+    thruster_ports = ports_from_layout(thruster_layout)
     print len(thruster_ports)
     print thruster_ports
     names_from_motor_id = {0: 'FLH', 1: 'FLV', 2: 'FRH', 3: 'FRV',

drivers/sub8_videoray_m5_thruster/package.xml

@@ -10,4 +10,4 @@
   <run_depend>rospy</run_depend>
   <export>
   </export>
-</package>
+</package>