Merge pull request #176 from Luos-io/rc_2.2.9

Pyluos version 2.2.9

.github/workflows/devpublish.yml

@@ -13,7 +13,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: [3.6, 3.7, 3.8]
+        python-version: [ '3.x' ]
 
     steps:
     - uses: actions/checkout@v2

.github/workflows/tests.yml

@@ -10,7 +10,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: [3.6, 3.7, 3.8]
+        python-version: [ '3.x' ]
 
     steps:
     - uses: actions/checkout@v2

pyluos/device.py

@@ -80,11 +80,13 @@ def __repr__(self):
             prefill = fill
         return s
 
+
 class Device(object):
     _heartbeat_timeout = 5  # in sec.
     _max_alias_length = 15
     _base_log_conf = os.path.join(os.path.dirname(__file__),
                                   'logging_conf.json')
+    _freedomLink = None
 
     def __init__(self, host,
                  IO=None,
@@ -136,6 +138,10 @@ def close(self):
                 print("Warning: device closed on timeout, background thread is still running.")
         self._io.close()
 
+    def link_to_freedomrobotics(self):
+        from .integration.freedomRobotics import FreedomLink
+        self._freedomLink = FreedomLink(self)
+
     def pause(self):
         self._pause = True
         time.sleep(1)
@@ -256,19 +262,25 @@ def _update(self, new_state):
             alias = new_state['dead_service']
             if hasattr(self, alias):
                 getattr(self, alias)._kill()
+            if (self._freedomLink != None):
+                self._freedomLink._kill(alias)
         if 'assert' in new_state.keys() :
             # A node assert, print assert informations
             if (('node_id' in new_state['assert']) and ('file' in new_state['assert']) and ('line' in new_state['assert'])):
                 s = "************************* ASSERT *************************\n"
                 s += "*  Node " + str(new_state['assert']['node_id']) + " assert in file " + new_state['assert']['file'] + " line " + str(new_state['assert']['line'])
                 s += "\n**********************************************************"
                 print (s)
+            if (self._freedomLink != None):
+                self._freedomLink._assert(alias)
         if 'services' not in new_state.keys():
             return
 
         for alias, mod in new_state['services'].items():
             if hasattr(self, alias):
                 getattr(self, alias)._update(mod)
+            if (self._freedomLink != None):
+                self._freedomLink._update(alias, mod)
 
         self._last_update = time.time()
 

pyluos/integration/__init__.py

@@ -0,0 +1 @@
+

pyluos/integration/freedomRobotics.py

@@ -0,0 +1,115 @@
+from freedomrobotics.link import Link
+
+import logging
+import time
+
+class FreedomLink(object):
+    def __init__(self, device):
+        self._link = Link("core", command_callback=self.callback)
+        self._delegate = device
+
+    def callback(msg):
+        print("I receive:" + str(msg) )
+        self._link.log("info", "I heard " + str(msg) )
+        # Topic represent services.
+        if hasattr(self._delegate, msg["topic"][1:]):
+            service = getattr(self._delegate, msg["topic"][1:])
+            print ("we have this service")
+            # We have this service.
+            if hasattr(service, msg["message"]):
+                service_data = getattr(service, msg["message"])
+                service_data = msg["message"][msg["message"]]
+
+    def _update(self, alias, new_state):
+        if 'io_state' in new_state:
+            self._link.message("/" + alias + "/io_state", \
+                    "sensor_msgs/Joy", \
+                    {"io_state": new_state['io_state']})
+        if 'temperature' in new_state:
+            self._link.message("/" + alias + "/temperature", \
+                    "sensor_msgs/Temperature", \
+                    {"temperature": new_state['temperature']})
+        if 'lux' in new_state:
+            self._link.message("/" + alias + "/lux", \
+                    "sensor_msgs/Illuminance", \
+                    {"illuminance": new_state['lux']})
+        if 'rot_position' in new_state:
+            self._link.message("/" + alias + "/rot_position", \
+                    "sensor_msgs/JointState", \
+                    {"position": new_state['rot_position'], "name":alias})
+        if 'trans_position' in new_state:
+            self._link.message("/" + alias + "/trans_position", \
+                    "sensor_msgs/Range", \
+                    {"range": new_state['trans_position']})
+        if 'rot_speed' in new_state:
+            self._link.message("/" + alias + "/rot_speed", \
+                    "sensor_msgs/JointState", \
+                    {"velocity": new_state['rot_speed'], "name":alias})
+        if 'trans_speed' in new_state:
+            self._link.message("/" + alias + "/trans_speed", \
+                    "sensor_msgs/JointState", \
+                    {"velocity": new_state['trans_speed'], "name":alias})
+        if 'force' in new_state:
+            self._link.message("/" + alias + "/force", \
+                    "sensor_msgs/JointState", \
+                    {"effort": new_state['force'], "name":alias})
+        if 'current' in new_state:
+            self._link.message("/" + alias + "/current", \
+                    "sensor_msgs/BatteryState", \
+                    {"current": new_state['current']})
+        if 'volt' in new_state:
+            self._link.message("/" + alias + "/volt", \
+                    "sensor_msgs/BatteryState", \
+                    {"voltage": new_state['volt']})
+        if 'quaternion' in new_state:
+            self._link.message("/" + alias + "/quaternion", \
+                    "geometry_msgs/Quaternion", \
+                    {"y": new_state['quaternion'][0],"x": new_state['quaternion'][1],"z": new_state['quaternion'][2],"w": new_state['quaternion'][3]})
+        if 'linear_accel' in new_state:
+            self._link.message("/" + alias + "/linear_accel", \
+                    "geometry_msgs/Accel", \
+                    {"linear": {"y": new_state['linear_accel'][0],"x": new_state['linear_accel'][1],"z": new_state['linear_accel'][2]}})
+        if 'accel' in new_state:
+            self._link.message("/" + alias + "/accel", \
+                    "geometry_msgs/Accel", \
+                    {"angular": {"y": new_state['accel'][0],"x": new_state['accel'][1],"z": new_state['accel'][2]}})
+        if 'gyro' in new_state:
+            self._link.message("/" + alias + "/gyro", \
+                    "geometry_msgs/Vector3", \
+                    {"y": new_state['gyro'][0],"x": new_state['gyro'][1],"z": new_state['gyro'][2]})
+        if 'euler' in new_state:
+            self._link.message("/" + alias + "/euler", \
+                    "geometry_msgs/Vector3", \
+                    {"y": new_state['euler'][0],"x": new_state['euler'][1],"z": new_state['euler'][2]})
+        if 'compass' in new_state:
+            self._link.message("/" + alias + "/compass", \
+                    "geometry_msgs/Vector3", \
+                    {"y": new_state['compass'][0],"x": new_state['compass'][1],"z": new_state['compass'][2]})
+        if 'gravity_vector' in new_state:
+            self._link.message("/" + alias + "/gravity", \
+                    "geometry_msgs/Vector3", \
+                    {"y": new_state['gravity_vector'][0],"x": new_state['gravity_vector'][1],"z": new_state['gravity_vector'][2]})
+
+# I don't know what to do with those ones :
+        # if 'rotational_matrix' in new_state:
+        #     self._rotational_matrix = new_state['rotational_matrix']
+        # if 'pedometer' in new_state:
+        #     self._pedometer = new_state['pedometer']
+        # if 'walk_time' in new_state:
+        #     self._walk_time = new_state['walk_time']
+
+        # if 'heading' in new_state:
+        #     self._heading = new_state['heading']
+        # if 'revision' in new_state:
+        #     self._firmware_revision = new_state['revision']
+        # if 'luos_revision' in new_state:
+        #     self._luos_revision = new_state['luos_revision']
+        # if 'luos_statistics' in new_state:
+        #     self._luos_statistics = new_state['luos_statistics']
+
+
+    def _kill(self, alias):
+        print ("service", alias, "have been excluded from the network due to no responses.")
+
+    def _assert(self, alias):
+        print ("service", alias, "assert.")

pyluos/io/__init__.py

@@ -16,7 +16,6 @@ def is_ready(self):
     def read(self, trials=5):
         try:
             data = self.recv()
-            return self.loads(data)
         except Exception as e:
             logging.getLogger(__name__).debug('Msg read failed: {}'.format(str(e)))
             if trials == 0:

pyluos/tools/bootloader.py

@@ -253,6 +253,30 @@ def erase_flash(device, topic, nodes_to_program):
                         print(u"\r\n  ╰> Flash memory of node ", response['node'], " erased.")
         state = device._poll_once()
 
+    # retry sending failed messages
+    for node in failed_nodes:
+        send_node_command(device, node, topic, BOOTLOADER_ERASE)
+        print(u"\r\n  ╰> Retry erase memory of node ", node)
+        init_time = time.time()
+        state = device._poll_once()
+        while len(failed_nodes):
+            if(time.time() - init_time > ERASE_TIMEOUT):
+                return_value = False
+                break
+
+            # check if it is a response message
+            if 'bootloader' in state:  
+                for response in state['bootloader']:
+                    if (response['response'] == BOOTLOADER_ERASE_RESP):
+                    # this node responded, delete it from the failed nodes list
+                        if response['node'] in failed_nodes:
+                            failed_nodes.remove(response['node'])
+                            print(u"\r\n  ╰> Flash memory of node ", response['node'], " erased.")
+            state = device._poll_once()
+
+    if not len(failed_nodes):
+        return_value = True
+
     waiting_bg.terminate()
     return return_value, failed_nodes
 
@@ -358,6 +382,32 @@ def send_frame_from_binary(device, topic, frame_size, file_offset, nodes_to_prog
                         failed_nodes.remove(response['node'])
         # wait for next message
         state = device._poll_once()
+
+    for node in failed_nodes:
+        # retry sending failed messages
+        send_data_node(device, node, BOOTLOADER_BIN_CHUNK, frame_size, data_bytes)
+        state = device._poll_once()
+        print(u"\r\n  ╰> Retry sending binary message to node ", node)
+        init_time = time.time()
+        while len(failed_nodes):
+            # check for timeout of nodes
+            if(time.time() - init_time > PROGRAM_TIMEOUT):
+                return_value = False
+                break
+
+            # check if it is a response message
+            if 'bootloader' in state:
+                for response in state['bootloader']:
+                    if (response['response'] == BOOTLOADER_BIN_CHUNK_RESP):
+                        # the node responsed, remove it for fails list
+                        if response['node'] in failed_nodes:
+                            failed_nodes.remove(response['node'])
+            # wait for next message
+            state = device._poll_once()
+
+    if not len(failed_nodes):
+        return_value = True
+
     return return_value, failed_nodes
 
 # *******************************************************************************
@@ -379,6 +429,25 @@ def send_data(device, topic, command, size, data):
     # send json command
     device._write(json.dumps(bootloader_cmd).encode() + '\n'.encode() + data)
 
+# *******************************************************************************
+# @brief send binary data with a header to a specific node
+# @param
+# @return None
+# *******************************************************************************
+def send_data_node(device, node, command, size, data):
+    # create a json file with the list of the nodes to program
+    bootloader_cmd = {
+        'bootloader': {
+            'command': {
+                'size': [size],
+                'type': command,
+                'node': node,
+            },
+        }
+    }
+    # send json command
+    device._write(json.dumps(bootloader_cmd).encode() + '\n'.encode() + data)
+
 # *******************************************************************************
 # @brief send the binary end command
 # @param
@@ -411,6 +480,31 @@ def send_binary_end(device, topic, nodes_to_program):
                         failed_nodes.remove(response['node'])
         state = device._poll_once()
 
+    for node in failed_nodes:
+        # retry sending failed messages
+        send_node_command(device, node, topic, BOOTLOADER_BIN_END)
+        print(u"\r\n  ╰> Retry sending end message to node ", node)
+        state = device._poll_once()
+        init_time = time.time()
+        while len(failed_nodes):
+            # check for timeout of nodes
+            if(time.time() - init_time > RESP_TIMEOUT):
+                return_value = False
+                break
+
+            # check if it is a response message
+            if 'bootloader' in state:
+                for response in state['bootloader']:
+                    if (response['response'] == BOOTLOADER_BIN_END_RESP):
+                        # the node responsed, remove it for fails list
+                        if response['node'] in failed_nodes:
+                            failed_nodes.remove(response['node'])
+            # wait for next message
+            state = device._poll_once()
+
+    if not len(failed_nodes):
+        return_value = True
+
     return return_value, failed_nodes
 
 # *******************************************************************************
@@ -475,6 +569,41 @@ def check_crc(device, topic, nodes_to_program):
                         return_value = False
         state = device._poll_once()
 
+    for node in failed_nodes:
+        # retry sending failed messages
+        send_node_command(device, node, topic, BOOTLOADER_CRC_TEST)
+        print(u"\r\n  ╰> Retry sending crc demand to node ", node)
+        state = device._poll_once()
+        init_time = time.time()
+        while len(failed_nodes):
+            # check for timeout of nodes
+            if(time.time() - init_time > RESP_TIMEOUT):
+                return_value = False
+                break
+
+            # check if it is a response message
+            if 'bootloader' in state:
+                for response in state['bootloader']:
+                    if (response['response'] == BOOTLOADER_CRC_RESP):
+                        source_crc = int.from_bytes(compute_crc(), byteorder='big')
+                        node_crc = response['crc_value']
+                        node_id = response['node']
+                            # crc properly received 
+                        if (source_crc == node_crc):
+                            print(u"  ╰> CRC test for node", node_id, " : OK.")
+                            if node_id in failed_nodes:
+                                timeout -= RESP_TIMEOUT
+                                failed_nodes.remove(node_id)
+                        else:
+                            # not a good crc
+                            print(u"  ╰> CRC test for node", node_id, ": NOK.")
+                            print(u"  ╰> waited :", hex(source_crc), ", received :", hex(node_crc))
+                            return_value = False
+            state = device._poll_once()
+
+    if not len(failed_nodes):
+        return_value = True
+
     return return_value, failed_nodes
 
 # *******************************************************************************

pyluos/version.py

@@ -1 +1 @@
-version = '2.2.8'
+version = '2.2.9'