def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
msg = te.Printer('Instantiating thruster_port:')
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
thruster_port = thruster_comm_factory(port_info,
thruster_definitions,
fake=False)
# Add the thrusters to the thruster dict
for name in thruster_names:
port_dict[name] = thruster_port
s = str(
np.sort([x[1] for x in thruster_port.thruster_dict.items()
]).tolist())
fprint(
msg.reset.text("\n\tName: ").set_yellow.text(port).reset.text(
"\n\tMotor id's on port: ").set_cyan.bold(s).reset)
except serial.serialutil.SerialException:
pass
return port_dict
def load_thruster_ports(self, ports_layout, thruster_definitions):
''' Loads a dictionary ThrusterPort objects '''
self.ports = {} # ThrusterPort objects
self.thruster_to_port_map = {} # node_id to ThrusterPort
rospack = rospkg.RosPack()
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'")
# Instantiate thruster comms port
for port_info in ports_layout:
port_name = port_info['port']
self.ports[port_name] = thruster_comm_factory(port_info, thruster_definitions, fake=self.make_fake)
# Add the thrusters to the thruster dict and configure if present
for thruster_name in port_info['thruster_names']:
self.thruster_to_port_map[thruster_name] = port_info['port']
if thruster_name not in self.ports[port_name].online_thruster_names:
rospy.logerr("ThrusterDriver: {} IS MISSING!".format(thruster_name))
else:
rospy.loginfo("ThrusterDriver: {} registered".format(thruster_name))
# Set firmware settings
port = self.ports[port_name]
node_id = thruster_definitions[thruster_name]['node_id']
config_path = (rospack.get_path('sub8_videoray_m5_thruster') + '/config/firmware_settings/' +
thruster_name + '.yaml')
rospy.loginfo('Configuring {} with settings specified in {}.'.format(thruster_name,
config_path))
port.set_registers_from_dict(node_id=node_id,
reg_dict=rosparam.load_file(config_path)[0][0])
port.reboot_thruster(node_id) # Necessary for some settings to take effect
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_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 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.")
else:
rospy.loginfo("{} registered".format(thruster_name))
self.thruster_heartbeats[thruster_name] = None
port_dict[thruster_name] = thruster_port
return port_dict
def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
msg = te.Printer('Instantiating thruster_port:')
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
thruster_port = thrust_comm.thruster_comm_factory(
port_info, thruster_definitions, fake=False)
# Add the thrusters to the thruster dict
for name in thruster_names:
port_dict[name] = thruster_port
s = list(thruster_port.online_thruster_names)
fprint(
msg.reset.text('\n\tName: ').set_yellow.text(port).reset.text(
'\n\tMotor id\'s on port: ').set_cyan.bold(s).reset)
except SubjuGatorException:
pass
return port_dict
def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
msg = "Instantiating thruster_port:\n\tName: {}".format(port)
thruster_port = thruster_comm_factory(port_info,
thruster_definitions,
fake=False)
# Add the thrusters to the thruster dict
for name in thruster_names:
port_dict[name] = thruster_port
msg += "\n\tMotor id's on port: {}".format(
thruster_port.motor_ids_on_port)
fprint(msg)
except serial.serialutil.SerialException as e:
pass
return port_dict
def test_thruster_comm_factory_fake(self):
'''Test that the thruster factory returns a proper simulated FakeThrusterPort'''
# This should succeed
thrust_comm = thruster_comm_factory(
self.thruster_layout['thruster_ports'][0],
self.thruster_layout['thrusters'],
fake=True)
self.assertIsNotNone(thrust_comm)
def test_thruster_comm_factory_real_fail(self):
'''Test that the comm factory fails to create a ThrusterPort on a port that does not exist'''
# This should fail
port_info = self.thruster_layout['thruster_ports'][0]
thruster_defs = self.thruster_layout['thrusters']
port_info['port'] = 'bad_name'
with self.assertRaises(IOError):
thrust_comm = thruster_comm_factory(port_info, thruster_defs, fake=False)
self.assertIsNotNone(thrust_comm)
def test_thruster_comm_factory_real_fail(self):
'''Test that the comm factory fails to create a ThrusterPort on a port that does not exist'''
# This should fail
port_info = self.thruster_layout['thruster_ports'][0]
thruster_defs = self.thruster_layout['thrusters']
port_info['port'] = 'bad_name'
with self.assertRaises(IOError):
thrust_comm = thruster_comm_factory(port_info, thruster_defs, fake=False)
self.assertIsNotNone(thrust_comm)
def load_bus_layout(self, layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
for port in layout:
thruster_port = thruster_comm_factory(port, fake=self.make_fake)
# Add the thrusters to the thruster dict
for thruster_name, thruster_info in port['thrusters'].items():
port_dict[thruster_name] = thruster_port
return port_dict
def load_bus_layout(self, layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
for port in layout:
thruster_port = thruster_comm_factory(port, fake=self.make_fake)
# Add the thrusters to the thruster dict
for thruster_name, thruster_info in port['thrusters'].items():
port_dict[thruster_name] = thruster_port
return port_dict
def load_thruster_ports(self, ports_layout, thruster_definitions):
''' Loads a dictionary ThrusterPort objects '''
self.ports = {} # ThrusterPort objects
self.thruster_to_port_map = {} # node_id to ThrusterPort
rospack = rospkg.RosPack()
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn(
"Running fake thrusters for simulation, based on parameter '/simulate'"
)
# Instantiate thruster comms port
for port_info in ports_layout:
port_name = port_info['port']
self.ports[port_name] = thruster_comm_factory(port_info,
thruster_definitions,
fake=self.make_fake)
# Add the thrusters to the thruster dict and configure if present
for thruster_name in port_info['thruster_names']:
self.thruster_to_port_map[thruster_name] = port_info['port']
if thruster_name not in self.ports[
port_name].online_thruster_names:
rospy.logerr(
"ThrusterDriver: {} IS MISSING!".format(thruster_name))
else:
rospy.loginfo(
"ThrusterDriver: {} registered".format(thruster_name))
# Set firmware settings
port = self.ports[port_name]
node_id = thruster_definitions[thruster_name]['node_id']
config_path = (
rospack.get_path('sub8_videoray_m5_thruster') +
'/config/firmware_settings/' + thruster_name + '.yaml')
rospy.loginfo(
'Configuring {} with settings specified in {}.'.format(
thruster_name, config_path))
port.set_registers_from_dict(
node_id=node_id,
reg_dict=rosparam.load_file(config_path)[0][0])
port.reboot_thruster(
node_id) # Necessary for some settings to take effect
def load_bus_layout(self, layout):
'''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)
# Add the thrusters to the thruster dict
for thruster_name, thruster_info in port['thrusters'].items():
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.")
else:
rospy.loginfo("{} registered".format(thruster_name))
self.thruster_heartbeats[thruster_name] = None
port_dict[thruster_name] = thruster_port
return port_dict
def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
msg = te.Printer('Instantiating thruster_port:')
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
thruster_port = thrust_comm.thruster_comm_factory(port_info, thruster_definitions, fake=False)
# Add the thrusters to the thruster dict
for name in thruster_names:
port_dict[name] = thruster_port
s = list(thruster_port.online_thruster_names)
fprint(msg.reset.text('\n\tName: ').set_yellow.text(port).reset
.text('\n\tMotor id\'s on port: ').set_cyan.bold(s).reset)
except SubjuGatorException:
pass
return port_dict
def ports_from_layout(layout):
'''Load and handle the thruster bus layout'''
port_dict = {}
thruster_definitions = layout['thrusters']
msg = te.Printer('Instantiating thruster_port:')
for port_info in layout['thruster_ports']:
try:
port = port_info['port']
thruster_names = port_info['thruster_names']
thruster_port = thruster_comm_factory(port_info, thruster_definitions, fake=False)
# Add the thrusters to the thruster dict
for name in thruster_names:
port_dict[name] = thruster_port
s = str(np.sort([x[1] for x in thruster_port.thruster_dict.items()]).tolist())
fprint(msg.reset.text("\n\tName: ").set_yellow.text(port).reset
.text("\n\tMotor id's on port: ").set_cyan.bold(s).reset)
except serial.serialutil.SerialException:
pass
return port_dict
def test_thruster_comm_factory_fake(self):
'''Test that the thruster factory returns a proper simulated FakeThrusterPort'''
# This should succeed
thrust_comm = thruster_comm_factory(self.port_info, fake=True)
def test_thruster_comm_factory_fake(self):
'''Test that the thruster factory returns a proper simulated FakeThrusterPort'''
# This should succeed
thrust_comm = thruster_comm_factory(self.thruster_layout['thruster_ports'][0],
self.thruster_layout['thrusters'], fake=True)
self.assertIsNotNone(thrust_comm)
def test_thruster_comm_factory_real_fail(self):
'''Test that the comm factory fails to create a ThrusterPort on a port that does not exist'''
# This should fail
with self.assertRaises(IOError):
thrust_comm = thruster_comm_factory(self.port_info, fake=False)
