Esempio n. 1
0
 def __init__(self, *args):
     self._current_alarm_state = None
     rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
     super(killtest, self).__init__(*args)
     self.AlarmListener = AlarmListener('hw-kill')
     self.AlarmBroadcaster = AlarmBroadcaster('kill')
     self.AlarmBroadcaster.clear_alarm()
Esempio n. 2
0
    def __init__(self):
        self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')

        AlarmListener('hw-kill', self.hw_kill_alarm_cb)
        AlarmListener('kill', self.kill_alarm_cb)
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber("/network", Header, self.network_cb)  # Passes along network hearbeat to kill board
Esempio n. 3
0
    def __init__(self, controller_name, wrench_pub=None):
        self.name = controller_name
        self.wrench_choices = itertools.cycle(
            ["rc", "emergency", "keyboard", "autonomous"])

        self.kill_broadcaster = AlarmBroadcaster('kill')
        self.station_hold_broadcaster = AlarmBroadcaster('station-hold')

        self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
        self.kill_listener = AlarmListener(
            'kill', callback_funct=self._update_kill_status)

        if (wrench_pub is None):
            self.wrench_pub = wrench_pub
        else:
            self.wrench_pub = rospy.Publisher(wrench_pub,
                                              WrenchStamped,
                                              queue_size=1)

        self.shooter_load_client = actionlib.SimpleActionClient(
            "/shooter/load", ShooterDoAction)
        self.shooter_fire_client = actionlib.SimpleActionClient(
            "/shooter/fire", ShooterDoAction)
        self.shooter_cancel_client = rospy.ServiceProxy(
            "/shooter/cancel", Trigger)
        self.shooter_manual_client = rospy.ServiceProxy(
            "/shooter/manual", ShooterManual)
        self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset",
                                                       Trigger)

        self.is_killed = False
        self.is_timed_out = False
        self.clear_wrench()
Esempio n. 4
0
    def __init__(self):
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

        # Thruster topics to listen to
        motor_topics = ['/FL_motor', '/FR_motor', '/BL_motor', '/BR_motor']

        # Dictionary for the faults as defined in roboteq_msgs/Status
        self.fault_codes = {
            1: 'OVERHEAT',
            2: 'OVERVOLTAGE',
            4: 'UNDERVOLTAGE',
            8: 'SHORT_CIRCUIT',
            16: 'EMERGENCY_STOP',
            32: 'SEPEX_EXCITATION_FAULT',
            64: 'MOSFET_FAILURE',
            128: 'STARTUP_CONFIG_FAULT'
        }

        self._raised = False
        self._raised_alarms = {}

        # Subscribe to the status message for all thruster topics
        [
            rospy.Subscriber(topic + '/status', Status, self._check_faults,
                             topic) for topic in motor_topics
        ]
Esempio n. 5
0
class StationHold(HandlerBase):
    alarm_name = 'station-hold'

    def __init__(self):
        self.move_client = SimpleActionClient('/move_to', MoveAction)
        self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

    def _client_cb(self, terminal_state, result):
        if terminal_state != 3:
            rospy.logwarn(
                'Station hold goal failed (Status={}, Result={})'.format(
                    TerminalState.to_string(terminal_state), result))
            return
        try:
            self.change_wrench('autonomous')
            rospy.loginfo('Now station holding')
            self.broadcaster.clear_alarm()
        except rospy.ServiceException as e:
            rospy.logwarn('Station hold changing wrench failed: {}'.format(e))

    def raised(self, alarm):
        rospy.loginfo("Attempting to station hold")
        station_hold_goal = MoveGoal()
        station_hold_goal.move_type = 'hold'
        self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)

    def cleared(self, alarm):
        # When cleared, do nothing and just wait for new goal / custom wrench
        pass
Esempio n. 6
0
    def __init__(self, port, baud=9600):
        self.load_yaml()

        rospy.init_node("actuator_driver")

        self.disconnection_alarm = AlarmBroadcaster('actuator-board-disconnect')
        self.packet_error_alarm = AlarmBroadcaster('actuator-board-packet-error')

        self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
        self.ser.flushInput()

        # Reset all valves
        rospy.loginfo("Resetting valves.")
        for actuator_key in self.actuators:
            actuator = self.actuators[actuator_key]
            for valve_ports in actuator['ports']:
                valve_port = actuator['ports'][valve_ports]
                self.send_data(valve_port['id'], valve_port['default'])

        rospy.loginfo("Valves ready to go.")

        rospy.Service('~actuate', SetValve, self.got_service_request)
        rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)

        r = rospy.Rate(.2)  # hz
        while not rospy.is_shutdown():
            # Heartbeat to make sure the board is still connected.
            r.sleep()
            self.ping()
Esempio n. 7
0
class BatteryVoltage(HandlerBase):
    alarm_name = 'battery-voltage'

    def __init__(self):
        self.broadcaster = AlarmBroadcaster(self.alarm_name)
        self.low_threshold = rospy.get_param('~battery-voltage/low')
        self.critical_threshold = rospy.get_param('~battery-voltage/critical')
        self.voltage_sub = rospy.Subscriber('/battery_monitor',
                                            Float32,
                                            self._check_voltage,
                                            queue_size=3)
        self._raised = False
        self._severity = 0

    def _check_voltage(self, msg):
        voltage = msg.data
        do_raise = voltage < self.low_threshold
        if do_raise:
            severity = 2 if voltage < self.critical_threshold else 1
            if not self._raised or self._severity != severity:
                self.broadcaster.raise_alarm(
                    severity=severity,
                    problem_description='battery critcaly low'
                    if severity == 2 else 'battery low',
                    parameters={'voltage': voltage})

    def raised(self, alarm):
        self._raised = True
        self._severity = alarm.severity

    def cleared(self, alarm):
        self._raised = False
        self._severity = alarm.severity
Esempio n. 8
0
 def __init__(self, *args, **kwargs):
     super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
     # Initialize thruster mapping from params
     self.thrusters = make_thruster_dictionary(
         rospy.get_param('/thruster_layout/thrusters'))
     # Tracks last hw-kill alarm update
     self._last_hw_kill = None
     # Tracks last soft kill status received from board
     self._last_soft_kill = None
     # Tracks last hard kill status received from board
     self._last_hard_kill = None
     # Tracks last go status broadcasted
     self._last_go = None
     # Used to raise/clear hw-kill when board updates
     self._kill_broadcaster = AlarmBroadcaster('hw-kill')
     # Alarm broadaster for GO command
     self._go_alarm_broadcaster = AlarmBroadcaster('go')
     # Listens to hw-kill updates to ensure another nodes doesn't manipulate it
     self._hw_kill_listener = AlarmListener('hw-kill',
                                            callback_funct=self.on_hw_kill)
     # Provide service for alarm handler to set/clear the motherboard kill
     self._unkill_service = rospy.Service('/set_mobo_kill', SetBool,
                                          self.set_mobo_kill)
     # Sends hearbeat to board
     self._hearbeat_timer = rospy.Timer(rospy.Duration(0.4),
                                        self.send_heartbeat)
     # Create a subscribe for thruster commands
     self._sub = rospy.Subscriber('/thrusters/thrust',
                                  Thrust,
                                  self.on_command,
                                  queue_size=10)
Esempio n. 9
0
class BatteryVoltage(HandlerBase):
    alarm_name = 'battery-voltage'

    def __init__(self):
        self.broadcaster = AlarmBroadcaster(self.alarm_name)
        self.low_threshold = rospy.get_param('~battery-voltage/low')
        self.critical_threshold = rospy.get_param('~battery-voltage/critical')
        self.voltage_sub = rospy.Subscriber('/battery_monitor', Float32, self._check_voltage, queue_size=3)
        self._raised = False
        self._severity = 0

    def _check_voltage(self, msg):
        voltage = msg.data
        do_raise = voltage < self.low_threshold
        if do_raise:
            severity = 2 if voltage < self.critical_threshold else 1
            if not self._raised or self._severity != severity:
                self.broadcaster.raise_alarm(
                    severity=severity,
                    problem_description='battery critcaly low' if severity == 2 else 'battery low',
                    parameters={'voltage': voltage}
                )

    def raised(self, alarm):
        self._raised = True
        self._severity = alarm.severity

    def cleared(self, alarm):
        self._raised = False
        self._severity = alarm.severity
Esempio n. 10
0
 def __init__(self, *args, **kwargs):
     super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
     # Initialize thruster mapping from params
     self.thrusters = make_thruster_dictionary(
         rospy.get_param('/thruster_layout/thrusters'))
     # Tracks last hw-kill alarm update
     self._last_hw_kill = None
     # Tracks last soft kill status received from board
     self._last_soft_kill = None
     # Tracks last hard kill status received from board
     self._last_hard_kill = None
     # Tracks last go status broadcasted
     self._last_go = None
     # Used to raise/clear hw-kill when board updates
     self._kill_broadcaster = AlarmBroadcaster('hw-kill')
     # Alarm broadaster for GO command
     self._go_alarm_broadcaster = AlarmBroadcaster('go')
     # Listens to hw-kill updates to ensure another nodes doesn't manipulate it
     self._hw_kill_listener = AlarmListener(
         'hw-kill', callback_funct=self.on_hw_kill)
     # Provide service for alarm handler to set/clear the motherboard kill
     self._unkill_service = rospy.Service(
         '/set_mobo_kill', SetBool, self.set_mobo_kill)
     # Sends hearbeat to board
     self._hearbeat_timer = rospy.Timer(
         rospy.Duration(0.4), self.send_heartbeat)
     # Create a subscribe for thruster commands
     self._sub = rospy.Subscriber(
         '/thrusters/thrust', Thrust, self.on_command, queue_size=10)
    def __init__(self, port, baud=9600):
        self.load_yaml()

        rospy.init_node("actuator_driver")

        self.disconnection_alarm = AlarmBroadcaster(
            'actuator-board-disconnect')
        self.packet_error_alarm = AlarmBroadcaster(
            'actuator-board-packet-error')

        self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
        self.ser.flushInput()

        # Reset all valves
        rospy.loginfo("Resetting valves.")
        for actuator_key in self.actuators:
            actuator = self.actuators[actuator_key]
            for valve_ports in actuator['ports']:
                valve_port = actuator['ports'][valve_ports]
                self.send_data(valve_port['id'], valve_port['default'])

        rospy.loginfo("Valves ready to go.")

        rospy.Service('~actuate', SetValve, self.got_service_request)
        rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)

        r = rospy.Rate(.2)  # hz
        while not rospy.is_shutdown():
            # Heartbeat to make sure the board is still connected.
            r.sleep()
            self.ping()
Esempio n. 12
0
class StationHold(HandlerBase):
    alarm_name = 'station-hold'

    def __init__(self):
        self.move_client = SimpleActionClient('/move_to', MoveAction)
        self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

    def _client_cb(self, terminal_state, result):
        if terminal_state != 3:
            rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
                TerminalState.to_string(terminal_state), result))
            return
        try:
            self.change_wrench('autonomous')
            rospy.loginfo('Now station holding')
            self.broadcaster.clear_alarm()
        except rospy.ServiceException as e:
            rospy.logwarn('Station hold changing wrench failed: {}'.format(e))

    def raised(self, alarm):
        rospy.loginfo("Attempting to station hold")
        station_hold_goal = MoveGoal()
        station_hold_goal.move_type = 'hold'
        self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)

    def cleared(self, alarm):
        # When cleared, do nothing and just wait for new goal / custom wrench
        pass
Esempio n. 13
0
    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 = []

        # Bus voltage
        self.bus_voltage_estimator = BusVoltageEstimator(self._window_duration)
        self.bus_voltage_pub = rospy.Publisher('bus_voltage',
                                               Float64,
                                               queue_size=1)
        self.bus_timer = rospy.Timer(rospy.Duration(0.1),
                                     self.publish_bus_voltage)
        self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
        self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
        self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")

        self.make_fake = rospy.get_param('simulate', False)
        if self.make_fake:
            rospy.logwarn(
                "Running fake thrusters for simulation, based on parameter '/simulate'"
            )

        # Individual thruster configuration data
        newtons, thruster_input = self.load_effort_to_thrust_map(config_path)
        self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input)

        self.thrust_service = rospy.Service('thrusters/thruster_range',
                                            ThrusterInfo,
                                            self.get_thruster_info)
        self.status_pub = rospy.Publisher('thrusters/thruster_status',
                                          ThrusterStatus,
                                          queue_size=8)

        # Port and thruster layout
        self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
        AlarmListener("thruster-out",
                      self.check_alarm_status,
                      call_when_raised=False)
        self.port_dict = self.load_thruster_layout(ports, thruster_definitions)
        self.drop_check = rospy.Timer(rospy.Duration(0.5),
                                      self.check_for_drops)

        # The bread and bones
        self.thrust_sub = rospy.Subscriber('thrusters/thrust',
                                           Thrust,
                                           self.thrust_cb,
                                           queue_size=1)

        # This is essentially only for testing
        self.fail_thruster_server = rospy.Service('fail_thruster',
                                                  FailThruster,
                                                  self.fail_thruster)
Esempio n. 14
0
    def __init__(self):
        self.port = rospy.get_param(
            '~port',
            "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics",
                                               DiagnosticArray,
                                               queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
        self.hw_kill_broadcaster.wait_for_server()

        self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
        self.ctrl_msg_received = False
        self.ctrl_msg_count = 0
        self.ctrl_msg_timeout = 0
        self.sticks = {}
        for stick in constants[
                'CTRL_STICKS']:  # These are 3 signed 16-bit values for stick positions
            self.sticks[stick] = 0x0000
        self.sticks_temp = 0x0000
        self.buttons = {}
        for button in constants[
                'CTRL_BUTTONS']:  # These are the button on/off states (16 possible inputs)
            self.buttons[button] = False
        self.buttons_temp = 0x0000

        self._hw_kill_listener = AlarmListener('hw-kill',
                                               self.hw_kill_alarm_cb)
        self._kill_listener = AlarmListener('kill', self.kill_alarm_cb)
        self._hw_kill_listener.wait_for_server()
        self._kill_listener.wait_for_server()
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber(
            "/network", Header,
            self.network_cb)  # Passes along network hearbeat to kill board
Esempio n. 15
0
 def __init__(self):
     self.broadcaster = AlarmBroadcaster(self.alarm_name)
     self.low_threshold = rospy.get_param('~battery-voltage/low')
     self.critical_threshold = rospy.get_param('~battery-voltage/critical')
     self.voltage_sub = rospy.Subscriber('/battery_monitor',
                                         Float32,
                                         self._check_voltage,
                                         queue_size=3)
     self._raised = False
     self._severity = 0
Esempio n. 16
0
    def __init__(self):
        gpio = rospy.get_param("/pause-kill/gpio", 507)

        # Use sysfs to monitor gpio
        self.gpio_file = '/sys/class/gpio/gpio{}/value'.format(gpio)

        self.ab = AlarmBroadcaster(self.alarm_name, node_name='pause-kill')
        self._killed = False

        # The correct way would be use 'poll' syscall but meh
        rospy.Timer(rospy.Duration(0.01), self._check)
Esempio n. 17
0
    def __init__(self, timeout=0.5):

        self.timeout = rospy.Duration(timeout)
        self.last_time = rospy.Time.now()
        self.last_position = None
        self.check_count = 0
        self.max_jump = 1.0
        self.odom_discontinuity = False
        self._killed = False
        self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
        self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
        rospy.Timer(rospy.Duration(0.1), self.check)
    def __init__(self, ports_layout, 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.failed_thrusters = set()  # This is only determined by comms
        self.deactivated_thrusters = set(
        )  # These will not come back online even if comms are good (user managed)

        # Alarms
        self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
        AlarmListener("thruster-out",
                      self.check_alarm_status,
                      call_when_raised=False)  # Prevent outside interference

        # Create ThrusterPort objects in a dict indexed by port name
        self.load_thruster_ports(ports_layout, thruster_definitions)

        # Feedback on thrusters (thruster mapper blocks until it can use this service)
        self.thruster_info_service = rospy.Service('thrusters/thruster_info',
                                                   ThrusterInfo,
                                                   self.get_thruster_info)
        self.status_publishers = {
            name: rospy.Publisher('thrusters/status/' + name,
                                  ThrusterStatus,
                                  queue_size=10)
            for name in self.thruster_to_port_map.keys()
        }

        # These alarms require this service to be available before things will work
        rospy.wait_for_service("update_thruster_layout")
        self.update_thruster_out_alarm()

        # Bus voltage
        self.bus_voltage_monitor = BusVoltageMonitor(self._window_duration)

        # Command thrusters
        self.thrust_sub = rospy.Subscriber('thrusters/thrust',
                                           Thrust,
                                           self.thrust_cb,
                                           queue_size=1)

        # To programmatically deactivate thrusters
        self.fail_thruster_server = rospy.Service('fail_thruster',
                                                  FailThruster,
                                                  self.fail_thruster)
        self.unfail_thruster_server = rospy.Service('unfail_thruster',
                                                    UnfailThruster,
                                                    self.unfail_thruster)
Esempio n. 19
0
class OdomKill(HandlerBase):
    '''
    Alarm raised when either Odometry has not been published in a while or the euclidean distance
    between the positions in the last two messages is too large, indicating a sensor error or other
    state estimation issue that will cause unsafe behavior
    '''

    alarm_name = 'odom-kill'
    TIMEOUT_SECONDS = 0.1  # Max time delta between Odometry messages before alarm is raised
    MAX_DISTANCE_METERS = 0.5  # Max distance in position between Odometry messages before alarm is raised

    def __init__(self):
        self.hm = HeartbeatMonitor(self.alarm_name,
                                   "/odom",
                                   Odometry,
                                   node_name="alarm_server",
                                   prd=self.TIMEOUT_SECONDS)
        self.MAX_JUMP = 0.5
        self.launch_time = rospy.Time.now()
        self.last_time = self.launch_time
        self.last_position = None
        self._raised = False
        self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
        rospy.Subscriber('/odom',
                         Odometry,
                         self.check_continuity,
                         queue_size=5)

    def check_continuity(self, odom):
        '''
        On new odom message, find distance in position between this message and the last one.
        If the distance is > MAX_JUMP, raise the alarm
        '''
        position = rosmsg_to_numpy(odom.pose.pose.position)
        if self.last_position is not None:
            jump = np.linalg.norm(position - self.last_position)
            if jump > self.MAX_JUMP and not self._killed:
                self._raised = True  # Avoid raising multiple times
                rospy.logwarn('ODOM DISCONTINUITY DETECTED')
                self.ab.raise_alarm(
                    problem_description=
                    'ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(
                        jump),
                    severity=5)
        self.last_position = position
        self.last_time = odom.header.stamp

    def raised(self, alarm):
        self._raised = True

    def cleared(self, alarm):
        self._raised = False
 def __init__(self, window_duration):
     '''
     window_duration - float (amount of seconds for which to keep a reading in the buffer)
     '''
     self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
     self.bus_voltage_pub = rospy.Publisher('bus_voltage',
                                            Float64,
                                            queue_size=1)
     self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
     self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
     self.last_estimate_time = rospy.Time.now()
     self.WINDOW_DURATION = rospy.Duration(window_duration)
     self.ESTIMATION_PERIOD = rospy.Duration(0.2)
     self.cached_severity = 0
     self.buffer = []
Esempio n. 21
0
 def __init__(self):
     self.hm = HeartbeatMonitor(self.alarm_name,
                                "/odom",
                                Odometry,
                                node_name="alarm_server",
                                prd=self.TIMEOUT_SECONDS)
     self.MAX_JUMP = 0.5
     self.launch_time = rospy.Time.now()
     self.last_time = self.launch_time
     self.last_position = None
     self._raised = False
     self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
     rospy.Subscriber('/odom',
                      Odometry,
                      self.check_continuity,
                      queue_size=5)
Esempio n. 22
0
 def __init__(self):
     self.broadcaster = AlarmBroadcaster(self.alarm_name)
     self.low_threshold = rospy.get_param('~battery-voltage/low')
     self.critical_threshold = rospy.get_param('~battery-voltage/critical')
     self.voltage_sub = rospy.Subscriber('/battery_monitor', Float32, self._check_voltage, queue_size=3)
     self._raised = False
     self._severity = 0
Esempio n. 23
0
 def __init__(self, *args):
     self._current_alarm_state = None
     rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
     super(killtest, self).__init__(*args)
     self.AlarmListener = AlarmListener('hw-kill')
     self.AlarmBroadcaster = AlarmBroadcaster('kill')
     self.AlarmBroadcaster.clear_alarm()
Esempio n. 24
0
    def __init__(self):
        self._killed = False
        self._update_height()

        self.ab = AlarmBroadcaster(self.alarm_name,
                                   node_name="height_over_bottom_kill")

        # Keep track of the current height
        self._last_height = 100
        set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
        rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)

        # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
        rospy.Timer(rospy.Duration(5), self._update_height)

        # This should smooth out random dips below the limit
        rospy.Timer(rospy.Duration(0.5), self._do_check)
Esempio n. 25
0
 def __init__(self, timeout=0.5):
     self.GRACE_PERIOD = rospy.Duration(
         5.0)  # Alarms won't be raised within grace period
     self.TIMEOUT = rospy.Duration(timeout)
     self.MAX_JUMP = 0.5
     self.LAUNCH_TIME = rospy.Time.now()
     self.last_time = self.LAUNCH_TIME
     self.last_position = None
     self.check_count = 0
     self.odom_discontinuity = False
     self._killed = False
     self.odom_listener = rospy.Subscriber('/odom',
                                           Odometry,
                                           self.got_odom_msg,
                                           queue_size=1)
     self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
     rospy.Timer(rospy.Duration(0.1), self.check)
Esempio n. 26
0
class PauseKill(HandlerBase):
    alarm_name = "pause-kill"

    def __init__(self):
        gpio = rospy.get_param("/pause-kill/gpio", 507)

        # Use sysfs to monitor gpio
        self.gpio_file = '/sys/class/gpio/gpio{}/value'.format(gpio)

        self.ab = AlarmBroadcaster(self.alarm_name, node_name='pause-kill')
        self._killed = False

        # The correct way would be use 'poll' syscall but meh
        rospy.Timer(rospy.Duration(0.01), self._check)

    def _check(self, *args):
        try:
            '''
                The following must be completed to enable GPIO sysfs
                echo "507" > /sys/class/gpio/export
                echo "in" > /sys/class/gpio/gpio507/direction
                chmod 777 /sys/class/gpio/gpio507/value
            '''
            file_open = open(self.gpio_file, 'r')
        except IOError:
            rospy.logwarn_throttle(60, 'Is Kill Plug GPIO enabled via sysfs?')
            return
        except Exception:
            rospy.logwarn_throttle(
                60, 'There was an error in opening GPIO for Kill Plug')
            return
        res = file_open.read(1)
        file_open.close()
        if not self._killed and not int(res):
            self._killed = True
            self.ab.raise_alarm(problem_description='KILL PULLED', severity=5)
        elif self._killed and int(res):
            self._killed = False
            self.ab.clear_alarm()

    def raised(self, alarm):
        self._killed = True

    def cleared(self, alarm):
        self._killed = False
Esempio n. 27
0
 def __init__(self):
     self.hm = HeartbeatMonitor(self.alarm_name, "/odom", Odometry,
                                node_name="alarm_server", prd=self.TIMEOUT_SECONDS)
     self.MAX_JUMP = 0.5
     self.launch_time = rospy.Time.now()
     self.last_time = self.launch_time
     self.last_position = None
     self._raised = False
     self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
     rospy.Subscriber('/odom', Odometry, self.check_continuity, queue_size=5)
Esempio n. 28
0
class OdomKill(HandlerBase):
    '''
    Alarm raised when either Odometry has not been published in a while or the euclidean distance
    between the positions in the last two messages is too large, indicating a sensor error or other
    state estimation issue that will cause unsafe behavior
    '''

    alarm_name = 'odom-kill'
    TIMEOUT_SECONDS = 0.1  # Max time delta between Odometry messages before alarm is raised
    MAX_DISTANCE_METERS = 0.5  # Max distance in position between Odometry messages before alarm is raised

    def __init__(self):
        self.hm = HeartbeatMonitor(self.alarm_name, "/odom", Odometry,
                                   node_name="alarm_server", prd=self.TIMEOUT_SECONDS)
        self.MAX_JUMP = 0.5
        self.launch_time = rospy.Time.now()
        self.last_time = self.launch_time
        self.last_position = None
        self._raised = False
        self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
        rospy.Subscriber('/odom', Odometry, self.check_continuity, queue_size=5)

    def check_continuity(self, odom):
        '''
        On new odom message, find distance in position between this message and the last one.
        If the distance is > MAX_JUMP, raise the alarm
        '''
        position = rosmsg_to_numpy(odom.pose.pose.position)
        if self.last_position is not None:
            jump = np.linalg.norm(position - self.last_position)
            if jump > self.MAX_JUMP and not self._killed:
                self._raised = True  # Avoid raising multiple times
                rospy.logwarn('ODOM DISCONTINUITY DETECTED')
                self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(jump),
                                    severity=5)
        self.last_position = position
        self.last_time = odom.header.stamp

    def raised(self, alarm):
        self._raised = True

    def cleared(self, alarm):
        self._raised = False
Esempio n. 29
0
class HeightOverBottom(HandlerBase):
    alarm_name = "height-over-bottom"

    def __init__(self):
        self._killed = False
        self._update_height()

        self.ab = AlarmBroadcaster(self.alarm_name,
                                   node_name="height_over_bottom_kill")

        # Keep track of the current height
        self._last_height = 100
        set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
        rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)

        # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
        rospy.Timer(rospy.Duration(5), self._update_height)

        # This should smooth out random dips below the limit
        rospy.Timer(rospy.Duration(0.5), self._do_check)

    def _do_check(self, *args):
        if self._last_height <= self._height_to_kill and not self._killed:
            rospy.logwarn("SUB TOO LOW!")
            self.ab.raise_alarm(
                problem_description="The sub was too low: {}".format(
                    self._last_height),
                parameters={"height": self._last_height},
                severity=5)
        elif self._last_height >= self._height_to_kill and self._killed:
            rospy.logwarn("REVIVING")
            self.ab.clear_alarm()

    def _update_height(self, *args):
        self._height_to_kill = rospy.get_param("/height_over_bottom", 0.4)

    def raised(self, alarm):
        self._killed = True

    def cleared(self, alarm):
        self._killed = False
Esempio n. 30
0
    def test_callbacks(self):
        al_a = AlarmListener("alarm_d")
        ab_a = AlarmBroadcaster("alarm_d")

        self.raised = False
        self.cleared = False
        self.both = False

        ab_a.clear_alarm()
        al_a.add_callback(self.raised_cb, call_when_cleared=False)
        al_a.add_callback(self.cleared_cb, call_when_raised=False)
        al_a.add_callback(self.both_cb)

        rospy.sleep(0.5)

        # Make sure callbacks are called on creation
        self.assertFalse(self.raised)
        self.assertTrue(self.cleared)
        self.assertTrue(self.both)

        self.raised = False
        self.cleared = False
        self.both = False

        # Make sure callbacks run when they're suppsed to
        ab_a.raise_alarm()
        rospy.sleep(0.5)

        self.assertTrue(self.raised)
        self.assertFalse(self.cleared)
        self.assertTrue(self.both)

        self.raised = False
        self.cleared = False
        self.both = False
Esempio n. 31
0
class HeightOverBottom(HandlerBase):
    alarm_name = "height-over-bottom"

    def __init__(self):
        self._killed = False
        self._update_height()

        self.ab = AlarmBroadcaster(self.alarm_name, node_name="height_over_bottom_kill")

        # Keep track of the current height
        self._last_height = 100
        set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
        rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)

        # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
        rospy.Timer(rospy.Duration(5), self._update_height)

        # This should smooth out random dips below the limit
        rospy.Timer(rospy.Duration(0.5), self._do_check)

    def _do_check(self, *args):
        if self._last_height <= self._height_to_kill and not self._killed:
            rospy.logwarn("SUB TOO LOW!")
            self.ab.raise_alarm(problem_description="The sub was too low: {}".format(self._last_height),
                                parameters={"height": self._last_height},
                                severity=5
                                )
        elif self._last_height >= self._height_to_kill and self._killed:
            rospy.logwarn("REVIVING")
            self.ab.clear_alarm()

    def _update_height(self, *args):
        self._height_to_kill = rospy.get_param("/height_over_bottom", 0.4)

    def raised(self, alarm):
        self._killed = True

    def cleared(self, alarm):
        self._killed = False
Esempio n. 32
0
class StationHold(HandlerBase):
    alarm_name = 'station-hold'

    def __init__(self):
        self.task_client = TaskClient()
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

    def _client_cb(self, terminal_state, result):
        if terminal_state != 3:
            rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
                TerminalState.to_string(terminal_state), result.result))
            return
        rospy.loginfo('Station holding!')
        self.broadcaster.clear_alarm()

    def raised(self, alarm):
        rospy.loginfo("Attempting to station hold")
        self.task_client.run_task('StationHold', done_cb=self._client_cb)

    def cleared(self, alarm):
        # When cleared, do nothing and just wait for new goal / custom wrench
        pass
Esempio n. 33
0
 def __init__(self, timeout=0.5):
     self.GRACE_PERIOD = rospy.Duration(5.0)  # Alarms won't be raised within grace period
     self.TIMEOUT = rospy.Duration(timeout)
     self.MAX_JUMP = 0.5
     self.LAUNCH_TIME = rospy.Time.now()
     self.last_time = self.LAUNCH_TIME
     self.last_position = None
     self.check_count = 0
     self.odom_discontinuity = False
     self._killed = False
     self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
     self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
     rospy.Timer(rospy.Duration(0.1), self.check)
Esempio n. 34
0
 def __init__(self, window_duration):
     '''
     window_duration - float (amount of seconds for which to keep a reading in the buffer)
     '''
     self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
     self.bus_voltage_pub = rospy.Publisher('bus_voltage', Float64, queue_size=1)
     self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
     self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
     self.last_estimate_time = rospy.Time.now()
     self.WINDOW_DURATION = rospy.Duration(window_duration)
     self.ESTIMATION_PERIOD = rospy.Duration(0.2)
     self.cached_severity = 0
     self.buffer = []
Esempio n. 35
0
class StationHold(HandlerBase):
    alarm_name = 'station-hold'

    def __init__(self):
        self.task_client = TaskClient()
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

    def _client_cb(self, terminal_state, result):
        if terminal_state != 3:
            rospy.logwarn(
                'Station hold goal failed (Status={}, Result={})'.format(
                    TerminalState.to_string(terminal_state), result.result))
            return
        rospy.loginfo('Station holding!')
        self.broadcaster.clear_alarm()

    def raised(self, alarm):
        rospy.loginfo("Attempting to station hold")
        self.task_client.run_task('StationHold', done_cb=self._client_cb)

    def cleared(self, alarm):
        # When cleared, do nothing and just wait for new goal / custom wrench
        pass
Esempio n. 36
0
    def __init__(self, controller_name, wrench_pub=None):
        self.name = controller_name
        self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"])

        self.kill_broadcaster = AlarmBroadcaster('kill')
        self.station_hold_broadcaster = AlarmBroadcaster('station-hold')

        self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
        self.kill_listener = AlarmListener('kill', callback_funct=self._update_kill_status)

        if (wrench_pub is None):
            self.wrench_pub = wrench_pub
        else:
            self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)

        self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
        self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
        self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
        self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
        self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)

        self.is_killed = False
        self.is_timed_out = False
        self.clear_wrench()
Esempio n. 37
0
    def __init__(self):
        self._killed = False
        self._update_height()

        self.ab = AlarmBroadcaster(self.alarm_name, node_name="height_over_bottom_kill")

        # Keep track of the current height
        self._last_height = 100
        set_last_height = lambda msg: setattr(self, "_last_height", msg.range)
        rospy.Subscriber("/dvl/range", RangeStamped, set_last_height)

        # Every 5 seconds, check for an updated height param. A pseudo dynamic reconfig thing.
        rospy.Timer(rospy.Duration(5), self._update_height)

        # This should smooth out random dips below the limit
        rospy.Timer(rospy.Duration(0.5), self._do_check)
Esempio n. 38
0
    def __init__(self):
        self.broadcaster = AlarmBroadcaster(self.alarm_name)

        # Thruster topics to listen to
        motor_topics = ['/FL_motor', '/FR_motor', '/BL_motor', '/BR_motor']

        # Subscribe to the status message for all thruster topics
        [rospy.Subscriber(
            topic + '/status', Status, self._check_faults, topic) for topic in motor_topics]

        # Dictionary for the faults as defined in roboteq_msgs/Status
        self.fault_codes = {1: 'OVERHEAT', 2: 'OVERVOLTAGE', 4: 'UNDERVOLTAGE', 8: 'SHORT_CIRCUIT',
                            16: 'EMERGENCY_STOP', 32: 'SEPEX_EXCITATION_FAULT', 64: 'MOSFET_FAILURE',
                            128: 'STARTUP_CONFIG_FAULT'}

        self._raised = False
        self._raised_alarms = {}
Esempio n. 39
0
    def __init__(self):
        self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')

        self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
        self.ctrl_msg_received = False
        self.ctrl_msg_count = 0
        self.ctrl_msg_timeout = 0
        self.sticks = {}
        for stick in constants['CTRL_STICKS']:  # These are 3 signed 16-bit values for stick positions
            self.sticks[stick] = 0x0000
        self.sticks_temp = 0x0000
        self.buttons = {}
        for button in constants['CTRL_BUTTONS']:  # These are the button on/off states (16 possible inputs)
            self.buttons[button] = False
        self.buttons_temp = 0x0000

        AlarmListener('hw-kill', self.hw_kill_alarm_cb)
        AlarmListener('kill', self.kill_alarm_cb)
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber("/network", Header, self.network_cb)  # Passes along network hearbeat to kill board
Esempio n. 40
0
    def __init__(self, ports_layout, 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.failed_thrusters = set()       # This is only determined by comms
        self.deactivated_thrusters = set()  # These will not come back online even if comms are good (user managed)

        # Alarms
        self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
        AlarmListener("thruster-out", self.check_alarm_status, call_when_raised=False)  # Prevent outside interference

        # Create ThrusterPort objects in a dict indexed by port name
        self.load_thruster_ports(ports_layout, thruster_definitions)

        # Feedback on thrusters (thruster mapper blocks until it can use this service)
        self.thruster_info_service = rospy.Service('thrusters/thruster_info', ThrusterInfo, self.get_thruster_info)
        self.status_publishers = {name: rospy.Publisher('thrusters/status/' + name, ThrusterStatus, queue_size=10)
                                  for name in self.thruster_to_port_map.keys()}

        # These alarms require this service to be available before things will work
        rospy.wait_for_service("update_thruster_layout")
        self.update_thruster_out_alarm()

        # Bus voltage
        self.bus_voltage_monitor = BusVoltageMonitor(self._window_duration)

        # Command thrusters
        self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1)

        # To programmatically deactivate thrusters
        self.fail_thruster_server = rospy.Service('fail_thruster', FailThruster, self.fail_thruster)
        self.unfail_thruster_server = rospy.Service('unfail_thruster', UnfailThruster, self.unfail_thruster)
Esempio n. 41
0
class KillInterface(object):
    """
    Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
    if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
    stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
    data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
    and will also periodicly request updates in case the async is not working (it often doesn't).
    """

    ALARM = 'hw-kill'  # Alarm to raise when hardware kill is detected
    YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard'
                       ]  # Wrenches which activate YELLOW LED
    GREEN_WRENCHES = ['autonomous', '/wrench/autonomous'
                      ]  # Wrenches which activate GREEN LED

    def __init__(self):
        self.port = rospy.get_param(
            '~port',
            "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics",
                                               DiagnosticArray,
                                               queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
        self.hw_kill_broadcaster.wait_for_server()

        self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
        self.ctrl_msg_received = False
        self.ctrl_msg_count = 0
        self.ctrl_msg_timeout = 0
        self.sticks = {}
        for stick in constants[
                'CTRL_STICKS']:  # These are 3 signed 16-bit values for stick positions
            self.sticks[stick] = 0x0000
        self.sticks_temp = 0x0000
        self.buttons = {}
        for button in constants[
                'CTRL_BUTTONS']:  # These are the button on/off states (16 possible inputs)
            self.buttons[button] = False
        self.buttons_temp = 0x0000

        self._hw_kill_listener = AlarmListener('hw-kill',
                                               self.hw_kill_alarm_cb)
        self._kill_listener = AlarmListener('kill', self.kill_alarm_cb)
        self._hw_kill_listener.wait_for_server()
        self._kill_listener.wait_for_server()
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber(
            "/network", Header,
            self.network_cb)  # Passes along network hearbeat to kill board

    def connect(self):
        if rospy.get_param(
                '/is_simulation', False
        ):  # If in Gazebo, run fake serial class following board's protocol
            from navigator_kill_board import SimulatedKillBoard
            self.ser = SimulatedKillBoard()
        else:
            baud = rospy.get_param('~baud', 9600)
            self.ser = serial.Serial(port=self.port, baudrate=baud)

    def run(self):
        '''
        Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
        kill board output.
        '''
        rate = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self.last_request is None:
                self.request_next()
            self.receive()
            self.update_ros()
            rate.sleep()

    def update_ros(self):
        self.update_hw_kill()
        self.publish_diagnostics()
        if self.ctrl_msg_received is True:
            self.publish_joy()
            self.ctrl_msg_received = False

    def handle_byte(self, msg):
        '''
        React to a byte recieved from the board. This could by an async update of a kill status or
        a known response to a recent request
        '''
        # If the controller message start byte is received, next 8 bytes are the controller data
        if msg == constants['CONTROLLER']:
            self.ctrl_msg_count = 8
            self.ctrl_msg_timeout = rospy.Time.now()
            return
        # If receiving the controller message, record the byte as stick/button data
        if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8):
            # If 1 second has passed since the message began, timeout and report warning
            if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1):
                self.ctrl_msg_received = False
                self.ctrl_msg_count = 0
                rospy.logwarn(
                    'Timeout receiving controller message. Please disconnect controller.'
                )
            if self.ctrl_msg_count > 2:  # The first 6 bytes in the message are stick data bytes
                if (self.ctrl_msg_count %
                        2) == 0:  # Even number byte: first byte in data word
                    self.sticks_temp = (int(msg.encode("hex"), 16) << 8)
                else:  # Odd number byte: combine two bytes into a stick's data word
                    self.sticks_temp += int(msg.encode("hex"), 16)
                    if (self.ctrl_msg_count > 6):
                        self.sticks['UD'] = self.sticks_temp
                    elif (self.ctrl_msg_count > 4):
                        self.sticks['LR'] = self.sticks_temp
                    else:
                        self.sticks['TQ'] = self.sticks_temp
                    self.sticks_temp = 0x0000
            else:  # The last 2 bytes are button data bytes
                if (self.ctrl_msg_count % 2) == 0:
                    self.buttons_temp = (int(msg.encode("hex"), 16) << 8)
                else:  # Combine two bytes into the button data word
                    self.buttons_temp += int(msg.encode("hex"), 16)
                    for button in self.buttons:  # Each of the 16 bits represents a button on/off state
                        button_check = int(
                            constants['CTRL_BUTTONS_VALUES'][button].encode(
                                "hex"), 16)
                        self.buttons[button] = ((
                            self.buttons_temp & button_check) == button_check)
                    self.buttons_temp = 0x0000
                    self.ctrl_msg_received = True  # After receiving last byte, trigger joy update
            self.ctrl_msg_count -= 1
            return
        # If a response has been recieved to a requested status (button, remove, etc), update internal state
        if self.last_request is not None:
            if msg == constants['RESPONSE_FALSE']:
                if self.board_status[self.last_request] is True:
                    rospy.logdebug('SYNC FALSE for {}'.format(
                        self.last_request))
                self.board_status[self.last_request] = False
                self.last_request = None
                return
            if msg == constants['RESPONSE_TRUE']:
                if self.board_status[self.last_request] is False:
                    rospy.logdebug('SYNC TRUE for {}'.format(
                        self.last_request))
                self.board_status[self.last_request] = True
                self.last_request = None
                return
        # If an async update was recieved, update internal state
        for kill in self.board_status:
            if msg == constants[kill]['FALSE']:
                if self.board_status[kill] is True:
                    rospy.logdebug('ASYNC FALSE for {}'.format(
                        self.last_request))
                self.board_status[kill] = False
                return
            if msg == constants[kill]['TRUE']:
                if self.board_status[kill] is False:
                    rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
                self.board_status[kill] = True
                return
        # If a response to another request, like ping or computer kill/clear is recieved
        for index, byte in enumerate(self.expected_responses):
            if msg == byte:
                del self.expected_responses[index]
                return
        # Log a warning if an unexpected byte was recieved
        rospy.logwarn(
            'Recieved an unexpected byte {}, remaining expected_responses={}'.
            format(hex(ord(msg)), len(self.expected_responses)))

    @thread_lock(lock)
    def receive(self):
        '''
        Recieve update bytes sent from the board without requests being sent, updating internal
        state, raising alarms, etc in response to board updates. Clears the in line buffer.
        '''
        while self.ser.in_waiting > 0 and not rospy.is_shutdown():
            msg = self.ser.read(1)
            self.handle_byte(msg)

    def request_next(self):
        '''
        Manually request status of the next kill, looping back to the first
        once all have been responsded to.
        '''
        self.request_index += 1
        if self.request_index == len(self.kills):
            self.request_index = 0
        self.last_request = self.kills[self.request_index]
        self.request(constants[self.last_request]['REQUEST'])

    def wrench_cb(self, msg):
        '''
        Updates wrench (autnomous vs teleop) diagnostic light if nessesary
        on wrench changes
        '''
        wrench = msg.data
        if wrench != self.wrench:
            if wrench in self.YELLOW_WRENCHES:
                self.request(constants['LIGHTS']['YELLOW_REQUEST'],
                             constants['LIGHTS']['YELLOW_RESPONSE'])
            elif wrench in self.GREEN_WRENCHES:
                self.request(constants['LIGHTS']['GREEN_REQUEST'],
                             constants['LIGHTS']['GREEN_RESPONSE'])
            else:
                self.request(constants['LIGHTS']['OFF_REQUEST'],
                             constants['LIGHTS']['OFF_RESPONSE'])
            self.wrench = wrench

    def network_cb(self, msg):
        '''
        Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
        real one does not work :(
        '''
        self.request(constants['PING']['REQUEST'],
                     constants['PING']['RESPONSE'])

    def publish_diagnostics(self, err=None):
        '''
        Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
        '''
        msg = DiagnosticArray()
        msg.header.stamp = rospy.Time.now()
        status = DiagnosticStatus()
        status.name = 'kill_board'
        status.hardware_id = self.port
        if not self.connected:
            status.level = DiagnosticStatus.ERROR
            status.message = 'Cannot connect to kill board. Retrying every second.'
            status.values.append(KeyValue("Exception", str(err)))
        else:
            status.level = DiagnosticStatus.OK
            for key, value in self.board_status.items():
                status.values.append(KeyValue(key, str(value)))
        msg.status.append(status)
        self.diagnostics_pub.publish(msg)

    def publish_joy(self):
        '''
        Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node
        '''
        current_joy = Joy()
        current_joy.axes.extend([0] * 4)
        current_joy.buttons.extend([0] * 16)
        for stick in self.sticks:
            if self.sticks[
                    stick] >= 0x8000:  # Convert 2's complement hex to signed decimal if negative
                self.sticks[stick] -= 0x10000
        current_joy.axes[0] = np.float32(self.sticks['LR']) / 2048
        current_joy.axes[1] = np.float32(self.sticks['UD']) / 2048
        current_joy.axes[3] = np.float32(self.sticks['TQ']) / 2048
        current_joy.buttons[0] = np.int32(self.buttons['STATION_HOLD'])
        current_joy.buttons[1] = np.int32(self.buttons['RAISE_KILL'])
        current_joy.buttons[2] = np.int32(self.buttons['CLEAR_KILL'])
        current_joy.buttons[4] = np.int32(self.buttons['THRUSTER_RETRACT'])
        current_joy.buttons[5] = np.int32(self.buttons['THRUSTER_DEPLOY'])
        current_joy.buttons[6] = np.int32(self.buttons['GO_INACTIVE'])
        current_joy.buttons[7] = np.int32(self.buttons['START'])
        current_joy.buttons[13] = np.int32(self.buttons['EMERGENCY_CONTROL'])
        current_joy.header.frame_id = "/base_link"
        current_joy.header.stamp = rospy.Time.now()
        self.joy_pub.publish(current_joy)

    def update_hw_kill(self):
        '''
        Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
        '''
        killed = self.board_status['OVERALL']
        if (killed and not self._hw_killed) or (
                killed and self.board_status != self._last_hw_kill_paramaters):
            self._hw_killed = True
            self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
            self._last_hw_kill_paramaters = copy.copy(self.board_status)
        if not killed and self._hw_killed:
            self._hw_killed = False
            self.hw_kill_broadcaster.clear_alarm()

    @thread_lock(lock)
    def request(self, write_str, expected_response=None):
        """
        Deals with requesting data and checking if the response matches some `recv_str`.
        Returns True or False depending on the response.
        With no `recv_str` passed in the raw result will be returned.
        """
        self.ser.write(write_str)
        if expected_response is not None:
            for byte in expected_response:
                self.expected_responses.append(byte)

    def kill_alarm_cb(self, alarm):
        '''
        Informs kill board about software kills through ROS Alarms
        '''
        if alarm.raised:
            self.request(constants['COMPUTER']['KILL']['REQUEST'],
                         constants['COMPUTER']['KILL']['RESPONSE'])
        else:
            self.request(constants['COMPUTER']['CLEAR']['REQUEST'],
                         constants['COMPUTER']['CLEAR']['RESPONSE'])

    def hw_kill_alarm_cb(self, alarm):
        self._hw_killed = alarm.raised
Esempio n. 42
0
class KillInterface(object):
    """
    Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
    if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
    stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
    data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
    and will also periodicly request updates in case the async is not working (it often doesn't).
    """

    ALARM = 'hw-kill'  # Alarm to raise when hardware kill is detected
    YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard']  # Wrenches which activate YELLOW LED
    GREEN_WRENCHES = ['autonomous', '/wrench/autonomous']  # Wrenches which activate GREEN LED

    def __init__(self):
        self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')

        AlarmListener('hw-kill', self.hw_kill_alarm_cb)
        AlarmListener('kill', self.kill_alarm_cb)
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber("/network", Header, self.network_cb)  # Passes along network hearbeat to kill board

    def connect(self):
        if rospy.get_param('/is_simulation', False):  # If in Gazebo, run fake serial class following board's protocol
            from navigator_kill_board import SimulatedKillBoard
            self.ser = SimulatedKillBoard()
        else:
            baud = rospy.get_param('~baud', 9600)
            self.ser = serial.Serial(port=self.port, baudrate=baud)

    def run(self):
        '''
        Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
        kill board output.
        '''
        rate = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self.last_request is None:
                self.request_next()
            self.receive()
            self.update_ros()
            rate.sleep()

    def update_ros(self):
        self.update_hw_kill()
        self.publish_diagnostics()

    def handle_byte(self, msg):
        '''
        React to a byte recieved from the board. This could by an async update of a kill status or
        a known response to a recent request
        '''
        # If a response has been recieved to a requested status (button, remove, etc), update internal state
        if self.last_request is not None:
            if msg == constants['RESPONSE_FALSE']:
                self.board_status[self.last_request] = False
                self.last_request = None
                return
            if msg == constants['RESPONSE_TRUE']:
                rospy.logdebug('RESPONSE TRUE for {}'.format(self.last_request))
                self.board_status[self.last_request] = True
                self.last_request = None
                return
        # If an async update was recieved, update internal state
        for kill in self.board_status:
            if msg == constants[kill]['FALSE']:
                rospy.logdebug('ASYNC FALSE FOR {}'.format(kill))
                self.board_status[kill] = False
                return
            if msg == constants[kill]['TRUE']:
                rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
                self.board_status[kill] = True
                return
        # If a response to another request, like ping or computer kill/clear is recieved
        for index, byte in enumerate(self.expected_responses):
            if msg == byte:
                del self.expected_responses[index]
                return
        # Log a warning if an unexpected byte was recieved
        rospy.logwarn('Recieved an unexpected byte {}, remaining expected_responses={}'.format(
            hex(ord(msg)), len(self.expected_responses)))

    @thread_lock(lock)
    def receive(self):
        '''
        Recieve update bytes sent from the board without requests being sent, updating internal
        state, raising alarms, etc in response to board updates. Clears the in line buffer.
        '''
        while self.ser.in_waiting > 0 and not rospy.is_shutdown():
            msg = self.ser.read(1)
            self.handle_byte(msg)

    def request_next(self):
        '''
        Manually request status of the next kill, looping back to the first
        once all have been responsded to.
        '''
        self.request_index += 1
        if self.request_index == len(self.kills):
            self.request_index = 0
        self.last_request = self.kills[self.request_index]
        self.request(constants[self.last_request]['REQUEST'])

    def wrench_cb(self, msg):
        '''
        Updates wrench (autnomous vs teleop) diagnostic light if nessesary
        on wrench changes
        '''
        wrench = msg.data
        if wrench != self.wrench:
            if wrench in self.YELLOW_WRENCHES:
                self.request(constants['LIGHTS']['YELLOW_REQUEST'], constants['LIGHTS']['YELLOW_RESPONSE'])
            elif wrench in self.GREEN_WRENCHES:
                self.request(constants['LIGHTS']['GREEN_REQUEST'], constants['LIGHTS']['GREEN_RESPONSE'])
            else:
                self.request(constants['LIGHTS']['OFF_REQUEST'], constants['LIGHTS']['OFF_RESPONSE'])
            self.wrench = wrench

    def network_cb(self, msg):
        '''
        Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
        real one does not work :(
        '''
        self.request(constants['PING']['REQUEST'], constants['PING']['RESPONSE'])

    def publish_diagnostics(self, err=None):
        '''
        Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
        '''
        msg = DiagnosticArray()
        msg.header.stamp = rospy.Time.now()
        status = DiagnosticStatus()
        status.name = 'kill_board'
        status.hardware_id = self.port
        if not self.connected:
            status.level = DiagnosticStatus.ERROR
            status.message = 'Cannot connect to kill board. Retrying every second.'
            status.values.append(KeyValue("Exception", str(err)))
        else:
            status.level = DiagnosticStatus.OK
            for key, value in self.board_status.items():
                status.values.append(KeyValue(key, str(value)))
        msg.status.append(status)
        self.diagnostics_pub.publish(msg)

    def update_hw_kill(self):
        '''
        Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
        '''
        killed = self.board_status['OVERALL']
        if (killed and not self._hw_killed) or (killed and self.board_status != self._last_hw_kill_paramaters):
            self._hw_killed = True
            self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
            self._last_hw_kill_paramaters = copy.copy(self.board_status)
        if not killed and self._hw_killed:
            self._hw_killed = False
            self.hw_kill_broadcaster.clear_alarm()

    @thread_lock(lock)
    def request(self, write_str, expected_response=None):
        """
        Deals with requesting data and checking if the response matches some `recv_str`.
        Returns True or False depending on the response.
        With no `recv_str` passed in the raw result will be returned.
        """
        self.ser.write(write_str)
        if expected_response is not None:
            for byte in expected_response:
                self.expected_responses.append(byte)

    def kill_alarm_cb(self, alarm):
        '''
        Informs kill board about software kills through ROS Alarms
        '''
        if alarm.raised:
            self.request(constants['COMPUTER']['KILL']['REQUEST'], constants['COMPUTER']['KILL']['RESPONSE'])
        else:
            self.request(constants['COMPUTER']['CLEAR']['REQUEST'], constants['COMPUTER']['CLEAR']['RESPONSE'])

    def hw_kill_alarm_cb(self, alarm):
        self._hw_killed = alarm.raised
Esempio n. 43
0
class OdomKill(HandlerBase):
    '''
    Will kill the sub when it stops hearing odometry messages or when there is a large
    discontinuity in state estimation. Only meant to run on the sub as a safety measure.
    '''
    def __init__(self, timeout=0.5):
        self.GRACE_PERIOD = rospy.Duration(
            5.0)  # Alarms won't be raised within grace period
        self.TIMEOUT = rospy.Duration(timeout)
        self.MAX_JUMP = 0.5
        self.LAUNCH_TIME = rospy.Time.now()
        self.last_time = self.LAUNCH_TIME
        self.last_position = None
        self.check_count = 0
        self.odom_discontinuity = False
        self._killed = False
        self.odom_listener = rospy.Subscriber('/odom',
                                              Odometry,
                                              self.got_odom_msg,
                                              queue_size=1)
        self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
        rospy.Timer(rospy.Duration(0.1), self.check)

    def check(self, *args):
        self.check_count += 1
        if not self._killed and self.need_kill():
            if self.last_position is None:
                if self.check_count < 10:  # Probably just havent received odom yet
                    pass
                else:  # Odom is probably not publishing
                    self._killed = True
                    self.ab.raise_alarm(
                        problem_description=
                        'STATE ESTIMATION NOT AVAILABLE: KILLING SUB',
                        severity=5)
                    rospy.logerr('STATE ESTIMATION NOT AVAILABLE: KILLING SUB')
            else:
                self._killed = True
                self.ab.raise_alarm(
                    problem_description='STATE ESTIMATION LOSS: KILLING SUB',
                    severity=5)
                rospy.logerr("STATE ESTIMATION LOSS: KILLING SUB")

    def got_odom_msg(self, msg):
        if self.last_position is not None:
            self.check_continuity(msg)
        self.last_position = msg.pose.pose.position
        self.last_time = rospy.Time.now()

    def check_continuity(self, new_odom_msg):  # True if 'continuous'
        if self.odom_discontinuity:
            return

        this_p = new_odom_msg.pose.pose.position
        last_p = self.last_position
        jump = ((this_p.x - last_p.x)**2 + (this_p.y - last_p.y)**2 +
                (this_p.z - last_p.z)**2)**0.5
        if jump > self.MAX_JUMP:
            rospy.logerr('ODOM DISCONTINUITY DETECTED')
            self.ab.raise_alarm(
                problem_description=
                'ODOM DISCONTINUITY DETECTED JUMPED {} METERS'.format(jump),
                severity=5)
            self.odom_discontinuity = True
            return False

    def need_kill(self):
        now = rospy.Time.now()
        in_grace_period = now - self.LAUNCH_TIME < self.GRACE_PERIOD
        odom_loss = now - self.last_time > self.TIMEOUT and not in_grace_period
        if odom_loss:
            rospy.logerr_throttle(
                1, 'LOST ODOM FOR {} SECONDS'.format(
                    (rospy.Time.now() - self.last_time).to_sec()))
            self.ab.raise_alarm(
                problem_description='LOST ODOM FOR {} SECONDS'.format(
                    (rospy.Time.now() - self.last_time).to_sec()),
                severity=5)
        return odom_loss or self.odom_discontinuity

    def clear_kill(self, alarm):
        msg = ""
        if alarm.clear:
            if self._killed:
                self._killed = False
                self.odom_discontinuity = False
                msg = "Odom kill successfully cleared!"
            else:
                msg = "Attempted to clear odom kill, but was already not killed."
            rospy.logwarn(msg)

    def raised(self, alarm):
        self._killed = True

    def cleared(self, alarm):
        self._killed = False
class ActuatorDriver():
    '''
    Allows high level ros code to interface with Daniel's pneumatics board.

    For dropper and grabber, call service with True or False to open or close.
    For shooter, sending a True signal will pulse the valve.

    TODO: Add a function to try and reconnect to the serial port if we lose connection.
    '''
    def __init__(self, port, baud=9600):
        self.load_yaml()

        rospy.init_node("actuator_driver")

        self.disconnection_alarm = AlarmBroadcaster(
            'actuator-board-disconnect')
        self.packet_error_alarm = AlarmBroadcaster(
            'actuator-board-packet-error')

        self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
        self.ser.flushInput()

        # Reset all valves
        rospy.loginfo("Resetting valves.")
        for actuator_key in self.actuators:
            actuator = self.actuators[actuator_key]
            for valve_ports in actuator['ports']:
                valve_port = actuator['ports'][valve_ports]
                self.send_data(valve_port['id'], valve_port['default'])

        rospy.loginfo("Valves ready to go.")

        rospy.Service('~actuate', SetValve, self.got_service_request)
        rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)

        r = rospy.Rate(.2)  # hz
        while not rospy.is_shutdown():
            # Heartbeat to make sure the board is still connected.
            r.sleep()
            self.ping()

    @thread_lock(lock)
    def set_raw_valve(self, srv):
        '''
        Set the valves manually so you don't have to have them defined in the YAML.

        Service parameters:
            actuator: PORT_ID
            opened: OPENED
        '''
        self.send_data(int(srv.actuator), srv.opened)
        return True

    @thread_lock(lock)
    def got_service_request(self, srv):
        '''
        Find out what actuator needs to be changed and how to change it with the valves.yaml file.
        '''
        try:
            this_valve = self.actuators[srv.actuator]
        except:
            rospy.logerr(
                "'%s' not found in valves.yaml so no configuration has been set for that actuator."
                % srv.actuator)
            return False

        if this_valve['type'] == 'pulse':
            # We want to pulse the port from the default value for the desired
            # pulse_time then go back to the default value.
            open_port = this_valve['ports']['open_port']
            open_id = open_port['id']
            open_default_value = open_port['default']

            close_port = this_valve['ports']['close_port']
            close_id = close_port['id']
            close_default_value = close_port['default']

            open_pulse_value = not open_default_value
            close_pulse_value = not close_default_value

            pulse_time = this_valve['pulse_time']

            self.send_data(open_id, open_pulse_value)
            self.send_data(close_id, close_pulse_value)
            rospy.sleep(pulse_time)
            self.send_data(open_id, open_default_value)
            self.send_data(close_id, close_default_value)

        elif this_valve['type'] == 'set':
            # If the desired action is to open, set the open valve to true and the
            # closed false (and visa versa for closing).
            open_port = this_valve['ports']['open_port']
            open_id = open_port['id']

            close_port = this_valve['ports']['close_port']
            close_id = close_port['id']

            if srv.opened:
                self.send_data(open_id, True)
                self.send_data(close_id, False)
            else:
                self.send_data(open_id, False)
                self.send_data(close_id, True)

        return True

    @thread_lock(lock)
    def ping(self):
        rospy.loginfo("ping")

        ping = 0x10
        chksum = ping ^ 0xFF
        data = struct.pack("BB", ping, chksum)
        self.ser.write(data)
        if not self.parse_response(None):
            rospy.logwarn(
                "The board appears to be disconnected, trying again in 3 seconds."
            )
            self.disconnection_alarm.raise_alarm(
                problem_description='The board appears to be disconnected.')
            rospy.sleep(3)

    def send_data(self, port, state):
        '''
        Infomation on communication protcol:

        Sending bytes: send byte (command), then send byte XOR w/ 0xFF (checksum)
        Receiving bytes: receive byte (response), then receive byte XOR w/ 0xFF (checksum)

        Base opcodes:
        - 0x10 ping
        - 0x20 open valve (allow air flow)
        - 0x30 close valve (prevent air flow)
        - 0x40 read switch

        - To 'ping' board (check if board is operational): send 0x10, if operational,
          will reply with 0x11.

        - To open valve (allow air flow): send 0x20 + valve number
          (ex. 0x20 + 0x04 (valve #4) = 0x24 <-- byte to send), will reply with 0x01.

        - To close valve (prevent air flow): send 0x30 + valve number
          (ex. 0x30 + 0x0B (valve #11) = 0x3B <-- byte to send),will reply with 0x00.

        - To read switch: send 0x40 + switch number
          (ex. 0x40 + 0x09 (valve #9) = 0x49 <-- byte to send), will reply with 0x00
          if switch is open (not pressed) or 0x01 if switch is closed (pressed).
        '''
        if port == -1:
            return

        # Calculate checksum and send data to board.
        # A true state tells the pnuematics controller to allow air into the tube.
        open_base_code = 0x20
        closed_base_code = 0x30
        op_code = port
        op_code += open_base_code if state else closed_base_code

        chksum = op_code ^ 0xFF
        data = struct.pack("BB", op_code, chksum)
        rospy.loginfo("Writing: %s. Chksum: %s." % (hex(op_code), hex(chksum)))

        try:
            self.ser.write(data)
        except:
            self.disconnection_alarm.raise_alarm(
                problem_description=
                "Actuator board serial connection has been terminated.")
            return False

        self.parse_response(state)

    def parse_response(self, state):
        '''
        State can be True, False, or None for Open, Closed, or Ping signals respectively.
        This will return True or False based on the correctness of the message.

        TODO: Test with board and make sure all serial signals can be sent and received without need delays.
        '''
        try:
            response = struct.unpack("BB", self.ser.read(2))
        except:
            self.disconnection_alarm.raise_alarm(
                problem_description=
                "Actuator board serial connection has been terminated.")
            return False

        data = response[0]
        chksum = response[1]
        rospy.loginfo("Received: %s. Chksum: %s." % (hex(data), hex(chksum)))

        # Check if packet is intact.
        if data != chksum ^ 0xFF:
            rospy.logerr("CHKSUM NOT MATCHING.")

            self.packet_error_alarm.raise_alarm(
                problem_description="Actuator board checksum error.",
                parameters={
                    'fault_info': {
                        'expected': data ^ 0xFF,
                        'got': response[1]
                    }
                })
            return False

        # Check if packet data is correct.
        if state is None:
            if data == 0x11:
                return True
            return False
        if data == state:
            return True

        self.packet_error_alarm.raise_alarm(
            problem_description="Incorrect response to command.",
            parameters={
                'fault_info': {
                    'expected_state': state,
                    'got_state': data == 1
                },
                'note':
                'True indicates an open port, false indicates a closed port.'
            })
        return False

    def load_yaml(self):
        with open(VALVES_FILE, 'r') as f:
            self.actuators = yaml.load(f)
Esempio n. 45
0
class killtest(unittest.TestCase):

    def __init__(self, *args):
        self._current_alarm_state = None
        rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
        super(killtest, self).__init__(*args)
        self.AlarmListener = AlarmListener('hw-kill')
        self.AlarmBroadcaster = AlarmBroadcaster('kill')
        self.AlarmBroadcaster.clear_alarm()

    def callback(self, msg):
        self._current_alarm_state = msg

    def test_AA_initial_state(self):  # test the initial state of kill signal
        self.AlarmBroadcaster.clear_alarm()
        rospy.sleep(0.1)
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')

    def test_AB_computer(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')

        self.AlarmBroadcaster.raise_alarm()  # raise the computer alarm
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_raised(),
            True,
            msg='COMPUTER raise not worked')

        self.AlarmBroadcaster.clear_alarm()  # clear the computer alarm
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='COMPUTER shutdown not worked')

    def test_AC_button_front_port(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')

        # call the service of button
        rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_PORT')
        try:
            bfp = rospy.ServiceProxy(
                '/kill_board_driver/BUTTON_FRONT_PORT', SetBool)

        except rospy.ServiceException as e:
            print "Service call failed: %s" % e

        bfp(True)  # set the button value to true
        rospy.sleep(0.2)
        self.assertEqual(self.AlarmListener.is_raised(), True,
                         msg='BUTTON_FRONT_PORT raise not worked')

        bfp(False)  # shut down the button
        # when we shut down the button, we also need to clear the computer
        # alarm
        self.AlarmBroadcaster.clear_alarm()
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='BUTTON_FRONT_PORT shutdown not worked. State = {}'.format(
                self._current_alarm_state))

    def test_AD_button_aft_port(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_PORT')
        try:
            bap = rospy.ServiceProxy(
                '/kill_board_driver/BUTTON_AFT_PORT', SetBool)

        except rospy.ServiceException as e:
            print "Service call failed: %s" % e

        bap(True)
        rospy.sleep(0.2)
        self.assertEqual(self.AlarmListener.is_raised(), True,
                         msg='BUTTTON_AFT_PORT raise not worked')

        bap(False)
        self.AlarmBroadcaster.clear_alarm()
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='BUTTTON_AFT_PORT shutdown not worked. State = {}'.format(
                self._current_alarm_state))

    def test_AE_button_front_starboard(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_STARBOARD')
        try:
            bfs = rospy.ServiceProxy(
                '/kill_board_driver/BUTTON_FRONT_STARBOARD', SetBool)

        except rospy.ServiceException as e:
            print "Service call failed: %s" % e

        bfs(True)
        rospy.sleep(0.2)
        self.assertEqual(self.AlarmListener.is_raised(), True,
                         msg='BUTTON_FRONT_STARBOARD raise not worked')

        bfs(False)
        self.AlarmBroadcaster.clear_alarm()
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='BUTTON_FRONT_STARBOARD shutdown not worked. State = {}'.format(
                self._current_alarm_state))

    def test_AF_button_aft_starboard(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_STARBOARD')
        try:
            bas = rospy.ServiceProxy(
                '/kill_board_driver/BUTTON_AFT_STARBOARD', SetBool)

        except rospy.ServiceException as e:
            print "Service call failed: %s" % e

        bas(True)
        rospy.sleep(0.2)
        self.assertEqual(self.AlarmListener.is_raised(), True,
                         msg='BUTTON_AFT_STARBOARD raise not worked')

        bas(False)
        self.AlarmBroadcaster.clear_alarm()
        rospy.sleep(0.2)
        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='BUTTON_AFT_STARBOARD shutdown not worked. State = {}'.format(
                self._current_alarm_state))

    def test_AG_remote(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(3)
        while rospy.Time.now() < t_end:
            hello_header = Header()
            hello_header.stamp = rospy.Time.now()
            rospy.loginfo(hello_header)
            pub.publish(hello_header)
            rate.sleep()

        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='REMOTE shutdown not worked. State = {}'.format(
                self._current_alarm_state))

        # stop sending the msg, the remote alarm will raise when stop recieving
        # the msg for 8 secs
        rospy.sleep(8.2)

        self.assertEqual(
            self.AlarmListener.is_raised(),
            True,
            msg='REMOTE raised not worked. State = {}'.format(
                self._current_alarm_state))
Esempio n. 46
0
class killtest(unittest.TestCase):

    def __init__(self, *args):
        self.hw_kill_alarm = None
        self.updated = False
        self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
        self.AlarmBroadcaster = AlarmBroadcaster('kill')
        super(killtest, self).__init__(*args)

    @thread_lock(lock)
    def reset_update(self):
        '''
        Reset update state to False so we can notice changes to hw-kill
        '''
        self.updated = False

    @thread_lock(lock)
    def _hw_kill_cb(self, alarm):
        '''
        Called on change in hw-kill alarm.
        If the raised status changed, set update flag to true so test an notice change
        '''
        if self.hw_kill_alarm is None or alarm.raised != self.hw_kill_alarm.raised:
            self.updated = True
        self.hw_kill_alarm = alarm

    def wait_for_kill_update(self, timeout=rospy.Duration(0.5)):
        '''
        Wait up to timeout time to an hw-kill alarm change. Returns a copy of the new alarm or throws if times out
        '''
        timeout = rospy.Time.now() + timeout
        while rospy.Time.now() < timeout:
            lock.acquire()
            updated = copy(self.updated)
            alarm = deepcopy(self.hw_kill_alarm)
            lock.release()
            if updated:
                return alarm
            rospy.sleep(0.01)
        raise Exception('timeout')

    def assert_raised(self, timeout=rospy.Duration(0.5)):
        '''
        Waits for update and ensures it is now raised
        '''
        alarm = self.wait_for_kill_update(timeout)
        self.assertEqual(alarm.raised, True)

    def assert_cleared(self, timeout=rospy.Duration(0.5)):
        '''
        Wait for update and ensures is now cleared
        '''
        alarm = self.wait_for_kill_update(timeout)
        self.assertEqual(alarm.raised, False)

    def test_1_initial_state(self):  # test the initial state of kill signal
        '''
        Tests initial state of system, which should have hw-kill raised beause kill is raised at startup.

        Because hw-kill will be initialized to cleared then later raised when alarm server is fully started,
        so we need to allow for pottentialy two updates before it is raised.
        '''
        alarm = self.wait_for_kill_update(timeout=rospy.Duration(10.0))  # Allow lots of time for initial alarm setup
        if alarm.raised:
            self.assertTrue(True)
            return
        self.reset_update()
        self.assert_raised(timeout=rospy.Duration(10.0))

    def test_2_computer_kill(self):
        '''
        Test raising/clearing kill alarm (user kill) will cause same change in hw-kill
        '''
        self.reset_update()
        self.AlarmBroadcaster.clear_alarm()
        self.assert_cleared()

        self.reset_update()
        self.AlarmBroadcaster.raise_alarm()
        self.assert_raised()

        self.reset_update()
        self.AlarmBroadcaster.clear_alarm()
        self.assert_cleared()

    def _test_button(self, button):
        '''
        Tests that button kills work through simulated service.
        '''
        bfp = rospy.ServiceProxy('/kill_board_interface/BUTTON_{}'.format(button), SetBool)
        bfp.wait_for_service(timeout=5.0)

        self.reset_update()
        bfp(True)  # set the button value to true
        self.assert_raised()

        self.reset_update()
        self.AlarmBroadcaster.clear_alarm()
        bfp(False)
        self.assert_cleared()

    def test_3_buttons(self):
        '''
        Tests each of the four buttons
        '''
        for button in ['FRONT_PORT', 'AFT_PORT', 'FRONT_STARBOARD', 'AFT_STARBOARD']:
            self._test_button('FRONT_PORT')

    def test_4_remote(self):
        '''
        Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then
        publishing hearbeat again to ensure alarm gets cleared.
        '''
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()

        self.reset_update()
        rospy.sleep(8.5)  # Wait slighly longer then the timeout on killboard
        self.assert_raised()

        self.reset_update()
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()
        self.AlarmBroadcaster.clear_alarm()
        self.assert_cleared()
Esempio n. 47
0
class OdomKill(HandlerBase):

    '''
    Will kill the sub when it stops hearing odometry messages or when there is a large
    discontinuity in state estimation. Only meant to run on the sub as a safety measure.
    '''

    def __init__(self, timeout=0.5):
        self.GRACE_PERIOD = rospy.Duration(5.0)  # Alarms won't be raised within grace period
        self.TIMEOUT = rospy.Duration(timeout)
        self.MAX_JUMP = 0.5
        self.LAUNCH_TIME = rospy.Time.now()
        self.last_time = self.LAUNCH_TIME
        self.last_position = None
        self.check_count = 0
        self.odom_discontinuity = False
        self._killed = False
        self.odom_listener = rospy.Subscriber('/odom', Odometry, self.got_odom_msg, queue_size=1)
        self.ab = AlarmBroadcaster('odom-kill', node_name='odom-kill')
        rospy.Timer(rospy.Duration(0.1), self.check)

    def check(self, *args):
        self.check_count += 1
        if not self._killed and self.need_kill():
            if self.last_position is None:
                if self.check_count < 10:  # Probably just havent received odom yet
                    pass
                else:  # Odom is probably not publishing
                    self._killed = True
                    self.ab.raise_alarm(problem_description='STATE ESTIMATION NOT AVAILABLE: KILLING SUB',
                                        severity=5)
                    rospy.logerr('STATE ESTIMATION NOT AVAILABLE: KILLING SUB')
            else:
                self._killed = True
                self.ab.raise_alarm(problem_description='STATE ESTIMATION LOSS: KILLING SUB', severity=5)
                rospy.logerr("STATE ESTIMATION LOSS: KILLING SUB")

    def got_odom_msg(self, msg):
        if self.last_position is not None:
            self.check_continuity(msg)
        self.last_position = msg.pose.pose.position
        self.last_time = rospy.Time.now()

    def check_continuity(self, new_odom_msg):  # True if 'continuous'
        if self.odom_discontinuity:
            return

        this_p = new_odom_msg.pose.pose.position
        last_p = self.last_position
        jump = ((this_p.x - last_p.x) ** 2 + (this_p.y - last_p.y) ** 2 + (this_p.z - last_p.z) ** 2) ** 0.5
        if jump > self.MAX_JUMP:
            rospy.logerr('ODOM DISCONTINUITY DETECTED')
            self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED JUMPED {} METERS'.format(jump),
                                severity=5)
            self.odom_discontinuity = True
            return False

    def need_kill(self):
        now = rospy.Time.now()
        in_grace_period = now - self.LAUNCH_TIME < self.GRACE_PERIOD
        odom_loss = now - self.last_time > self.TIMEOUT and not in_grace_period
        if odom_loss:
            rospy.logerr_throttle(1, 'LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()))
            self.ab.raise_alarm(
                problem_description='LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()),
                severity=5
            )
        return odom_loss or self.odom_discontinuity

    def clear_kill(self, alarm):
        msg = ""
        if alarm.clear:
            if self._killed:
                self._killed = False
                self.odom_discontinuity = False
                msg = "Odom kill successfully cleared!"
            else:
                msg = "Attempted to clear odom kill, but was already not killed."
            rospy.logwarn(msg)

    def raised(self, alarm):
        self._killed = True

    def cleared(self, alarm):
        self._killed = False
Esempio n. 48
0
 def __init__(self):
     self.move_client = SimpleActionClient('/move_to', MoveAction)
     self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
     self.broadcaster = AlarmBroadcaster(self.alarm_name)
Esempio n. 49
0
class ThrusterAndKillBoard(CANDeviceHandle):
    '''
    Device handle for the thrust and kill board.
    '''

    def __init__(self, *args, **kwargs):
        super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
        # Initialize thruster mapping from params
        self.thrusters = make_thruster_dictionary(
            rospy.get_param('/thruster_layout/thrusters'))
        # Tracks last hw-kill alarm update
        self._last_hw_kill = None
        # Tracks last soft kill status received from board
        self._last_soft_kill = None
        # Tracks last hard kill status received from board
        self._last_hard_kill = None
        # Tracks last go status broadcasted
        self._last_go = None
        # Used to raise/clear hw-kill when board updates
        self._kill_broadcaster = AlarmBroadcaster('hw-kill')
        # Alarm broadaster for GO command
        self._go_alarm_broadcaster = AlarmBroadcaster('go')
        # Listens to hw-kill updates to ensure another nodes doesn't manipulate it
        self._hw_kill_listener = AlarmListener(
            'hw-kill', callback_funct=self.on_hw_kill)
        # Provide service for alarm handler to set/clear the motherboard kill
        self._unkill_service = rospy.Service(
            '/set_mobo_kill', SetBool, self.set_mobo_kill)
        # Sends hearbeat to board
        self._hearbeat_timer = rospy.Timer(
            rospy.Duration(0.4), self.send_heartbeat)
        # Create a subscribe for thruster commands
        self._sub = rospy.Subscriber(
            '/thrusters/thrust', Thrust, self.on_command, queue_size=10)

    def set_mobo_kill(self, req):
        '''
        Called on service calls to /set_mobo_kill, sending the approrpriate packet to the board
        to unassert or assert to motherboard-origin kill
        '''
        self.send_data(KillMessage.create_kill_message(command=True, hard=False, asserted=req.data).to_bytes(),
                       can_id=KILL_SEND_ID)
        return {'success': True}

    def send_heartbeat(self, timer):
        '''
        Send the special heartbeat packet when the timer triggers
        '''
        self.send_data(HeartbeatMessage.create().to_bytes(),
                       can_id=KILL_SEND_ID)

    def on_hw_kill(self, alarm):
        '''
        Update the classe's hw-kill alarm to the latest update
        '''
        self._last_hw_kill = alarm

    def on_command(self, msg):
        '''
        When a thrust command message is received from the Subscriber, send the appropriate packet
        to the board
        '''
        for cmd in msg.thruster_commands:
            # If we don't have a mapping for this thruster, ignore it
            if cmd.name not in self.thrusters:
                rospy.logwarn(
                    'Command received for {}, but this is not a thruster.'.format(cmd.name))
                continue
            # Map commanded thrust (in newetons) to effort value (-1 to 1)
            effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust)
            # Send packet to command specified thruster the specified force
            packet = ThrustPacket.create_thrust_packet(
                ThrustPacket.ID_MAPPING[cmd.name], effort)
            self.send_data(packet.to_bytes(), can_id=THRUST_SEND_ID)

    def update_hw_kill(self, reason):
        '''
        If needed, update the hw-kill alarm so it reflects the latest status from the board
        '''
        if self._last_soft_kill is None and self._last_hard_kill is None:
            return

        # Set serverity / problem message appropriately
        severity = 0
        message = ""
        if self._last_hard_kill is True:
            severity = 2
            message = "Hard kill"
        elif self._last_soft_kill is True:
            severity = 1
            message = "Soft kill"
        raised = severity != 0
        message = message + ": " + reason

        # If the current status differs from the alarm, update the alarm
        if self._last_hw_kill is None or self._last_hw_kill.raised != raised or \
           self._last_hw_kill.problem_description != message or self._last_hw_kill.severity != severity:
            if raised:
                self._kill_broadcaster.raise_alarm(
                    severity=severity, problem_description=message)
            else:
                self._kill_broadcaster.clear_alarm(
                    severity=severity, problem_description=message)

    def on_data(self, data):
        '''
        Parse the two bytes and raise kills according to the specs:

        First Byte => Kill trigger statuses (Asserted = 1, Unasserted = 0):
            Bit 7: Hard kill status, changed by On/Off Hall effect switch. If this becomes 1, begin shutdown procedure
            Bit 6: Overall soft kill status, 1 if any of the following flag bits are 1. This becomes 0 if all kills are cleared and the thrusters are done re-initializing
            Bit 5: Hall effect soft kill flag
            Bit 4: Mobo command soft kill flag
            Bit 3: Heartbeat lost flag (times out after 1 s)
            Bit 2-0: Reserved
        Second Byte => Input statuses ("Pressed" = 1, "Unpressed" = 0) and thruster initializing status:
            Bit 7: On (1) / Off (0) Hall effect switch status. If this becomes 0, the hard kill in the previous byte becomes 1.
            Bit 6: Soft kill Hall effect switch status. If this is "pressed" = 1, bit 5 of previous byte is unkilled = 0. "Removing" the magnet will "unpress" the switch
            Bit 5: Go Hall effect switch status. "Pressed" = 1, removing the magnet will "unpress" the switch
            Bit 4: Thruster initializing status: This becomes 1 when the board is unkilling, and starts powering thrusters. After the "grace period" it becomes 0 and the overall soft kill status becomes 0. This flag also becomes 0 if killed in the middle of initializing thrusters.
            Bit 3-0: Reserved
        '''
        #print(ord(data[0]), ord(data[1]))
        # Hard Kill - bit 7
        if (ord(data[0]) & 0x8):
            self._last_hard_kill = True
        else:
            self._last_hard_kill = False
        # Soft Kill - bit 6
        if (ord(data[0]) & 0x40):
            self._last_soft_kill = True
        else:
            self._last_soft_kill = False

        reason = ""
        # hall effect - bit 5
        if (ord(data[0]) & 0x20):
            reason = 'hall effect'
        # mobo soft kill - bit 4
        if (ord(data[0]) & 0x10):
            reason = 'mobo soft kill'
        # heartbeat loss - bit 3
        if (ord(data[0]) & 0x08):
            reason = 'Heart beat lost'
        self.update_hw_kill(reason)

        # Switch - bit 5
        if (ord(data[1]) & 0x20):
            asserted = True
        else:
            asserted = False
        if self._last_go is None or asserted != self._last_go:
            if asserted:
                self._go_alarm_broadcaster.raise_alarm(
                    problem_description="Go plug pulled!")
            else:
                self._go_alarm_broadcaster.clear_alarm(
                    problem_description="Go plug returned")
            self._last_go = asserted
Esempio n. 50
0
 def __init__(self):
     self.task_client = TaskClient()
     self.broadcaster = AlarmBroadcaster(self.alarm_name)
Esempio n. 51
0
class RemoteControl(object):

    def __init__(self, controller_name, wrench_pub=None):
        self.name = controller_name
        self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"])

        self.kill_broadcaster = AlarmBroadcaster('kill')
        self.station_hold_broadcaster = AlarmBroadcaster('station-hold')

        self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
        self.kill_listener = AlarmListener('kill', callback_funct=self._update_kill_status)

        if (wrench_pub is None):
            self.wrench_pub = wrench_pub
        else:
            self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1)

        self.shooter_load_client = actionlib.SimpleActionClient("/shooter/load", ShooterDoAction)
        self.shooter_fire_client = actionlib.SimpleActionClient("/shooter/fire", ShooterDoAction)
        self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger)
        self.shooter_manual_client = rospy.ServiceProxy("/shooter/manual", ShooterManual)
        self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger)

        self.is_killed = False
        self.is_timed_out = False
        self.clear_wrench()

    def _timeout_check(function):
        '''
        Simple decorator to check whether or not the remote control device is
        timed out before running the function that was called.
        '''
        @functools.wraps(function)
        def wrapper(self, *args, **kwargs):
            if (not self.is_timed_out):
                return function(self, *args, **kwargs)
        return wrapper

    def _update_kill_status(self, alarm):
        '''
        Updates the kill status display when there is an update on the kill
        alarm.
        '''
        self.is_killed = alarm.raised

    @_timeout_check
    def kill(self, *args, **kwargs):
        '''
        Kills the system regardless of what state it is in.
        '''
        rospy.loginfo("Killing")
        self.kill_broadcaster.raise_alarm(
            problem_description="System kill from user remote control",
            parameters={'location': self.name}
        )

    @_timeout_check
    def clear_kill(self, *args, **kwargs):
        '''
        Clears the system kill regardless of what state it is in.
        '''
        rospy.loginfo("Reviving")
        self.kill_broadcaster.clear_alarm()

    @_timeout_check
    def toggle_kill(self, *args, **kwargs):
        '''
        Toggles the kill status when the toggle_kill_button is pressed.
        '''
        rospy.loginfo("Toggling Kill")

        # Responds to the kill broadcaster and checks the status of the kill alarm
        if self.is_killed:
            self.kill_broadcaster.clear_alarm()
        else:
            self.kill_broadcaster.raise_alarm(
                problem_description="System kill from user remote control",
                parameters={'location': self.name}
            )

    @_timeout_check
    def station_hold(self, *args, **kwargs):
        '''
        Sets the goal point to the current location and switches to autonomous
        mode in order to stay at that point.
        '''
        rospy.loginfo("Station Holding")

        # Trigger station holding at the current pose
        self.station_hold_broadcaster.raise_alarm(
            problem_description="Request to station hold from remote control",
            parameters={'location': self.name}
        )

    @_timeout_check
    def select_autonomous_control(self, *args, **kwargs):
        '''
        Selects the autonomously generated trajectory as the active controller.
        '''
        rospy.loginfo("Changing Control to Autonomous")
        self.wrench_changer("autonomous")

    @_timeout_check
    def select_rc_control(self, *args, **kwargs):
        '''
        Selects the XBox remote joystick as the active controller.
        '''
        rospy.loginfo("Changing Control to RC")
        self.wrench_changer("rc")

    def select_emergency_control(self, *args, **kwargs):
        '''
        Selects the emergency controller as the active controller.
        '''
        rospy.loginfo("Changing Control to Emergency Controller")
        self.wrench_changer("emergency")

    @_timeout_check
    def select_keyboard_control(self, *args, **kwargs):
        '''
        Selects the keyboard teleoperation service as the active controller.
        '''
        rospy.loginfo("Changing Control to Keyboard")
        self.wrench_changer("keyboard")

    @_timeout_check
    def select_next_control(self, *args, **kwargs):
        '''
        Selects the autonomously generated trajectory as the active controller.
        '''
        mode = next(self.wrench_choices)
        rospy.loginfo("Changing Control Mode: {}".format(mode))
        self.wrench_changer(mode)

    def _shooter_load_feedback(self, status, result):
        '''
        Prints the feedback that is returned by the shooter load action client
        '''
        rospy.loginfo("Shooter Load Status={} Success={} Error={}".format(status, result.success, result.error))

    @_timeout_check
    def shooter_load(self, *args, **kwargs):
        '''
        Loads the shooter by using the action client to retract the linear actuator
        '''
        self.shooter_load_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_load_feedback)
        rospy.loginfo("Kip, do not throw away your shot.")

    def _shooter_fire_feedback(self, status, result):
        '''
        Prints the feedback that is returned by the shooter fire action client
        '''
        rospy.loginfo("Shooter Fire Status={} Success={} Error={}".format(status, result.success, result.error))

    @_timeout_check
    def shooter_fire(self, *args, **kwargs):
        '''
        Fires the shooter by using the action client to spin up the
        acceleration discs and extend the linear actuator.
        '''
        self.shooter_fire_client.send_goal(goal=ShooterDoActionGoal(), done_cb=self._shooter_fire_feedback)
        rospy.loginfo("One, two, three, four, five, six, seven, eight, nine. Number... TEN PACES! FIRE!")

    @_timeout_check
    def shooter_cancel(self, *args, **kwargs):
        '''
        Cancels the process that the shooter action client is currently
        running.
        '''
        rospy.loginfo("Canceling shooter requests")
        self.shooter_cancel_client(TriggerRequest())
        rospy.loginfo("I imaging death so much it feels more like a memory.")
        rospy.loginfo("When's it gonna get me? While I'm blocked? Seven clocks ahead of me?")

    def _shooter_reset_helper(self, event):
        '''
        Used to actually call the shooter's reset service.
        '''
        rospy.loginfo("Reseting the shooter service")
        self.shooter_reset_client(TriggerRequest())
        rospy.loginfo("In New York you can be a new man! In New York you can be a new man!")

    @_timeout_check
    def shooter_reset(self, *args, **kwargs):
        '''
        Ensures that the shooter is fully retracted by initiating a retract and
        using a ~6s delay before calling the actual reset service.
        '''
        self.shooter_linear_retract()
        rospy.Timer(rospy.Duration(6), self._shooter_reset_helper, oneshot=True)

    @_timeout_check
    def shooter_linear_extend(self, *args, **kwargs):
        '''
        Extends the shooter's linear actuator by setting it's speed to full
        forward
        '''
        rospy.loginfo("Extending the shooter's linear actuator")
        self.shooter_manual_client(1, 0)

    @_timeout_check
    def shooter_linear_retract(self, *args, **kwargs):
        '''
        Retracts the shooter's linear actuator by setting it's speed to full
        reverse
        '''
        rospy.loginfo("Retracting the shooter's linear actuator")
        self.shooter_manual_client(-1, 0)

    @_timeout_check
    def set_disc_speed(self, speed, *args, **kwargs):
        '''
        Sets the shooters disc speed to the speed value passed in. The value is
        a percentage from -100 to 100, which is scaled down to a number from -1
        to 1.
        '''
        rospy.loginfo("Setting the shooter's accelerator disc speed to {}".format(speed))
        self.shooter_manual_client(0, float(speed) / -100)

    @_timeout_check
    def publish_wrench(self, x, y, rotation, stamp=None, *args, **kwargs):
        '''
        Publishes a wrench to the specified node based on force inputs from the
        controller.
        '''
        if (stamp is None):
            stamp = rospy.Time.now()

        if (self.wrench_pub is not None):
            wrench = WrenchStamped()
            wrench.header.frame_id = "/base_link"
            wrench.header.stamp = stamp
            wrench.wrench.force.x = x
            wrench.wrench.force.y = y
            wrench.wrench.torque.z = rotation
            self.wrench_pub.publish(wrench)

    @_timeout_check
    def clear_wrench(self, *args, **kwargs):
        '''
        Publishes a wrench to the specified node based on force inputs from the
        controller.
        '''
        if (self.wrench_pub is not None):
            wrench = WrenchStamped()
            wrench.header.frame_id = "/base_link"
            wrench.header.stamp = rospy.Time.now()
            wrench.wrench.force.x = 0
            wrench.wrench.force.y = 0
            wrench.wrench.torque.z = 0
            self.wrench_pub.publish(wrench)
 def __init__(self, *args):
     self.hw_kill_alarm = None
     self.updated = False
     self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
     self.AlarmBroadcaster = AlarmBroadcaster('kill')
     super(killtest, self).__init__(*args)
Esempio n. 53
0
class RvizVisualizer(object):

    '''Cute tool for drawing both depth and height-from-bottom in RVIZ
    '''

    def __init__(self):
        rospy.init_node('revisualizer')
        self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
        self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
        self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
        self.kill_server = InteractiveMarkerServer("interactive_kill")

        # text marker
        # TODO: Clean this up, there should be a way to set all of this inline
        self.surface_marker = visualization_msgs.Marker()
        self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
        self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
        self.surface_marker.scale.z = 0.1

        self.depth_marker = visualization_msgs.Marker()
        self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
        self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
        self.depth_marker.scale.z = 0.1

        # create marker for displaying current battery voltage
        self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
        self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
        self.voltage_marker = visualization_msgs.Marker()
        self.voltage_marker.header.frame_id = "base_link"
        self.voltage_marker.lifetime = rospy.Duration(5)
        self.voltage_marker.ns = 'sub'
        self.voltage_marker.id = 22
        self.voltage_marker.pose.position.x = -2.0
        self.voltage_marker.scale.z = 0.2
        self.voltage_marker.color.a = 1
        self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING

        # create an interactive marker to display kill status and change it
        self.need_kill_update = True
        self.kill_marker = InteractiveMarker()
        self.kill_marker.header.frame_id = "base_link"
        self.kill_marker.pose.position.x = -2.3
        self.kill_marker.name = "kill button"
        kill_status_marker = Marker()
        kill_status_marker.type = Marker.TEXT_VIEW_FACING
        kill_status_marker.text = "UNKILLED"
        kill_status_marker.id = 55
        kill_status_marker.scale.z = 0.2
        kill_status_marker.color.a = 1.0
        kill_button_control = InteractiveMarkerControl()
        kill_button_control.name = "kill button"
        kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
        kill_button_control.markers.append(kill_status_marker)
        self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
        self.kill_marker.controls.append(kill_button_control)
        self.killed = False

        # connect kill marker to kill alarm
        self.kill_listener = AlarmListener("kill")
        self.kill_listener.add_callback(self.kill_alarm_callback)
        self.kill_alarm = AlarmBroadcaster("kill")

        # distance to bottom
        self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
        # distance to surface
        self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
        # battery voltage
        self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)

    def update_kill_button(self):
        if self.killed:
            self.kill_marker.controls[0].markers[0].text = "KILLED"
            self.kill_marker.controls[0].markers[0].color.r = 1
            self.kill_marker.controls[0].markers[0].color.g = 0
        else:
            self.kill_marker.controls[0].markers[0].text = "UNKILLED"
            self.kill_marker.controls[0].markers[0].color.r = 0
            self.kill_marker.controls[0].markers[0].color.g = 1
        self.kill_server.insert(self.kill_marker)
        self.kill_server.applyChanges()

    def kill_alarm_callback(self, alarm):
        self.need_kill_update = False
        self.killed = alarm.raised
        self.update_kill_button()

    def kill_buttton_callback(self, feedback):
        if not feedback.event_type == 3:
            return
        if self.need_kill_update:
            return
        self.need_kill_update = True
        if self.killed:
            self.kill_alarm.clear_alarm()
        else:
            self.kill_alarm.raise_alarm()

    def voltage_callback(self, voltage):
        self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts'
        self.voltage_marker.header.stamp = rospy.Time()
        if voltage.data < self.low_battery_threshold:
            self.voltage_marker.color.r = 1
            self.voltage_marker.color.g = 0
        elif voltage.data < self.warn_battery_threshold:
            self.voltage_marker.color.r = 1
            self.voltage_marker.color.g = 1
        else:
            self.voltage_marker.color.r = 0
            self.voltage_marker.color.g = 1
        self.rviz_pub_utils.publish(self.voltage_marker)

    def depth_callback(self, msg):
        '''Handle depth data sent from depth sensor'''
        frame = '/depth'
        distance = msg.depth
        marker = self.make_cylinder_marker(
            np.array([0.0, 0.0, 0.0]),  # place at origin
            length=distance,
            color=(0.0, 1.0, 0.2, 0.7),  # green,
            frame=frame,
            id=0  # Keep these guys from overwriting eachother
        )
        self.surface_marker.ns = 'sub'
        self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
        self.surface_marker.pose = marker.pose
        self.surface_marker.text = str(round(distance, 3)) + 'm'
        self.surface_marker.id = 0

        self.rviz_pub.publish(marker)
        self.rviz_pub_t.publish(self.depth_marker)

    def range_callback(self, msg):
        '''Handle range data grabbed from dvl'''
        # future: should be /base_link/dvl, no?
        frame = '/dvl'
        distance = msg.range

        # Color a sharper red if we're in danger
        if distance < 1.0:
            color = (1.0, 0.1, 0.0, 0.9)
        else:
            color = (0.2, 0.8, 0.0, 0.7)

        marker = self.make_cylinder_marker(
            np.array([0.0, 0.0, 0.0]),  # place at origin
            length=distance,
            color=color,  # red,
            frame=frame,
            up_vector=np.array([0.0, 0.0, -1.0]),  # up is down in range world
            id=1  # Keep these guys from overwriting eachother
        )
        self.depth_marker.ns = 'sub'
        self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
        self.depth_marker.pose = marker.pose
        self.depth_marker.text = str(round(distance, 3)) + 'm'
        self.depth_marker.id = 1

        self.rviz_pub_t.publish(self.depth_marker)
        self.rviz_pub.publish(marker)

    def make_cylinder_marker(self, base, length, color, frame='/base_link',
                             up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
        '''Handle the frustration that Rviz cylinders are designated by their center, not base'''

        center = base + (up_vector * (length / 2))

        pose = Pose(
            position=mil_ros_tools.numpy_to_point(center),
            orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
        )

        marker = visualization_msgs.Marker(
            ns='sub',
            header=mil_ros_tools.make_header(frame=frame),
            type=visualization_msgs.Marker.CYLINDER,
            action=visualization_msgs.Marker.ADD,
            pose=pose,
            color=ColorRGBA(*color),
            scale=Vector3(0.2, 0.2, length),
            lifetime=rospy.Duration(),
            **kwargs
        )
        return marker
Esempio n. 54
0
    def __init__(self):
        rospy.init_node('revisualizer')
        self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
        self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
        self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
        self.kill_server = InteractiveMarkerServer("interactive_kill")

        # text marker
        # TODO: Clean this up, there should be a way to set all of this inline
        self.surface_marker = visualization_msgs.Marker()
        self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
        self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
        self.surface_marker.scale.z = 0.1

        self.depth_marker = visualization_msgs.Marker()
        self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
        self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
        self.depth_marker.scale.z = 0.1

        # create marker for displaying current battery voltage
        self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
        self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
        self.voltage_marker = visualization_msgs.Marker()
        self.voltage_marker.header.frame_id = "base_link"
        self.voltage_marker.lifetime = rospy.Duration(5)
        self.voltage_marker.ns = 'sub'
        self.voltage_marker.id = 22
        self.voltage_marker.pose.position.x = -2.0
        self.voltage_marker.scale.z = 0.2
        self.voltage_marker.color.a = 1
        self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING

        # create an interactive marker to display kill status and change it
        self.need_kill_update = True
        self.kill_marker = InteractiveMarker()
        self.kill_marker.header.frame_id = "base_link"
        self.kill_marker.pose.position.x = -2.3
        self.kill_marker.name = "kill button"
        kill_status_marker = Marker()
        kill_status_marker.type = Marker.TEXT_VIEW_FACING
        kill_status_marker.text = "UNKILLED"
        kill_status_marker.id = 55
        kill_status_marker.scale.z = 0.2
        kill_status_marker.color.a = 1.0
        kill_button_control = InteractiveMarkerControl()
        kill_button_control.name = "kill button"
        kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
        kill_button_control.markers.append(kill_status_marker)
        self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
        self.kill_marker.controls.append(kill_button_control)
        self.killed = False

        # connect kill marker to kill alarm
        self.kill_listener = AlarmListener("kill")
        self.kill_listener.add_callback(self.kill_alarm_callback)
        self.kill_alarm = AlarmBroadcaster("kill")

        # distance to bottom
        self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
        # distance to surface
        self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
        # battery voltage
        self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
Esempio n. 55
0
class KillInterface(object):
    """
    Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
    if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
    stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
    data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
    and will also periodicly request updates in case the async is not working (it often doesn't).
    """

    ALARM = 'hw-kill'  # Alarm to raise when hardware kill is detected
    YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard']  # Wrenches which activate YELLOW LED
    GREEN_WRENCHES = ['autonomous', '/wrench/autonomous']  # Wrenches which activate GREEN LED

    def __init__(self):
        self.port = rospy.get_param('~port', "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
        self.connected = False
        self.diagnostics_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=3)
        while not self.connected and not rospy.is_shutdown():
            try:
                self.connect()
                self.connected = True
            except serial.SerialException as e:
                rospy.logerr('Cannot connect to kill board. {}'.format(e))
                self.publish_diagnostics(e)
                rospy.sleep(1)
        rospy.loginfo('Board connected!')
        self.board_status = {}
        for kill in constants['KILLS']:
            self.board_status[kill] = False
        self.kills = self.board_status.keys()
        self.expected_responses = []
        self.network_msg = None
        self.wrench = ''
        self._hw_killed = False
        self._last_hw_kill_paramaters = self.board_status
        self.last_request = None
        self.request_index = -1

        self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')

        self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
        self.ctrl_msg_received = False
        self.ctrl_msg_count = 0
        self.ctrl_msg_timeout = 0
        self.sticks = {}
        for stick in constants['CTRL_STICKS']:  # These are 3 signed 16-bit values for stick positions
            self.sticks[stick] = 0x0000
        self.sticks_temp = 0x0000
        self.buttons = {}
        for button in constants['CTRL_BUTTONS']:  # These are the button on/off states (16 possible inputs)
            self.buttons[button] = False
        self.buttons_temp = 0x0000

        AlarmListener('hw-kill', self.hw_kill_alarm_cb)
        AlarmListener('kill', self.kill_alarm_cb)
        rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
        rospy.Subscriber("/network", Header, self.network_cb)  # Passes along network hearbeat to kill board

    def connect(self):
        if rospy.get_param('/is_simulation', False):  # If in Gazebo, run fake serial class following board's protocol
            from navigator_kill_board import SimulatedKillBoard
            self.ser = SimulatedKillBoard()
        else:
            baud = rospy.get_param('~baud', 9600)
            self.ser = serial.Serial(port=self.port, baudrate=baud)

    def run(self):
        '''
        Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
        kill board output.
        '''
        rate = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self.last_request is None:
                self.request_next()
            self.receive()
            self.update_ros()
            rate.sleep()

    def update_ros(self):
        self.update_hw_kill()
        self.publish_diagnostics()
        if self.ctrl_msg_received is True:
            self.publish_joy()
            self.ctrl_msg_received = False

    def handle_byte(self, msg):
        '''
        React to a byte recieved from the board. This could by an async update of a kill status or
        a known response to a recent request
        '''
        # If the controller message start byte is received, next 8 bytes are the controller data
        if msg == constants['CONTROLLER']:
            self.ctrl_msg_count = 8
            self.ctrl_msg_timeout = rospy.Time.now()
            return
        # If receiving the controller message, record the byte as stick/button data
        if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8):
            # If 1 second has passed since the message began, timeout and report warning
            if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1):
                self.ctrl_msg_received = False
                self.ctrl_msg_count = 0
                rospy.logwarn('Timeout receiving controller message. Please disconnect controller.')
            if self.ctrl_msg_count > 2:  # The first 6 bytes in the message are stick data bytes
                if (self.ctrl_msg_count % 2) == 0:  # Even number byte: first byte in data word
                    self.sticks_temp = (int(msg.encode("hex"), 16) << 8)
                else:  # Odd number byte: combine two bytes into a stick's data word
                    self.sticks_temp += int(msg.encode("hex"), 16)
                    if (self.ctrl_msg_count > 6):
                        self.sticks['UD'] = self.sticks_temp
                    elif (self.ctrl_msg_count > 4):
                        self.sticks['LR'] = self.sticks_temp
                    else:
                        self.sticks['TQ'] = self.sticks_temp
                    self.sticks_temp = 0x0000
            else:  # The last 2 bytes are button data bytes
                if (self.ctrl_msg_count % 2) == 0:
                    self.buttons_temp = (int(msg.encode("hex"), 16) << 8)
                else:  # Combine two bytes into the button data word
                    self.buttons_temp += int(msg.encode("hex"), 16)
                    for button in self.buttons:  # Each of the 16 bits represents a button on/off state
                        button_check = int(constants['CTRL_BUTTONS_VALUES'][button].encode("hex"), 16)
                        self.buttons[button] = ((self.buttons_temp & button_check) == button_check)
                    self.buttons_temp = 0x0000
                    self.ctrl_msg_received = True  # After receiving last byte, trigger joy update
            self.ctrl_msg_count -= 1
            return
        # If a response has been recieved to a requested status (button, remove, etc), update internal state
        if self.last_request is not None:
            if msg == constants['RESPONSE_FALSE']:
                self.board_status[self.last_request] = False
                self.last_request = None
                return
            if msg == constants['RESPONSE_TRUE']:
                rospy.logdebug('RESPONSE TRUE for {}'.format(self.last_request))
                self.board_status[self.last_request] = True
                self.last_request = None
                return
        # If an async update was recieved, update internal state
        for kill in self.board_status:
            if msg == constants[kill]['FALSE']:
                rospy.logdebug('ASYNC FALSE FOR {}'.format(kill))
                self.board_status[kill] = False
                return
            if msg == constants[kill]['TRUE']:
                rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
                self.board_status[kill] = True
                return
        # If a response to another request, like ping or computer kill/clear is recieved
        for index, byte in enumerate(self.expected_responses):
            if msg == byte:
                del self.expected_responses[index]
                return
        # Log a warning if an unexpected byte was recieved
        rospy.logwarn('Recieved an unexpected byte {}, remaining expected_responses={}'.format(
            hex(ord(msg)), len(self.expected_responses)))

    @thread_lock(lock)
    def receive(self):
        '''
        Recieve update bytes sent from the board without requests being sent, updating internal
        state, raising alarms, etc in response to board updates. Clears the in line buffer.
        '''
        while self.ser.in_waiting > 0 and not rospy.is_shutdown():
            msg = self.ser.read(1)
            self.handle_byte(msg)

    def request_next(self):
        '''
        Manually request status of the next kill, looping back to the first
        once all have been responsded to.
        '''
        self.request_index += 1
        if self.request_index == len(self.kills):
            self.request_index = 0
        self.last_request = self.kills[self.request_index]
        self.request(constants[self.last_request]['REQUEST'])

    def wrench_cb(self, msg):
        '''
        Updates wrench (autnomous vs teleop) diagnostic light if nessesary
        on wrench changes
        '''
        wrench = msg.data
        if wrench != self.wrench:
            if wrench in self.YELLOW_WRENCHES:
                self.request(constants['LIGHTS']['YELLOW_REQUEST'], constants['LIGHTS']['YELLOW_RESPONSE'])
            elif wrench in self.GREEN_WRENCHES:
                self.request(constants['LIGHTS']['GREEN_REQUEST'], constants['LIGHTS']['GREEN_RESPONSE'])
            else:
                self.request(constants['LIGHTS']['OFF_REQUEST'], constants['LIGHTS']['OFF_RESPONSE'])
            self.wrench = wrench

    def network_cb(self, msg):
        '''
        Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
        real one does not work :(
        '''
        self.request(constants['PING']['REQUEST'], constants['PING']['RESPONSE'])

    def publish_diagnostics(self, err=None):
        '''
        Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
        '''
        msg = DiagnosticArray()
        msg.header.stamp = rospy.Time.now()
        status = DiagnosticStatus()
        status.name = 'kill_board'
        status.hardware_id = self.port
        if not self.connected:
            status.level = DiagnosticStatus.ERROR
            status.message = 'Cannot connect to kill board. Retrying every second.'
            status.values.append(KeyValue("Exception", str(err)))
        else:
            status.level = DiagnosticStatus.OK
            for key, value in self.board_status.items():
                status.values.append(KeyValue(key, str(value)))
        msg.status.append(status)
        self.diagnostics_pub.publish(msg)

    def publish_joy(self):
        '''
        Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node
        '''
        current_joy = Joy()
        current_joy.axes.extend([0] * 4)
        current_joy.buttons.extend([0] * 16)
        for stick in self.sticks:
            if self.sticks[stick] >= 0x8000:  # Convert 2's complement hex to signed decimal if negative
                self.sticks[stick] -= 0x10000
        current_joy.axes[0] = np.float32(self.sticks['UD']) / 2048
        current_joy.axes[1] = np.float32(self.sticks['LR']) / 2048
        current_joy.axes[3] = np.float32(self.sticks['TQ']) / 2048
        current_joy.buttons[7] = np.int32(self.buttons['START'])
        current_joy.buttons[3] = np.int32(self.buttons['Y'])
        current_joy.buttons[2] = np.int32(self.buttons['X'])
        current_joy.buttons[0] = np.int32(self.buttons['A'])
        current_joy.buttons[1] = np.int32(self.buttons['B'])
        current_joy.buttons[11] = np.int32(self.buttons['DL'])  # Dpad Left
        current_joy.buttons[12] = np.int32(self.buttons['DR'])  # Dpad Right
        current_joy.header.frame_id = "/base_link"
        current_joy.header.stamp = rospy.Time.now()
        self.joy_pub.publish(current_joy)

    def update_hw_kill(self):
        '''
        Raise/Clear hw-kill ROS Alarm is nessesary (any kills on board are engaged)
        '''
        killed = self.board_status['OVERALL']
        if (killed and not self._hw_killed) or (killed and self.board_status != self._last_hw_kill_paramaters):
            self._hw_killed = True
            self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
            self._last_hw_kill_paramaters = copy.copy(self.board_status)
        if not killed and self._hw_killed:
            self._hw_killed = False
            self.hw_kill_broadcaster.clear_alarm()

    @thread_lock(lock)
    def request(self, write_str, expected_response=None):
        """
        Deals with requesting data and checking if the response matches some `recv_str`.
        Returns True or False depending on the response.
        With no `recv_str` passed in the raw result will be returned.
        """
        self.ser.write(write_str)
        if expected_response is not None:
            for byte in expected_response:
                self.expected_responses.append(byte)

    def kill_alarm_cb(self, alarm):
        '''
        Informs kill board about software kills through ROS Alarms
        '''
        if alarm.raised:
            self.request(constants['COMPUTER']['KILL']['REQUEST'], constants['COMPUTER']['KILL']['RESPONSE'])
        else:
            self.request(constants['COMPUTER']['CLEAR']['REQUEST'], constants['COMPUTER']['CLEAR']['RESPONSE'])

    def hw_kill_alarm_cb(self, alarm):
        self._hw_killed = alarm.raised
Esempio n. 56
0
class ActuatorDriver():

    '''
    Allows high level ros code to interface with Daniel's pneumatics board.

    For dropper and grabber, call service with True or False to open or close.
    For shooter, sending a True signal will pulse the valve.

    TODO: Add a function to try and reconnect to the serial port if we lose connection.
    '''

    def __init__(self, port, baud=9600):
        self.load_yaml()

        rospy.init_node("actuator_driver")

        self.disconnection_alarm = AlarmBroadcaster('actuator-board-disconnect')
        self.packet_error_alarm = AlarmBroadcaster('actuator-board-packet-error')

        self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
        self.ser.flushInput()

        # Reset all valves
        rospy.loginfo("Resetting valves.")
        for actuator_key in self.actuators:
            actuator = self.actuators[actuator_key]
            for valve_ports in actuator['ports']:
                valve_port = actuator['ports'][valve_ports]
                self.send_data(valve_port['id'], valve_port['default'])

        rospy.loginfo("Valves ready to go.")

        rospy.Service('~actuate', SetValve, self.got_service_request)
        rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)

        r = rospy.Rate(.2)  # hz
        while not rospy.is_shutdown():
            # Heartbeat to make sure the board is still connected.
            r.sleep()
            self.ping()

    @thread_lock(lock)
    def set_raw_valve(self, srv):
        '''
        Set the valves manually so you don't have to have them defined in the YAML.

        Service parameters:
            actuator: PORT_ID
            opened: OPENED
        '''
        self.send_data(int(srv.actuator), srv.opened)
        return True

    @thread_lock(lock)
    def got_service_request(self, srv):
        '''
        Find out what actuator needs to be changed and how to change it with the valves.yaml file.
        '''
        try:
            this_valve = self.actuators[srv.actuator]
        except:
            rospy.logerr(
                "'%s' not found in valves.yaml so no configuration has been set for that actuator." %
                srv.actuator)
            return False

        if this_valve['type'] == 'pulse':
            # We want to pulse the port from the default value for the desired
            # pulse_time then go back to the default value.
            open_port = this_valve['ports']['open_port']
            open_id = open_port['id']
            open_default_value = open_port['default']

            close_port = this_valve['ports']['close_port']
            close_id = close_port['id']
            close_default_value = close_port['default']

            open_pulse_value = not open_default_value
            close_pulse_value = not close_default_value

            pulse_time = this_valve['pulse_time']

            self.send_data(open_id, open_pulse_value)
            self.send_data(close_id, close_pulse_value)
            rospy.sleep(pulse_time)
            self.send_data(open_id, open_default_value)
            self.send_data(close_id, close_default_value)

        elif this_valve['type'] == 'set':
            # If the desired action is to open, set the open valve to true and the
            # closed false (and visa versa for closing).
            open_port = this_valve['ports']['open_port']
            open_id = open_port['id']

            close_port = this_valve['ports']['close_port']
            close_id = close_port['id']

            if srv.opened:
                self.send_data(open_id, True)
                self.send_data(close_id, False)
            else:
                self.send_data(open_id, False)
                self.send_data(close_id, True)

        return True

    @thread_lock(lock)
    def ping(self):
        rospy.loginfo("ping")

        ping = 0x10
        chksum = ping ^ 0xFF
        data = struct.pack("BB", ping, chksum)
        self.ser.write(data)
        if not self.parse_response(None):
            rospy.logwarn("The board appears to be disconnected, trying again in 3 seconds.")
            self.disconnection_alarm.raise_alarm(
                problem_description='The board appears to be disconnected.'
            )
            rospy.sleep(3)

    def send_data(self, port, state):
        '''
        Infomation on communication protcol:

        Sending bytes: send byte (command), then send byte XOR w/ 0xFF (checksum)
        Receiving bytes: receive byte (response), then receive byte XOR w/ 0xFF (checksum)

        Base opcodes:
        - 0x10 ping
        - 0x20 open valve (allow air flow)
        - 0x30 close valve (prevent air flow)
        - 0x40 read switch

        - To 'ping' board (check if board is operational): send 0x10, if operational,
          will reply with 0x11.

        - To open valve (allow air flow): send 0x20 + valve number
          (ex. 0x20 + 0x04 (valve #4) = 0x24 <-- byte to send), will reply with 0x01.

        - To close valve (prevent air flow): send 0x30 + valve number
          (ex. 0x30 + 0x0B (valve #11) = 0x3B <-- byte to send),will reply with 0x00.

        - To read switch: send 0x40 + switch number
          (ex. 0x40 + 0x09 (valve #9) = 0x49 <-- byte to send), will reply with 0x00
          if switch is open (not pressed) or 0x01 if switch is closed (pressed).
        '''
        if port == -1:
            return

        # Calculate checksum and send data to board.
        # A true state tells the pnuematics controller to allow air into the tube.
        open_base_code = 0x20
        closed_base_code = 0x30
        op_code = port
        op_code += open_base_code if state else closed_base_code

        chksum = op_code ^ 0xFF
        data = struct.pack("BB", op_code, chksum)
        rospy.loginfo("Writing: %s. Chksum: %s." % (hex(op_code), hex(chksum)))

        try:
            self.ser.write(data)
        except:
            self.disconnection_alarm.raise_alarm(
                problem_description="Actuator board serial connection has been terminated."
            )
            return False

        self.parse_response(state)

    def parse_response(self, state):
        '''
        State can be True, False, or None for Open, Closed, or Ping signals respectively.
        This will return True or False based on the correctness of the message.

        TODO: Test with board and make sure all serial signals can be sent and received without need delays.
        '''
        try:
            response = struct.unpack("BB", self.ser.read(2))
        except:
            self.disconnection_alarm.raise_alarm(
                problem_description="Actuator board serial connection has been terminated."
            )
            return False

        data = response[0]
        chksum = response[1]
        rospy.loginfo("Received: %s. Chksum: %s." % (hex(data), hex(chksum)))

        # Check if packet is intact.
        if data != chksum ^ 0xFF:
            rospy.logerr("CHKSUM NOT MATCHING.")

            self.packet_error_alarm.raise_alarm(
                problem_description="Actuator board checksum error.",
                parameters={
                    'fault_info': {'expected': data ^ 0xFF, 'got': response[1]}
                }
            )
            return False

        # Check if packet data is correct.
        if state is None:
            if data == 0x11:
                return True
            return False
        if data == state:
            return True

        self.packet_error_alarm.raise_alarm(
            problem_description="Incorrect response to command.",
            parameters={
                'fault_info': {'expected_state': state, 'got_state': data == 1},
                'note': 'True indicates an open port, false indicates a closed port.'
            }
        )
        return False

    def load_yaml(self):
        with open(VALVES_FILE, 'r') as f:
            self.actuators = yaml.load(f)
#!/usr/bin/env python
import rospy
from ros_alarms import AlarmListener, AlarmBroadcaster

if __name__ == "__main__":
    rospy.init_node("broadcaster_listener_test")

    ab = AlarmBroadcaster("test_alarm")
    al = AlarmListener("test_alarm")

    print "Inited"
    assert al.is_cleared()

    ab.raise_alarm()
    assert al.is_raised()

    rospy.sleep(0.5)
    ab.clear_alarm()
    assert al.is_cleared()

    rospy.sleep(0.5)
    ab.raise_alarm(parameters={
        "int": 4,
        "list": [1, 2, 3],
        "str": "stringing"
    })

    print "All checks passed"
Esempio n. 58
0
 def __init__(self, *args):
     self.hw_kill_alarm = None
     self.updated = False
     self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
     self.AlarmBroadcaster = AlarmBroadcaster('kill')
     super(killtest, self).__init__(*args)