class Kill(HandlerBase):
alarm_name = 'kill'
def __init__(self):
self._killed = False
self.initial_alarm = Alarm(self.alarm_name, True,
node_name='alarm_server',
problem_description='Initial kill')
self.bag_client = SimpleActionClient("/online_bagger/bag", BagOnlineAction)
self.task_client = MissionClient()
self.first = True
def _online_bagger_cb(self, status, result):
if status == 3:
rospy.loginfo('KILL BAG WRITTEN TO {}'.format(result.filename))
else:
rospy.logwarn('KILL BAG {}, status: {}'.format(TerminalState.to_string(status), result.status))
def _kill_task_cb(self, status, result):
if status == 3:
rospy.loginfo('Killed task success!')
return
rospy.logwarn('Killed task failed ({}): {}'.format(TerminalState.to_string(status), result.result))
def raised(self, alarm):
self._killed = True
if self.first:
self.first = False
return
if 'BAG_ALWAYS' not in os.environ or 'bag_kill' not in os.environ:
rospy.logwarn('BAG_ALWAYS or BAG_KILL not set. Not making kill bag.')
else:
goal = BagOnlineGoal(bag_name='kill.bag')
goal.topics = os.environ['BAG_ALWAYS'] + ' ' + os.environ['bag_kill']
self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb)
self.task_client.run_mission('Killed', done_cb=self._kill_task_cb)
def cleared(self, alarm):
self._killed = False
def meta_predicate(self, meta_alarm, alarms):
ignore = []
# Don't kill on low battery, only on critical
# if alarms['battery-voltage'].raised and alarms['battery-voltage'].severity < 2:
# ignore.append('battery-voltage')
# Raised if any alarms besides the two above are raised
raised = [name for name, alarm in alarms.items() if name not in ignore and alarm.raised]
if len(raised):
return Alarm('kill', True, node_name=rospy.get_name(),
problem_description='Killed by meta alarm(s) ' + ', '.join(raised),
parameters={'Raised': raised})
return self._killed
class RunMissionTest(unittest.TestCase):
def __init__(self, *args):
self.client = MissionClient()
self.client.wait_for_server()
super(RunMissionTest, self).__init__(*args)
def test_run_mission(self):
self.client.run_mission('PrintAndWait')
self.client.wait_for_result()
result = self.client.get_result()
state = self.client.get_state()
self.assertEqual(state, TerminalState.SUCCEEDED)
self.assertTrue(result.success)
self.assertEqual(result.parameters, '')
self.assertEqual(result.result, 'The darkness isnt so scary')
class RunMissionTest(unittest.TestCase):
def __init__(self, *args):
self.client = MissionClient()
self.client.wait_for_server()
super(RunMissionTest, self).__init__(*args)
def test_run_mission(self):
self.client.run_mission('PrintAndWait')
self.client.wait_for_result()
result = self.client.get_result()
state = self.client.get_state()
self.assertEqual(state, TerminalState.SUCCEEDED)
self.assertTrue(result.success)
self.assertEqual(result.parameters, '')
self.assertEqual(result.result, 'The darkness isnt so scary')
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.task_client = MissionClient()
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_mission('StationHold', done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass