diff --git a/socs/agents/hwp_gripper/agent.py b/socs/agents/hwp_gripper/agent.py index c2fdb0e34..c4d2c061c 100644 --- a/socs/agents/hwp_gripper/agent.py +++ b/socs/agents/hwp_gripper/agent.py @@ -41,6 +41,13 @@ def __init__(self, agent, args): self.client: Optional[cli.GripperClient] = None + self.decoded_alarm_group = {False: 'No alarm was detected', + 'A': 'Unrecognized error', + 'B': 'Controller saved parameter issue', + 'C': 'Issue with position calibration', + 'D': 'Could not reach position within time limit', + 'E': 'General controller erorr'} + agg_params = {'frame_length': 60} self.agent.register_feed('hwp_gripper', record=True, agg_params=agg_params) self.agent.register_feed('gripper_action', record=True) @@ -274,6 +281,43 @@ def alarm(self, session, params=None): session.data['response'] = return_dict return return_dict['result'], f"Success: {return_dict['result']}" + def alarm_group(self, session, params=None): + """alarm_group() + + **Task** - Queries the actuator controller alarm group + + Notes: + Return one of six values depending on the controller alarm state + False: No alarm was detected + 'A': Unrecognized error + 'B': Controller saved parameter issue + 'C': Issue with position calibration + 'D': Could not reach position within time limit + 'E': General controller error + + The most recent data collected is stored in session data in the + structure: + + >>> response.session['response'] + {'result': 'B', + 'log': ["ALARM GROUP B detected"]} + """ + state_return_dict = self._run_client_func( + self.client.get_state, job='get_state', check_shutdown=False) + + if not bool(state_return_dict['result']['jxc']['alarm']): + return_dict = {'result': False, 'log': ['Alarm not triggered']} + else: + return_dict = self._run_client_func( + self.client.alarm_group, job='alarm_group', check_shutdown=False) + + if return_dict['result'] is None: + return_dict['result'] = 'A' + + session.data['response'] = return_dict + session.data['decoded_alarm_group'] = self.decoded_alarm_group[return_dict['result']] + return return_dict['result'], f"Success: {return_dict['result']}" + def reset(self, session, params=None): """reset() **Task** - Resets the current active controller alarm @@ -633,6 +677,26 @@ def cancel_shutdown(self, session, params=None): self.shutdown_mode = False return True, 'Cancelled shutdown mode' + def restart(self, session, params=None): + """restart() + + **Task** - Restarts the beaglebone processes and the socket connection + + Notes: + The most recent data collected is stored in session data in the + structure: + + >>> response.session['response'] + {'result': True, + 'log': ["Previous connection closed", + "Restart command response: Success", + "Control socket reconnected"]} + """ + return_dict = self._run_client_func( + self.client.restart, job='restart', check_shutdown=False) + session.data['response'] = return_dict + return return_dict['result'], f"Success: {return_dict['result']}" + def monitor_state(self, session, params=None): """monitor_state() @@ -652,6 +716,7 @@ def monitor_state(self, session, params=None): 'jxc_inp': False, 'jxc_svre': False, 'jxc_alarm': False, + 'jxc_alarm_message': 'No alarm was detected', 'jxc_out': 0, 'act{axis}_pos': 0, 'act{axis}_limit_warm_grip_state': False, @@ -692,6 +757,22 @@ def monitor_state(self, session, params=None): 'jxc_out': int(state['jxc']['out']), }) + alarm_group_mapping = {4: 'B', + 2: 'C', + 1: 'D', + 0: 'E'} + + if bool(state['jxc']['alarm']): + out_value = int(state['jxc']['out']) % 16 + if out_value in alarm_group_mapping.keys(): + alarm_message = self.decoded_alarm_group[alarm_group_mapping] + else: + alarm_message = self.decoded_alarm_group['A'] + else: + alarm_message = self.decoded_alarm_group[False] + + data.update({'jxc_alarm_message': alarm_message}) + for act in state['actuators']: axis = act['axis'] data.update({ @@ -851,6 +932,7 @@ def main(args=None): agent.register_task('home', gripper_agent.home) agent.register_task('inp', gripper_agent.inp) agent.register_task('alarm', gripper_agent.alarm) + agent.register_task('alarm_group', gripper_agent.alarm_group) agent.register_task('reset', gripper_agent.reset) agent.register_task('act', gripper_agent.act) agent.register_task('is_cold', gripper_agent.is_cold) @@ -859,6 +941,7 @@ def main(args=None): agent.register_task('grip', gripper_agent.grip) agent.register_task('ungrip', gripper_agent.ungrip) agent.register_task('cancel_shutdown', gripper_agent.cancel_shutdown) + agent.register_task('restart', gripper_agent.restart) runner.run(agent, auto_reconnect=True) diff --git a/socs/agents/hwp_gripper/drivers/gripper_client.py b/socs/agents/hwp_gripper/drivers/gripper_client.py index 2b1c0b5c1..cd92400db 100644 --- a/socs/agents/hwp_gripper/drivers/gripper_client.py +++ b/socs/agents/hwp_gripper/drivers/gripper_client.py @@ -1,16 +1,20 @@ import pickle as pkl import socket +import time class GripperClient(object): def __init__(self, ip, port): self.ip = ip self.port = port - self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.s.connect((self.ip, self.port)) - + self.control_socket = self._init_connection(self.ip, self.port) self.data = b'' + def _init_connection(self, ip, port): + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.connect((ip, port)) + return s + def power(self, state): if not isinstance(state, bool): return False @@ -76,6 +80,9 @@ def emg(self, state, actuator=0): def alarm(self): return self.send_command('ALARM') + def alarm_group(self): + return self.send_command('ALARM_GROUP') + def reset(self): return self.send_command('RESET') @@ -105,12 +112,34 @@ def get_state(self): def send_command(self, command): if isinstance(command, str): - self.s.sendall(bytes(command, 'utf-8')) + self.control_socket.sendall(bytes(command, 'utf-8')) elif isinstance(command, bytes): - self.s.sendall(command) + self.control_socket.sendall(command) + + return pkl.loads(self.control_socket.recv(4096)) + + def restart(self): + log = [] + try: + self.close() + log.append('Previous connection closed') + except BaseException: + log.append('Previous connection already closed') + + _restart_socket = self._init_connection(self.ip, 5656) + _restart_socket.sendall(('reset\n').encode()) + time.sleep(0.5) + + resp = _restart_socket.recv(4096).decode().strip() + log.append(f'Restart command response: {resp}') + result = True if resp == 'Success' else False + _restart_socket.close() + time.sleep(10) - return pkl.loads(self.s.recv(4096)) + self.control_socket = self._init_connection(self.ip, self.port) + log.append('Control socket reconnected') + return {'result': result, 'log': log} def close(self): """Close socket connection""" - self.s.close() + self.control_socket.close()