HWP Gripper Updates (Alarm group and restart processes) (#737)

* Added alarm group query and the ability to remotely restart the beaglebone processes

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

* Added decoded messages for each alarm group

* Fix syntax errors

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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)
 

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()