diff --git a/command/sub8_launch/launch/subsystems/thrusters.launch b/command/sub8_launch/launch/subsystems/thrusters.launch
index 16665dbd..91dd3a11 100644
--- a/command/sub8_launch/launch/subsystems/thrusters.launch
+++ b/command/sub8_launch/launch/subsystems/thrusters.launch
@@ -1,4 +1,5 @@
+
diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py
index d0ca7de3..b361d0ca 100755
--- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py
+++ b/drivers/sub8_videoray_m5_thruster/nodes/thruster_driver.py
@@ -18,7 +18,7 @@
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
@@ -26,13 +26,13 @@ def __init__(self, config_path, bus_layout):
- 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,7 +133,7 @@ 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 = {}
@@ -140,11 +141,11 @@ def load_bus_layout(self, layout):
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()
diff --git a/drivers/sub8_videoray_m5_thruster/nodes/thruster_spin_test.py b/drivers/sub8_videoray_m5_thruster/nodes/thruster_spin_test.py
index c739dc93..b942156b 100755
--- a/drivers/sub8_videoray_m5_thruster/nodes/thruster_spin_test.py
+++ b/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',
diff --git a/drivers/sub8_videoray_m5_thruster/package.xml b/drivers/sub8_videoray_m5_thruster/package.xml
index dd94a413..43fc5146 100644
--- a/drivers/sub8_videoray_m5_thruster/package.xml
+++ b/drivers/sub8_videoray_m5_thruster/package.xml
@@ -10,4 +10,4 @@
rospy
-
\ No newline at end of file
+