text: #!/usr/bin/env python
  import numpy as np
  import sys
  import serial
  import rospy
  import rospkg
  import rosparam
  from sub8_thruster_comm import thruster_comm_factory
  import mil_misc_tools.text_effects as te
  from mil_misc_tools.terminal_input import get_ch
  
  __author__ = 'David Soto'
  
  rospack = rospkg.RosPack()
  
  fprint = te.FprintFactory(title='ThrusterSpinner').fprint
  
  
  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
  
  rospy.init_node('thruster_spinner')
  
  sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper')
  thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0]
  thruster_ports = ports_from_layout(thruster_layout)
  if thruster_ports == {}:
      fprint('Unable to connect to any thruster ports. Quitting.')
      sys.exit()
  names_from_motor_id = {0: 'FLH', 1: 'FLV', 2: 'FRH', 3: 'FRV',
                         4: 'BLH', 5: 'BLV', 6: 'BRH', 7: 'BRV'}
  usage_msg = \
      '''
  Welcome to David's thruster_spin_test tool.
  \tInstructions:
  \t* press up or down to control the thrust commanded
  \t* press the thruster id to toggle spinning for that thruster (0-indexed, 8 for all)
  \t* press 'C' to check for thrusters on a specific port (even if not in layout)
  \t* press 'H' to see this usage message again
  \t* press 'Q' to exit
  '''
  fprint(usage_msg)
  
  active_thrusters = set()
  
  thrust = 0.0  # Normalized thrust in [-1, 1]
  step = 0.05
  
  key = None
  
  
  def check_for_thrusters():
      ''' Checks for a response from thrusters on a given port '''
      active_thrusters = set()
      fprint("Active thrusters: {}".format(list(active_thrusters)))
  
      declared_ports = []
      for port in thruster_ports.values():
          if port not in declared_ports:
              declared_ports.append(port)
  
      declared_ports.sort(key=lambda p: p.port_name)
  
      fprint('Declared ports:')
      for i, p in enumerate(declared_ports):
          print "{})".format(i), p.port_name
  
      fprint("Select a port by index: ")
      sel = int(raw_input())
  
      start_id = 0
      end_id = 10
      fprint("Checking for thrusters with id's in [{}, {}]".format(start_id, end_id))
  
      found_motor_ids, avg_turnaround_time = \
          declared_ports[sel].get_motor_ids_on_port(start_id, end_id)
      fprint("Responding motor ids:\t\t\t{}".format(te.Printer().set_cyan.bold(found_motor_ids)))
      fprint("Average packet turnaround time:\t{} seconds".format(te.Printer().set_cyan.bold(avg_turnaround_time)))
  
  
  def command_thrusters(timer_event):
      thrusters_to_command = active_thrusters.copy()
      for motor_id in thrusters_to_command:
          name = names_from_motor_id[motor_id]
          thruster_ports[name].command_thruster(name, thrust)
  
  timer = rospy.Timer(period=rospy.Duration(0.01), callback=command_thrusters)
  
  interesting = rospy.Rate(20)
  while not rospy.is_shutdown():
      interesting.sleep()
      key = get_ch()
  
      # quitting
      if key == 'q':
          fprint("Quitting")
          sys.exit()
  
      # modify thrust
      if key == '\x1b':              # ESC sequence
          if get_ch() == '[':        # Got an arrow key
              arrow_type = get_ch()
              if arrow_type == 'A':  # UP key
                  thrust = thrust + step
              if arrow_type == 'B':  # DOWN key
                  thrust = thrust - step
          else:                      # Got other ESC sequence
              continue
          thrust = np.clip(thrust, -1.0, 1.0)
          fprint("Thrust: {}".format(thrust))
          continue
  
      # toggle active thrusters
      if ord(key) in range(ord('0'), ord('9')):
          motor_id = int(key)
          if motor_id is 8:
              if active_thrusters == {0, 1, 2, 3, 4, 5, 6, 7}:
                  active_thrusters = set()
              elif active_thrusters == set():
                  active_thrusters = {0, 1, 2, 3, 4, 5, 6, 7}
              else:
                  active_thrusters = set()
          else:
              if motor_id in active_thrusters:
                  active_thrusters.remove(motor_id)
              else:
                  active_thrusters.add(motor_id)
          fprint("Active thrusters: {}".format(list(active_thrusters)))
  
      # Perform thruster check
      if key == 'c':
          try:
              check_for_thrusters()
          except BaseException as e:
              fprint(e)
          continue
  
      # Display help
      if key == 'h':
          fprint(usage_msg)
  