Merge pull request #66 from sonelu/on-device

RangeRead in dynamixel and fix #63; additional testing

roboglia/base/bus.py

@@ -240,7 +240,7 @@ def write(self, reg, value):
             try:
                 self.__fp.write(text + '\n')
                 self.__fp.flush()
-            except Exception:
+            except Exception:           # pragma: no cover
                 logger.error(f'error executing write and flush to file '
                              f'for bus: {self.name}')
             logger.debug(f'FileBus "{self.name}" {text}')
@@ -284,7 +284,7 @@ def read(self, reg):
         try:
             self.__fp.write(text+'\n')
             self.__fp.flush()
-        except Exception:
+        except Exception:               # pragma: no cover
             logger.error(f'error executing write and flush to file '
                          f'for bus: {self.name}')
         logger.debug(f'FileBus "{self.name}" {text}')

roboglia/base/thread.py

@@ -289,10 +289,10 @@ def run(self):
                 wait_time = self.__period - (end_time - start_time) + adjust
                 if wait_time > 0:
                     time.sleep(wait_time)
-                else:
-                    logger.debug(f'Loop "{self.name}" took longer to run '
-                                 f'{end_time - start_time:.5f} than '
-                                 f'loop period {self.period:.5f}; check')
+                # else:
+                #     logger.debug(f'Loop "{self.name}" took longer to run '
+                #                  f'{end_time - start_time:.5f} than '
+                #                  f'loop period {self.period:.5f}; check')
                 # statistics:
                 exec_counts += 1
                 if exec_counts >= self.__frequency * self.__review:
@@ -302,12 +302,13 @@ def run(self):
                     diff = self.__period - exec_time / exec_counts
                     # fine tune the frequency
                     adjust += diff * self.__throttle
+                    if adjust < - self.__period:
+                        adjust = - self.__period
                     if actual_freq < (self.__frequency * self.__warning):
                         logger.warning(
                             f'Loop "{self.name}" running under '
                             f'warning threshold at {actual_freq:.2f}[Hz] '
                             f'({rate*100:.0f}%)')
-                        logger.warning(f'adjust={adjust}')
                     else:
                         logger.info(
                             f'Loop "{self.name}" running at '

roboglia/dynamixel/__init__.py

@@ -14,6 +14,7 @@
 from .sync import DynamixelSyncWriteLoop
 from .sync import DynamixelBulkReadLoop
 from .sync import DynamixelBulkWriteLoop
+from .sync import DynamixelRangeReadLoop
 
 register_class(DynamixelAXBaudRateRegister)
 register_class(DynamixelAXComplianceSlopeRegister)
@@ -29,3 +30,4 @@
 register_class(DynamixelSyncWriteLoop)
 register_class(DynamixelBulkReadLoop)
 register_class(DynamixelBulkWriteLoop)
+register_class(DynamixelRangeReadLoop)

roboglia/dynamixel/bus.py

@@ -112,7 +112,7 @@ def open(self):
             self.__port_handler = 'MockBus'
             self.__packet_handler = MockPacketHandler(self.protocol,
                                                       self.robot)
-        else:
+        else:               # pragma: no cover
             self.__port_handler = PortHandler(self.port)
             self.__port_handler.openPort()
             self.__port_handler.setBaudRate(self.baudrate)
@@ -127,9 +127,9 @@ def close(self):
         check if there is ok to close the bus and no other objects are
         using it."""
         if self.is_open:
-            if super().close():
+            if super().close():             # pragma: no branch
                 self.__packet_handler = None
-                if not self.__mock:
+                if not self.__mock:         # pragma: no cover
                     self.__port_handler.closePort()
                 self.__port_handler = None
                 logger.info(f'Bus "{self.name}" closed')
@@ -216,7 +216,7 @@ def read(self, reg):
             try:
                 res, cerr, derr = function(self.__port_handler,
                                            dev.dev_id, reg.address)
-            except Exception as e:
+            except Exception as e:          # pragma: no cover
                 logger.error(f'Exception raised while reading bus '
                              f'"{self.name}" device "{dev.name}" register '
                              f'"{reg.name}"')
@@ -279,7 +279,7 @@ def write(self, reg, value):
             try:
                 cerr, derr = function(self.__port_handler, dev.dev_id,
                                       reg.address, value)
-            except Exception as e:
+            except Exception as e:      # pragma: no cover
                 logger.error(f'Exception raised while writing bus '
                              f'"{self.name}" device "{dev.name}" register '
                              f'"{reg.name}"')
@@ -517,6 +517,49 @@ def readRx(self, port, dxl_id, length):
         self.__index += 1
         return device.register_low_endian(value, register.size), 0, 0
 
+    def readTxRx(self, port, dxl_id, address, length):
+        """Mocks a read package received. Used by RangeRead.
+        It will attempt to produce a response based on the data already
+        exiting in the registers. If the register is a read-only one, we
+        will add a random value between (-10, 10) to the exiting value and
+        then trim it to the ``min`` and ``max`` limits of the register. When
+        passing back the data, for registers that are more than 1 byte a
+        *low endian* conversion is executed (see
+        :py:meth:`DynamixelRegister.register_low_endian`).
+        """
+        if random.random() < self.__err:
+            logger.error('Mock exception raised by MockPacketHandler')
+            raise OSError
+        if random.random() < self.__err:
+            logger.error('Mock comm error raised by MockPacketHandler')
+            return [0], -3001, 0
+        device = self.__robot.device_by_id(dxl_id)
+        parsed_len = 0
+        res = []
+        register = None
+        while parsed_len < length:
+            while not register:
+                register = device.register_by_address(address)
+                if not register:
+                    address += 1
+                    parsed_len += 1
+                    res.append(0)
+            if register.access == 'RW':
+                value = register.int_value
+            else:
+                value = register.int_value + random.randint(-10, 10)
+                value = max(register.minim, min(register.maxim, value))
+            res.extend(device.register_low_endian(value, register.size))
+            address += register.size
+            parsed_len += register.size
+            register = None
+        if random.random() < self.__err:
+            logger.error('Mock device error raised by MockPacketHandler')
+            derr = 4        # overheat
+        else:
+            derr = 0
+        return res, 0, derr
+
     def bulkWriteTxOnly(self, port, param, param_length):
         """Simulate a BulkWrite transmit package. We return randomly an error
         or success."""

roboglia/dynamixel/sync.py

@@ -60,7 +60,7 @@ def atomic(self):
                     device.dev_id,
                     device.register_low_endian(register.int_value,
                                                register.size))
-                if not result:
+                if not result:      # pragma: no cover
                     logger.error(f'failed to setup SyncWrite for loop '
                                  f'{self.name} for device {device.name}')
             # execute write
@@ -112,7 +112,7 @@ def setup(self):
                                 register.size)
             for device in self.devices:
                 result = gsr.addParam(device.dev_id)
-                if result is not True:
+                if result is not True:          # pragma: no cover
                     logger.error(f'failed to setup SyncRead for loop '
                                  f'{self.name} for device {device.name}')
             self.gsrs.append(gsr)
@@ -190,7 +190,7 @@ def atomic(self):
                 result = gbw.addParam(device.dev_id, register.address,
                                       register.size,
                                       data)
-                if not result:
+                if not result:          # pragma: no cover
                     logger.error(f'failed to setup BulkWrite for loop '
                                  f'{self.name} for device {device.name}')
             # execute write
@@ -231,7 +231,7 @@ def setup(self):
                 register = getattr(device, reg_name)
                 result = gbr.addParam(device.dev_id, register.address,
                                       register.size)
-                if result is not True:
+                if result is not True:          # pragma: no cover
                     logger.error(f'failed to setup BulkRead for loop '
                                  f'{self.name} for device {device.name}')
             self.gbrs.append(gbr)
@@ -265,3 +265,82 @@ def atomic(self):
                         register.int_value = gbr.getData(device.dev_id,
                                                          register.address,
                                                          register.size)
+
+
+class DynamixelRangeReadLoop(BaseSync):
+    """Implements Read for a list of registers as specified in the frequency
+    parameter.
+
+    This method is provided as an alternative for AX devices that do not
+    support BulkRead or SyncRead and for which reading registers with
+    BaseReadSync would be extremely inefficient. With this method we still
+    have to send / recieive a communication packet for each device, but we
+    would get all the registers in one go.
+
+    The devices are provided in the `group` parameter and the registers
+    in the `registers` as a list of register names. The registers do not
+    need to be sequential.
+    It will update the `int_value` of each register in every device with
+    the result of the call.
+    Will raise exceptions if the BulkRead cannot be setup or fails to
+    execute.
+    """
+
+    def setup(self):
+        """Prepares to start the loop."""
+        self.start_address, self.length = self.get_register_range()
+
+    def atomic(self):
+        """Executes a RangeRead for all devices."""
+        # execute read
+        if not self.bus.can_use():
+            logger.error(f'Sync "{self.name}" '
+                         f'failed to acquire bus "{self.bus.name}"')
+            return
+
+        for device in self.devices:
+            # call the function
+            try:
+                res, cerr, derr = self.bus.packet_handler.readTxRx(
+                    self.bus.port_handler, device.dev_id,
+                    self.start_address, self.length)
+            except Exception as e:
+                logger.error(f'Exception raised while reading bus '
+                             f'"{self.name}" device "{device.name}"')
+                logger.error(str(e))
+                continue
+
+            # success call - log DEBUG
+            logger.debug(f'[RangeRead] dev={device.dev_id} '
+                         f'{res} (cerr={cerr}, derr={derr})')
+            # process result
+            if cerr != 0:
+                # communication error
+                err_desc = self.bus.packet_handler.getTxRxResult(cerr)
+                logger.error(f'[RangeRead "{self.name}"] '
+                             f'device "{device.name}", cerr={err_desc}')
+                continue
+
+            if derr != 0:
+                # device error
+                err_desc = self.bus.packet_handler.getRxPacketError(derr)
+                logger.warning(f'Device "{device.name}" responded with a '
+                               f'return error: {err_desc}')
+
+            # processs results
+            for reg_name in self.register_names:
+                reg = getattr(device, reg_name)
+                pos = reg.address - self.start_address
+                if reg.size == 1:
+                    value = res[pos]
+                elif reg.size == 2:
+                    value = res[pos] + res[pos + 1] * 256
+                elif reg.size == 4:
+                    value = res[pos] + res[pos + 1] * 256 + \
+                        res[pos + 2] * 65536 + \
+                        res[pos + 3] * 16777216
+                else:
+                    raise NotImplementedError
+                reg.int_value = value
+
+        self.bus.stop_using()       # !! as soon as possible

tests.py

@@ -620,6 +620,13 @@ def test_dynamixel_bulkread(self, mock_robot_init):
         time.sleep(1)
         robot.stop()
 
+    def test_dynamixel_rangeread(self, mock_robot_init):
+        robot = BaseRobot(**mock_robot_init['dynamixel'])
+        robot.start()
+        robot.syncs['rangeread'].start()
+        time.sleep(1)
+        robot.stop()
+
     def test_protocol1_syncread(self, mock_robot_init):
         mock_robot_init['dynamixel']['buses']['ttys1']['protocol'] = 1.0
         # we remove the bulkwrite so that the error will refer to syncread
@@ -810,7 +817,7 @@ def test_i2c_robot_bus_error(self, mock_robot_init, caplog):
         robot = BaseRobot(**mock_robot_init['i2crobot'])
         caplog.clear()
         robot.buses['i2c2'].open()
-        assert len(caplog.records) == 2
+        assert len(caplog.records) >= 2
         assert 'failed to open I2C bus' in caplog.text
         # caplog.clear()
         # robot.buses['i2c2'].close()

tests/dynamixel_robot.yml

@@ -57,3 +57,10 @@ dynamixel:
       registers: [present_voltage, present_temperature]
       frequency: 10.0
       auto: False
+
+    rangeread:
+      group: all_servos
+      class: DynamixelRangeReadLoop
+      registers: [present_position_deg, present_speed_rpm, present_voltage, present_temperature]
+      frequency: 10.0
+      auto: False