admin verbessert

src/imu_node.py

@@ -26,6 +26,7 @@ ROSBRIDGE_PORT = int(os.environ.get('IMU_ROSBRIDGE_PORT', '9090'))
 
 RUNNING = True
 WORKAROUND_APPLIED = False
+LAST_ERROR_TEXT = None
 
 
 def handle_shutdown(signum, frame):
@@ -40,6 +41,7 @@ def apply_bno08x_workaround():
     if WORKAROUND_APPLIED:
         return
 
+    adafruit_bno08x._dbg = lambda *args, **kwargs: None
     original_handle_packet = adafruit_bno08x.BNO08X._handle_packet
 
     def safe_handle_packet(self, packet):
@@ -49,6 +51,10 @@ def apply_bno08x_workaround():
             if len(exc.args) == 1 and isinstance(exc.args[0], int):
                 return
             raise
+        except IndexError as exc:
+            if 'list assignment index out of range' in str(exc):
+                return
+            raise
 
     adafruit_bno08x.BNO08X._handle_packet = safe_handle_packet
     WORKAROUND_APPLIED = True
@@ -107,6 +113,27 @@ def publish_status(topic, text):
     topic.publish(roslibpy.Message({'data': text}))
 
 
+def report_error(status_topic, text):
+    global LAST_ERROR_TEXT
+
+    if text == LAST_ERROR_TEXT:
+        return
+
+    LAST_ERROR_TEXT = text
+    print(text)
+    sys.stdout.flush()
+    if status_topic is not None:
+        try:
+            publish_status(status_topic, text)
+        except Exception:
+            pass
+
+
+def clear_error_state():
+    global LAST_ERROR_TEXT
+    LAST_ERROR_TEXT = None
+
+
 def publish_measurements(imu_topic, mag_topic, status_topic, sensor):
     stamp = ros_time_now()
     acceleration = sensor.acceleration
@@ -174,23 +201,22 @@ def main():
 
             if sensor is None:
                 sensor = create_sensor()
+                clear_error_state()
                 publish_status(status_topic, 'BNO085 initialisiert auf {0}'.format(ADDRESS))
                 print('BNO085 initialisiert')
                 sys.stdout.flush()
 
             publish_measurements(imu_topic, mag_topic, status_topic, sensor)
+            clear_error_state()
             time.sleep(sleep_seconds)
         except Exception as exc:
             message = 'IMU Fehler: {0}'.format(exc)
-            print(message)
-            sys.stdout.flush()
-            if status_topic is not None and ros_client is not None and ros_client.is_connected:
-                try:
-                    publish_status(status_topic, message)
-                except Exception:
-                    pass
+            report_error(status_topic if ros_client is not None and ros_client.is_connected else None, message)
             sensor = None
-            time.sleep(1.0)
+            if isinstance(exc, OSError) and getattr(exc, 'errno', None) == 5:
+                time.sleep(1.5)
+            else:
+                time.sleep(1.0)
 
     if ros_client is not None:
         try: