def connect_ros(self): ''' Connect ROS nodes, services, and alarms to variables and methods within this class. ''' # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set) self.battery_low_voltage = rospy.get_param( "/battery_monitor/battery_low_voltage", 22.1) self.battery_critical_voltage = rospy.get_param( "/battery_monitor/battery_critical_voltage", 20.6) rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage) rospy.Subscriber("/clock", Clock, self.cache_system_time) self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) self.kill_listener = AlarmListener('kill', self.update_kill_status) alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = alarm_broadcaster.add_alarm(name='kill', action_required=True, severity=0) self.station_hold_alarm = alarm_broadcaster.add_alarm( name='station_hold', action_required=True, severity=3)
def __init__(self, controller_name, wrench_pub=None): self.name = controller_name self.wrench_choices = itertools.cycle(["rc", "keyboard", "autonomous"]) self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name="kill", action_required=True, severity=0 ) self.station_hold_alarm = self.alarm_broadcaster.add_alarm( name="station_hold", action_required=False, severity=3 ) # rospy.wait_for_service("/change_wrench") self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.kill_listener = AlarmListener("kill", 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 connect_ros(self): ''' Connect ROS nodes, services, and alarms to variables and methods within this class. ''' # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set) self.battery_low_voltage = rospy.get_param("/battery_monitor/battery_low_voltage", 22.1) self.battery_critical_voltage = rospy.get_param("/battery_monitor/battery_critical_voltage", 20.6) rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage) rospy.Subscriber("/clock", Clock, self.cache_system_time) self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) self.kill_listener = AlarmListener('kill', self.update_kill_status) alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = alarm_broadcaster.add_alarm( name='kill', action_required=True, severity=0 ) self.station_hold_alarm = alarm_broadcaster.add_alarm( name='station_hold', action_required=True, severity=3 )
def __init__(self, port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0", baud=9600): self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25) self.ser.flush() self.killstatus_pub = rospy.Publisher("/killstatus", KillStatus, queue_size=1) ab = AlarmBroadcaster() self.kill_alarm = ab.add_alarm("hw_kill", problem_description="Hardware kill from a kill switch.") self.disconnect = ab.add_alarm("kill_system_disconnect") self.killed = False # Initial check of kill status self.get_status() self.current_wrencher = '' _set_wrencher = lambda msg: setattr(self, 'current_wrencher', msg.data) rospy.Subscriber("/wrench/current", String, _set_wrencher) al = AlarmListener("kill", self.alarm_kill_cb) # Dict of op-codes to functions that need to be run with each code for aysnc responses. self.update_cbs = {'\x10': self.set_kill, '\x11': self.set_unkill, '\x12': lambda: True, '\x13': lambda: True, '\x14': lambda: True, '\x15': lambda: True, '\x16': lambda: True, '\x17': lambda: True, '\x18': lambda: True, '\x19': lambda: True, '\x1A': lambda: True, '\x1B': lambda: True, '\x1C': lambda: True, '\x1D': lambda: True, '\x1E': self.disconnect.clear_alarm, '\x1F': self.disconnect.raise_alarm,} while not rospy.is_shutdown(): rospy.sleep(.1) while self.ser.inWaiting() > 0: self.check_buffer() self.control_check() self.get_status()
def test_add_alarm(self): '''Ensure that adding an alarm succeeds without errors''' broadcaster = AlarmBroadcaster() alarm = broadcaster.add_alarm( name='wake-up', action_required=True, severity=1, problem_description='This is a problem', parameters={"concern": ["a", "b", "c"]} ) self.assertIsNotNone(alarm)
def __init__(self): self.force_scale = rospy.get_param("~force_scale", 600) self.torque_scale = rospy.get_param("~torque_scale", 500) self.wrench_choices = itertools.cycle(['rc', 'autonomous']) self.current_pose = Odometry() self.active = False self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name='kill', action_required=True, severity=0 ) self.station_hold = self.alarm_broadcaster.add_alarm( name='station_hold', action_required=False, severity=3 ) # self.docking_alarm = self.alarm_broadcaster.add_alarm( # name='docking', # action_required=True, # severity=0 # ) self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1) # rospy.wait_for_service('/change_wrench') self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) rospy.Subscriber("joy", Joy, self.joy_cb) self.reset()
def __init__(self, controller_name, wrench_pub=None): self.name = controller_name self.wrench_choices = itertools.cycle(["rc", "emergency", "keyboard", "autonomous"]) self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name="rc_kill", action_required=True, severity=0 ) self.station_hold_alarm = self.alarm_broadcaster.add_alarm( name="station_hold", action_required=False, severity=3 ) # rospy.wait_for_service("/change_wrench") self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.kill_listener = AlarmListener("rc_kill", 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()
class HandlerBase(object): ''' We will listen for an alarm with this `alarm_name`. When that alarm is raised, self.handle will be called. When that alarm is cleared, self.cancel will be called. ''' alarm_name = 'generic_name' alarm_broadcaster = AlarmBroadcaster() def handle(self, alarm, time_sent, parameters): rospy.logwarn("No handle function defined for '{}'.".format( alarm.alarm_name)) return def cancel(self, alarm, time_sent, parameters): rospy.logwarn("No cancel function defined for '{}'.".format( alarm.alarm_name)) return
def __init__(self): # Initialize the average voltage to none for the case of no feedback self.voltage = None # Initialize a list to hold the 1000 most recent supply voltage readings # Holding 1000 values ensures that changes in the average are gradual rather than sharp self.supply_voltages = [] # The publisher for the averaged voltage self.pub_voltage = rospy.Publisher("/battery_monitor", Float32, queue_size=1) # Subscribes to the feedback from each of the four thrusters rospy.Subscriber("/FL_motor/feedback", Feedback, self.add_voltage) rospy.Subscriber("/FR_motor/feedback", Feedback, self.add_voltage) rospy.Subscriber("/BL_motor/feedback", Feedback, self.add_voltage) rospy.Subscriber("/BR_motor/feedback", Feedback, self.add_voltage) # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set) self.battery_low_voltage = rospy.get_param("~battery_low_voltage", 26) self.battery_critical_voltage = rospy.get_param("~battery_critical_voltage", 24) self.battery_kill_voltage = rospy.get_param("~battery_kill_voltage", 20) # Sets up the battery voltage alarms alarm_broadcaster = AlarmBroadcaster() self.battery_status_unknown_alarm = alarm_broadcaster.add_alarm( name="battery_status_unknown", action_required=True, severity=2 ) self.battery_low_alarm = alarm_broadcaster.add_alarm( name="battery_low", action_required=False, severity=2 ) self.battery_critical_alarm = alarm_broadcaster.add_alarm( name="battery_critical", action_required=True, severity=1 ) self.battery_kill_alarm = alarm_broadcaster.add_alarm( name="kill", action_required=True, severity=0 )
def test_instantiate_alarm_broadcaster(self): '''Ensure that the alarm broadcaster instantiates without errors''' broadcaster = AlarmBroadcaster() self.assertIsNotNone(broadcaster)
class Joystick(object): def __init__(self): self.force_scale = rospy.get_param("~force_scale", 600) self.torque_scale = rospy.get_param("~torque_scale", 500) self.wrench_choices = itertools.cycle(['rc', 'autonomous']) self.current_pose = Odometry() self.active = False self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name='kill', action_required=True, severity=0 ) self.station_hold = self.alarm_broadcaster.add_alarm( name='station_hold', action_required=False, severity=3 ) # self.docking_alarm = self.alarm_broadcaster.add_alarm( # name='docking', # action_required=True, # severity=0 # ) self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1) # rospy.wait_for_service('/change_wrench') self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) rospy.Subscriber("joy", Joy, self.joy_cb) self.reset() def reset(self): ''' Used to reset the state of the controller. Sometimes when it disconnects then comes back online, the settings are all out of wack. ''' self.last_change_mode = False self.last_station_hold_state = False self.last_kill = False self.last_rc = False self.last_auto = False self.start_count = 0 self.last_joy = None self.active = False self.killed = False # self.docking = False wrench = WrenchStamped() wrench.header.frame_id = "/base_link" wrench.wrench.force.x = 0 wrench.wrench.force.y = 0 wrench.wrench.torque.z = 0 self.wrench_pub.publish(wrench) def check_for_timeout(self, joy): if self.last_joy is None: self.last_joy = joy return if joy.axes == self.last_joy.axes and \ joy.buttons == self.last_joy.buttons: # No change in state if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60): # The controller times out after 15 minutes if self.active: rospy.logwarn("Controller Timed out. Hold start to resume.") self.reset() else: joy.header.stamp = rospy.Time.now() # In the sim, stamps weren't working right self.last_joy = joy def joy_cb(self, joy): self.check_for_timeout(joy) # Handle Button presses start = joy.buttons[7] change_mode = bool(joy.buttons[3]) # Y kill = bool(joy.buttons[2]) # X station_hold = bool(joy.buttons[0]) # A # docking = bool(joy.buttons[1]) # B rc_control = bool(joy.buttons[11]) # d-pad left auto_control = bool(joy.buttons[12]) # d-pad right # Reset controller state if only start is pressed down for awhile self.start_count += start if self.start_count > 10: # About 3 seconds rospy.loginfo("Resetting controller state.") self.reset() self.active = True self.kill_alarm.clear_alarm() self.wrench_changer("rc") if not self.active: return # Change vehicle mode if change_mode == 1 and change_mode != self.last_change_mode: mode = next(self.wrench_choices) rospy.loginfo("Changing Control Mode: {}".format(mode)) self.wrench_changer(mode) if rc_control == 1 and rc_control != self.last_rc: rospy.loginfo("Changing Control to RC") self.wrench_changer("rc") if auto_control == 1 and auto_control != self.last_auto: rospy.loginfo("Changing Control to Autonomous") self.wrench_changer("autonomous") # Station hold if station_hold == 1 and station_hold != self.last_station_hold_state: rospy.loginfo("Station Holding") self.station_hold.raise_alarm() self.wrench_changer("autonomous") # Turn on full system kill if kill == 1 and kill != self.last_kill: rospy.loginfo("Toggling Kill") if self.killed: self.kill_alarm.clear_alarm() else: self.wrench_changer("rc") self.kill_alarm.raise_alarm( problem_description='System kill from location: joystick' ) self.killed = not self.killed # # Turn on docking mode # if docking == 1 and docking != self.last_docking_state: # rospy.loginfo("Toggling Docking") # if self.docking: # self.docking_alarm.clear_alarm() # else: # self.docking_alarm.raise_alarm( # problem_description='Docking kill from location: joystick' # ) # self.docking = not self.docking self.last_start = start self.last_change_mode = change_mode self.last_kill = kill self.last_station_hold_state = station_hold # self.last_docking_state = docking self.last_auto_control = auto_control self.last_rc = rc_control self.last_auto = auto_control # Handle joystick commands left_stick_x = joy.axes[1] left_stick_y = joy.axes[0] right_stick_y = joy.axes[3] wrench = WrenchStamped() wrench.header.frame_id = "/base_link" wrench.header.stamp = joy.header.stamp wrench.wrench.force.x = self.force_scale * left_stick_x wrench.wrench.force.y = self.force_scale * left_stick_y wrench.wrench.torque.z = self.torque_scale * right_stick_y self.wrench_pub.publish(wrench)
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.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name="rc_kill", action_required=True, severity=0 ) self.station_hold_alarm = self.alarm_broadcaster.add_alarm( name="station_hold", action_required=False, severity=3 ) # rospy.wait_for_service("/change_wrench") self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.kill_listener = AlarmListener("rc_kill", 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 = not alarm.clear @_timeout_check def kill(self, *args, **kwargs): ''' Kills the system regardless of what state it is in. ''' rospy.loginfo("Killing") self.wrench_changer("rc") self.kill_alarm.raise_alarm( problem_description="System kill from location: {}".format(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.wrench_changer("rc") self.kill_alarm.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_alarm.clear_alarm() else: self.wrench_changer("rc") self.kill_alarm.raise_alarm( problem_description="System kill from location: {}".format(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_alarm.raise_alarm( problem_description="Request to station hold from: {}".format(self.name) ) rospy.sleep(0.2) self.station_hold_alarm.clear_alarm() @_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, port="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0", baud=9600): self.ser = serial.Serial(port=port, baudrate=baud, timeout=0.25) self.ser.flush() self.timeout = rospy.Duration(1) self.network_msg = None update_network = lambda msg: setattr(self, "network_msg", msg) self.network_listener = rospy.Subscriber("/keep_alive", Header, update_network) self.killstatus_pub = rospy.Publisher("/killstatus", KillStatus, queue_size=1) ab = AlarmBroadcaster() self.kill_alarm = ab.add_alarm( "hw_kill", problem_description="Hardware kill from a kill switch.") self.disconnect = ab.add_alarm("kill_system_disconnect") self.killed = False self.kill_status = { 'overall': False, 'PF': False, 'PA': False, 'SF': False, 'SA': False, 'computer': False, 'remote': False } # Dict of op-codes to functions that need to be run with each code for aysnc responses self.update_cbs = { '\x10': lambda: self.update('overall', True), '\x11': lambda: self.update('overall', False), '\x12': lambda: self.update('PF', True), '\x13': lambda: self.update('PF', False), '\x14': lambda: self.update('PA', True), '\x15': lambda: self.update('PA', False), '\x16': lambda: self.update('SF', True), '\x17': lambda: self.update('SF', False), '\x18': lambda: self.update('SA', True), '\x19': lambda: self.update('SA', False), '\x1A': lambda: self.update('remote', True), '\x1B': lambda: self.update('remote', False), '\x1C': lambda: self.update('computer', True), '\x1D': lambda: self.update('computer', False), '\x1E': self.disconnect.clear_alarm, '\x1F': self.disconnect.raise_alarm } # Initial check of kill status self.get_status() self.current_wrencher = '' _set_wrencher = lambda msg: setattr(self, 'current_wrencher', msg.data) rospy.Subscriber("/wrench/current", String, _set_wrencher) al = AlarmListener("kill", self.alarm_kill_cb) rospy.Timer(rospy.Duration(.5), self.get_status) while not rospy.is_shutdown(): self.control_check() while self.ser.inWaiting() > 0 and not rospy.is_shutdown(): self.check_buffer() if not self.network_kill(): self.ping() else: rospy.logerr("KILLBOARD: Network Kill!")
class RemoteControl(object): def __init__(self, controller_name, wrench_pub=None): self.name = controller_name self.wrench_choices = itertools.cycle(["rc", "keyboard", "autonomous"]) self.alarm_broadcaster = AlarmBroadcaster() self.kill_alarm = self.alarm_broadcaster.add_alarm( name="kill", action_required=True, severity=0 ) self.station_hold_alarm = self.alarm_broadcaster.add_alarm( name="station_hold", action_required=False, severity=3 ) # rospy.wait_for_service("/change_wrench") self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect) self.kill_listener = AlarmListener("kill", 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 = not alarm.clear @_timeout_check def kill(self, *args, **kwargs): ''' Kills the system regardless of what state it is in. ''' rospy.loginfo("Killing") self.wrench_changer("rc") self.kill_alarm.raise_alarm( problem_description="System kill from location: {}".format(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.wrench_changer("rc") self.kill_alarm.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_alarm.clear_alarm() else: self.wrench_changer("rc") self.kill_alarm.raise_alarm( problem_description="System kill from location: {}".format(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_alarm.raise_alarm( problem_description="Request to station hold from: {}".format(self.name) ) rospy.sleep(0.2) self.station_hold_alarm.clear_alarm() @_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") @_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)