dexcontrol 0.2.12__py3-none-any.whl → 0.3.4__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (60) hide show
  1. dexcontrol/__init__.py +17 -8
  2. dexcontrol/apps/dualsense_teleop_base.py +1 -1
  3. dexcontrol/comm/__init__.py +51 -0
  4. dexcontrol/comm/rtc.py +401 -0
  5. dexcontrol/comm/subscribers.py +329 -0
  6. dexcontrol/config/core/chassis.py +9 -4
  7. dexcontrol/config/core/hand.py +1 -0
  8. dexcontrol/config/sensors/cameras/__init__.py +1 -2
  9. dexcontrol/config/sensors/cameras/zed_camera.py +2 -2
  10. dexcontrol/config/sensors/vega_sensors.py +12 -18
  11. dexcontrol/config/vega.py +4 -1
  12. dexcontrol/core/arm.py +66 -42
  13. dexcontrol/core/chassis.py +142 -120
  14. dexcontrol/core/component.py +107 -58
  15. dexcontrol/core/hand.py +119 -86
  16. dexcontrol/core/head.py +22 -33
  17. dexcontrol/core/misc.py +331 -158
  18. dexcontrol/core/robot_query_interface.py +467 -0
  19. dexcontrol/core/torso.py +5 -9
  20. dexcontrol/robot.py +245 -574
  21. dexcontrol/sensors/__init__.py +1 -2
  22. dexcontrol/sensors/camera/__init__.py +0 -2
  23. dexcontrol/sensors/camera/base_camera.py +150 -0
  24. dexcontrol/sensors/camera/rgb_camera.py +68 -64
  25. dexcontrol/sensors/camera/zed_camera.py +140 -164
  26. dexcontrol/sensors/imu/chassis_imu.py +81 -62
  27. dexcontrol/sensors/imu/zed_imu.py +54 -43
  28. dexcontrol/sensors/lidar/rplidar.py +16 -20
  29. dexcontrol/sensors/manager.py +4 -14
  30. dexcontrol/sensors/ultrasonic.py +15 -28
  31. dexcontrol/utils/__init__.py +0 -11
  32. dexcontrol/utils/comm_helper.py +110 -0
  33. dexcontrol/utils/constants.py +1 -1
  34. dexcontrol/utils/error_code.py +2 -4
  35. dexcontrol/utils/os_utils.py +172 -4
  36. dexcontrol/utils/pb_utils.py +6 -28
  37. {dexcontrol-0.2.12.dist-info → dexcontrol-0.3.4.dist-info}/METADATA +16 -3
  38. dexcontrol-0.3.4.dist-info/RECORD +62 -0
  39. {dexcontrol-0.2.12.dist-info → dexcontrol-0.3.4.dist-info}/WHEEL +1 -1
  40. dexcontrol/config/sensors/cameras/luxonis_camera.py +0 -51
  41. dexcontrol/proto/dexcontrol_msg_pb2.py +0 -73
  42. dexcontrol/proto/dexcontrol_msg_pb2.pyi +0 -220
  43. dexcontrol/proto/dexcontrol_query_pb2.py +0 -77
  44. dexcontrol/proto/dexcontrol_query_pb2.pyi +0 -162
  45. dexcontrol/sensors/camera/luxonis_camera.py +0 -169
  46. dexcontrol/utils/motion_utils.py +0 -199
  47. dexcontrol/utils/rate_limiter.py +0 -172
  48. dexcontrol/utils/rtc_utils.py +0 -144
  49. dexcontrol/utils/subscribers/__init__.py +0 -52
  50. dexcontrol/utils/subscribers/base.py +0 -281
  51. dexcontrol/utils/subscribers/camera.py +0 -332
  52. dexcontrol/utils/subscribers/decoders.py +0 -88
  53. dexcontrol/utils/subscribers/generic.py +0 -110
  54. dexcontrol/utils/subscribers/imu.py +0 -175
  55. dexcontrol/utils/subscribers/lidar.py +0 -172
  56. dexcontrol/utils/subscribers/protobuf.py +0 -111
  57. dexcontrol/utils/subscribers/rtc.py +0 -316
  58. dexcontrol/utils/zenoh_utils.py +0 -122
  59. dexcontrol-0.2.12.dist-info/RECORD +0 -75
  60. {dexcontrol-0.2.12.dist-info → dexcontrol-0.3.4.dist-info}/licenses/LICENSE +0 -0
dexcontrol/core/misc.py CHANGED
@@ -11,15 +11,17 @@
11
11
  """Miscellaneous robot components module.
12
12
 
13
13
  This module provides classes for various auxiliary robot components such as Battery,
14
- EStop (emergency stop), and UltraSonicSensor.
14
+ EStop (emergency stop), ServerLogSubscriber, and UltraSonicSensor.
15
15
  """
16
16
 
17
+ import json
17
18
  import os
18
19
  import threading
19
20
  import time
20
- from typing import TypeVar
21
+ from typing import Any, TypeVar, cast
21
22
 
22
- import zenoh
23
+ from dexcomm import Subscriber, call_service
24
+ from dexcomm.serialization.protobuf import control_msg_pb2, control_query_pb2
23
25
  from google.protobuf.message import Message
24
26
  from loguru import logger
25
27
  from rich.console import Console
@@ -27,10 +29,8 @@ from rich.table import Table
27
29
 
28
30
  from dexcontrol.config.core import BatteryConfig, EStopConfig, HeartbeatConfig
29
31
  from dexcontrol.core.component import RobotComponent
30
- from dexcontrol.proto import dexcontrol_msg_pb2, dexcontrol_query_pb2
31
- from dexcontrol.utils.constants import DISABLE_HEARTBEAT_ENV_VAR
32
+ from dexcontrol.utils.comm_helper import get_zenoh_config_path
32
33
  from dexcontrol.utils.os_utils import resolve_key_name
33
- from dexcontrol.utils.subscribers.generic import GenericZenohSubscriber
34
34
 
35
35
  # Type variable for Message subclasses
36
36
  M = TypeVar("M", bound=Message)
@@ -49,16 +49,13 @@ class Battery(RobotComponent):
49
49
  _shutdown_event: Event to signal thread shutdown.
50
50
  """
51
51
 
52
- def __init__(self, configs: BatteryConfig, zenoh_session: zenoh.Session) -> None:
52
+ def __init__(self, configs: BatteryConfig) -> None:
53
53
  """Initialize the Battery component.
54
54
 
55
55
  Args:
56
56
  configs: Battery configuration containing subscription topics.
57
- zenoh_session: Active Zenoh session for communication.
58
57
  """
59
- super().__init__(
60
- configs.state_sub_topic, zenoh_session, dexcontrol_msg_pb2.BMSState
61
- )
58
+ super().__init__(configs.state_sub_topic, control_msg_pb2.BMSState)
62
59
  self._console = Console()
63
60
  self._shutdown_event = threading.Event()
64
61
  self._monitor_thread = threading.Thread(
@@ -133,7 +130,7 @@ class Battery(RobotComponent):
133
130
  power = state.current * state.voltage
134
131
  power_style = self._get_power_style(power)
135
132
  table.add_row(
136
- "Power",
133
+ "Power Consumption",
137
134
  f"[{power_style}]{power:.2f}W[/] ([blue]{state.current:.2f}A[/] "
138
135
  f"× [blue]{state.voltage:.2f}V[/])",
139
136
  )
@@ -218,18 +215,14 @@ class EStop(RobotComponent):
218
215
  def __init__(
219
216
  self,
220
217
  configs: EStopConfig,
221
- zenoh_session: zenoh.Session,
222
218
  ) -> None:
223
219
  """Initialize the EStop component.
224
220
 
225
221
  Args:
226
222
  configs: EStop configuration containing subscription topics.
227
- zenoh_session: Active Zenoh session for communication.
228
223
  """
229
224
  self._enabled = configs.enabled
230
- super().__init__(
231
- configs.state_sub_topic, zenoh_session, dexcontrol_msg_pb2.EStopState
232
- )
225
+ super().__init__(configs.state_sub_topic, control_msg_pb2.EStopState)
233
226
  self._estop_query_name = configs.estop_query_name
234
227
  if not self._enabled:
235
228
  logger.warning("EStop monitoring is DISABLED via configuration")
@@ -265,12 +258,16 @@ class EStop(RobotComponent):
265
258
  Args:
266
259
  enable: If True, activates the software E-Stop. If False, deactivates it.
267
260
  """
268
- query_msg = dexcontrol_query_pb2.SetEstop(enable=enable)
269
- self._zenoh_session.get(
261
+ query_msg = control_query_pb2.SetEstop(enable=enable)
262
+ call_service(
270
263
  resolve_key_name(self._estop_query_name),
271
- handler=lambda reply: logger.info(f"Set E-Stop to {enable}"),
272
- payload=query_msg.SerializeToString(),
264
+ request=query_msg,
265
+ timeout=0.05,
266
+ config=get_zenoh_config_path(),
267
+ request_serializer=lambda x: x.SerializeToString(),
268
+ response_deserializer=None,
273
269
  )
270
+ logger.info(f"Set E-Stop to {enable}")
274
271
 
275
272
  def get_status(self) -> dict[str, bool]:
276
273
  """Gets the current EStop state information.
@@ -281,25 +278,40 @@ class EStop(RobotComponent):
281
278
  - software_estop_enabled: Software EStop enabled
282
279
  """
283
280
  state = self._get_state()
281
+ state = cast(control_msg_pb2.EStopState, state)
284
282
  if state is None:
285
283
  return {
286
284
  "button_pressed": False,
287
285
  "software_estop_enabled": False,
288
286
  }
287
+ button_pressed = (
288
+ state.left_button_pressed
289
+ or state.right_button_pressed
290
+ or state.waist_button_pressed
291
+ or state.wireless_button_pressed
292
+ )
289
293
  return {
290
- "button_pressed": state.button_pressed,
294
+ "button_pressed": button_pressed,
291
295
  "software_estop_enabled": state.software_estop_enabled,
292
296
  }
293
297
 
294
298
  def is_button_pressed(self) -> bool:
295
299
  """Checks if the EStop button is pressed."""
296
300
  state = self._get_state()
297
- return state.button_pressed if state is not None else False
301
+ state = cast(control_msg_pb2.EStopState, state)
302
+ button_pressed = (
303
+ state.left_button_pressed
304
+ or state.right_button_pressed
305
+ or state.waist_button_pressed
306
+ or state.wireless_button_pressed
307
+ )
308
+ return button_pressed
298
309
 
299
310
  def is_software_estop_enabled(self) -> bool:
300
311
  """Checks if the software EStop is enabled."""
301
312
  state = self._get_state()
302
- return state.software_estop_enabled if state is not None else False
313
+ state = cast(control_msg_pb2.EStopState, state)
314
+ return state.software_estop_enabled
303
315
 
304
316
  def activate(self) -> None:
305
317
  """Activates the software emergency stop (E-Stop)."""
@@ -325,27 +337,19 @@ class EStop(RobotComponent):
325
337
 
326
338
  def show(self) -> None:
327
339
  """Displays the current EStop status as a formatted table with color indicators."""
328
- state = self._get_state()
329
-
330
340
  table = Table(title="E-Stop Status")
331
341
  table.add_column("Parameter", style="cyan")
332
342
  table.add_column("Value")
333
343
 
334
- if state is None:
335
- table.add_row("Status", "[red]No E-Stop data available[/]")
336
- console = Console()
337
- console.print(table)
338
- return
339
-
340
- button_style = "bold red" if state.button_pressed else "bold dark_green"
341
- table.add_row("Button Pressed", f"[{button_style}]{state.button_pressed}[/]")
344
+ button_pressed = self.is_button_pressed()
345
+ button_style = "bold red" if button_pressed else "bold dark_green"
346
+ table.add_row("Button Pressed", f"[{button_style}]{button_pressed}[/]")
342
347
 
343
- software_style = (
344
- "bold red" if state.software_estop_enabled else "bold dark_green"
345
- )
348
+ if_software_estop_enabled = self.is_software_estop_enabled()
349
+ software_style = "bold red" if if_software_estop_enabled else "bold dark_green"
346
350
  table.add_row(
347
351
  "Software E-Stop Enabled",
348
- f"[{software_style}]{state.software_estop_enabled}[/]",
352
+ f"[{software_style}]{if_software_estop_enabled}[/]",
349
353
  )
350
354
 
351
355
  console = Console()
@@ -372,138 +376,182 @@ class Heartbeat:
372
376
  def __init__(
373
377
  self,
374
378
  configs: HeartbeatConfig,
375
- zenoh_session: zenoh.Session,
376
379
  ) -> None:
377
380
  """Initialize the Heartbeat monitor.
378
381
 
379
382
  Args:
380
383
  configs: Heartbeat configuration containing topic and timeout settings.
381
- zenoh_session: Active Zenoh session for communication.
382
384
  """
383
385
  self._timeout_seconds = configs.timeout_seconds
384
386
  self._enabled = configs.enabled
385
387
  self._paused = False
386
388
  self._paused_lock = threading.Lock()
387
389
  self._shutdown_event = threading.Event()
388
- if not self._enabled:
389
- logger.info("Heartbeat monitoring is DISABLED via configuration")
390
- # Create a dummy subscriber that's never active
391
- self._subscriber = None
392
- self._monitor_thread = None
393
- return
394
390
 
395
- # Create a generic subscriber for the heartbeat topic
396
- self._subscriber = GenericZenohSubscriber(
397
- topic=configs.heartbeat_topic,
398
- zenoh_session=zenoh_session,
399
- decoder=self._decode_heartbeat,
400
- name="heartbeat_monitor",
401
- enable_fps_tracking=False,
391
+ # Always create the subscriber to monitor heartbeat, regardless of enabled state
392
+ def heartbeat_callback(data):
393
+ """Process heartbeat data and update internal state."""
394
+ try:
395
+ decoded_data = self._decode_heartbeat(data)
396
+ # Store the decoded heartbeat data for monitoring
397
+ self._latest_heartbeat_data = decoded_data
398
+ self._last_heartbeat_time = time.time()
399
+ except Exception as e:
400
+ logger.debug(f"Failed to process heartbeat data: {e}")
401
+
402
+ # Define a simple deserializer that just returns the raw data
403
+ def heartbeat_deserializer(data: bytes) -> bytes:
404
+ """Pass through deserializer for raw heartbeat data."""
405
+ return data
406
+
407
+ self._subscriber = Subscriber(
408
+ topic=resolve_key_name(configs.heartbeat_topic),
409
+ callback=heartbeat_callback,
410
+ deserializer=heartbeat_deserializer,
411
+ config=get_zenoh_config_path(),
402
412
  )
403
413
 
414
+ # Initialize tracking variables
415
+ self._latest_heartbeat_data = None
416
+ self._last_heartbeat_time = None
417
+
404
418
  # Start monitoring thread
405
419
  self._monitor_thread = threading.Thread(
406
420
  target=self._heartbeat_monitor, daemon=True
407
421
  )
408
422
  self._monitor_thread.start()
409
423
 
410
- logger.info(f"Heartbeat monitor started with {self._timeout_seconds}s timeout")
424
+ if not self._enabled:
425
+ logger.info(
426
+ "Heartbeat monitoring is DISABLED - will monitor but not exit on timeout"
427
+ )
428
+ else:
429
+ logger.info(
430
+ f"Heartbeat monitor started with {self._timeout_seconds}s timeout"
431
+ )
411
432
 
412
- def _decode_heartbeat(self, data: zenoh.ZBytes) -> float:
433
+ def _decode_heartbeat(self, data) -> float:
413
434
  """Decode heartbeat data from raw bytes.
414
435
 
415
436
  Args:
416
- data: Raw Zenoh bytes containing heartbeat value.
437
+ data: Raw bytes containing heartbeat value.
417
438
 
418
439
  Returns:
419
440
  Decoded heartbeat timestamp value in seconds.
420
441
  """
421
- # Decode UTF-8 string and convert to float
422
- # Publisher sends: str(self.timecount_now).encode() in milliseconds
423
- timestamp_str = data.to_bytes().decode("utf-8")
424
- timestamp_ms = float(timestamp_str)
425
- # Convert from milliseconds to seconds
426
- return timestamp_ms / 1000.0
442
+ try:
443
+ # Handle different data formats
444
+ if isinstance(data, bytes):
445
+ timestamp_str = data.decode("utf-8")
446
+ elif isinstance(data, str):
447
+ timestamp_str = data
448
+ else:
449
+ # If it's something else, try to convert to string
450
+ timestamp_str = str(data)
451
+
452
+ # Parse the timestamp (expected to be in milliseconds)
453
+ timestamp_ms = float(timestamp_str)
454
+ # Convert from milliseconds to seconds
455
+ return timestamp_ms / 1000.0
456
+ except (ValueError, AttributeError, UnicodeDecodeError) as e:
457
+ logger.debug(
458
+ f"Failed to decode heartbeat data: {e}, data type: {type(data)}"
459
+ )
460
+ raise
461
+
462
+ def _is_subscriber_active(self) -> bool:
463
+ """Check if the subscriber is active (has received data)."""
464
+ return self._last_heartbeat_time is not None
465
+
466
+ def _get_time_since_last_data(self) -> float | None:
467
+ """Get time since last heartbeat data was received."""
468
+ if self._last_heartbeat_time is None:
469
+ return None
470
+ return time.time() - self._last_heartbeat_time
471
+
472
+ def _get_latest_heartbeat_data(self) -> float | None:
473
+ """Get the latest heartbeat data."""
474
+ return self._latest_heartbeat_data
427
475
 
428
476
  def _heartbeat_monitor(self) -> None:
429
477
  """Background thread that continuously monitors heartbeat signal."""
430
- if not self._enabled or self._subscriber is None:
478
+ if self._subscriber is None:
431
479
  return
432
480
 
433
481
  while not self._shutdown_event.is_set():
434
482
  try:
435
- # Skip monitoring if paused
483
+ # Skip if paused
436
484
  with self._paused_lock:
437
- is_paused = self._paused
438
- if is_paused:
439
- self._shutdown_event.wait(0.1)
440
- continue
441
-
442
- # Check if fresh data is being received within the timeout period
443
- if self._subscriber.is_active():
444
- if not self._subscriber.is_data_fresh(self._timeout_seconds):
445
- time_since_fresh = self._subscriber.get_time_since_last_data()
446
- if time_since_fresh is not None:
447
- logger.critical(
448
- f"HEARTBEAT TIMEOUT! No fresh heartbeat data received for {time_since_fresh:.2f}s "
449
- f"(timeout: {self._timeout_seconds}s). Low-level controller may have failed. "
450
- "Exiting program immediately for safety."
451
- )
452
- else:
453
- logger.critical(
454
- f"HEARTBEAT TIMEOUT! No heartbeat data ever received "
455
- f"(timeout: {self._timeout_seconds}s). Low-level controller may have failed. "
456
- "Exiting program immediately for safety."
457
- )
458
- # Exit immediately for safety
459
- os._exit(1)
485
+ if self._paused:
486
+ self._shutdown_event.wait(0.1)
487
+ continue
488
+
489
+ # Check timeout
490
+ time_since_last = self._get_time_since_last_data()
491
+ if time_since_last and time_since_last > self._timeout_seconds:
492
+ self._handle_timeout(time_since_last)
460
493
 
461
494
  # Check every 50ms for responsive monitoring
462
495
  self._shutdown_event.wait(0.05)
463
496
 
464
497
  except Exception as e:
465
498
  logger.error(f"Heartbeat monitor error: {e}")
466
- # Continue monitoring even if there's an error
467
499
  self._shutdown_event.wait(0.1)
468
500
 
501
+ def _handle_timeout(self, time_since_last: float) -> None:
502
+ """Handle heartbeat timeout based on enabled state."""
503
+ if self._enabled:
504
+ logger.critical(
505
+ f"HEARTBEAT TIMEOUT! No fresh heartbeat data received for {time_since_last:.2f}s "
506
+ f"(timeout: {self._timeout_seconds}s). Low-level controller may have failed. "
507
+ "Exiting program immediately for safety."
508
+ )
509
+ os._exit(1)
510
+ else:
511
+ # Log warning only once per timeout period to avoid spam
512
+ if (
513
+ not hasattr(self, "_last_warning_time")
514
+ or time.time() - self._last_warning_time > self._timeout_seconds
515
+ ):
516
+ logger.warning(
517
+ f"Heartbeat timeout detected ({time_since_last:.2f}s > {self._timeout_seconds}s) "
518
+ "but exit is disabled"
519
+ )
520
+ self._last_warning_time = time.time()
521
+
469
522
  def pause(self) -> None:
470
523
  """Pause heartbeat monitoring temporarily.
471
524
 
472
525
  When paused, the heartbeat monitor will not check for timeouts or exit
473
526
  the program. This is useful for scenarios where you need to temporarily
474
527
  disable safety monitoring (e.g., during system maintenance or testing).
475
-
476
- Warning: Use with caution as this disables a critical safety mechanism.
477
528
  """
478
- if not self._enabled:
479
- logger.warning("Cannot pause heartbeat monitoring - it's already disabled")
480
- return
481
-
482
529
  with self._paused_lock:
530
+ if self._paused:
531
+ return
483
532
  self._paused = True
484
- logger.warning(
485
- "Heartbeat monitoring PAUSED - safety mechanism temporarily disabled"
486
- )
487
533
 
488
- def resume(self) -> None:
489
- """Resume heartbeat monitoring after being paused.
490
-
491
- This re-enables the heartbeat timeout checking that was temporarily
492
- disabled by pause(). The monitor will immediately start checking
493
- for fresh heartbeat data again.
494
- """
495
- if not self._enabled:
496
- logger.warning("Cannot resume heartbeat monitoring - it's disabled")
497
- return
534
+ if self._enabled:
535
+ logger.warning(
536
+ "Heartbeat monitoring PAUSED - safety mechanism temporarily disabled"
537
+ )
538
+ else:
539
+ logger.info("Heartbeat monitoring paused (exit already disabled)")
498
540
 
541
+ def resume(self) -> None:
542
+ """Resume heartbeat monitoring after being paused."""
499
543
  with self._paused_lock:
500
544
  if not self._paused:
501
- logger.info("Heartbeat monitoring is already active")
502
545
  return
503
546
  self._paused = False
504
- # sleep for some time to make sure the heartbeat subscriber is resumed
505
- time.sleep(self._timeout_seconds)
506
- logger.info("Heartbeat monitoring RESUMED - safety mechanism re-enabled")
547
+
548
+ # Wait briefly to allow fresh heartbeat data
549
+ time.sleep(0.1)
550
+
551
+ if self._enabled:
552
+ logger.info("Heartbeat monitoring RESUMED - safety mechanism re-enabled")
553
+ else:
554
+ logger.info("Heartbeat monitoring resumed (exit still disabled)")
507
555
 
508
556
  def is_paused(self) -> bool:
509
557
  """Check if heartbeat monitoring is currently paused.
@@ -514,7 +562,7 @@ class Heartbeat:
514
562
  with self._paused_lock:
515
563
  return self._paused
516
564
 
517
- def get_status(self) -> dict[str, bool | float | float | None]:
565
+ def get_status(self) -> dict[str, bool | float | None]:
518
566
  """Gets the current heartbeat status information.
519
567
 
520
568
  Returns:
@@ -526,28 +574,28 @@ class Heartbeat:
526
574
  - enabled: Whether heartbeat monitoring is enabled (bool)
527
575
  - paused: Whether heartbeat monitoring is paused (bool)
528
576
  """
529
- if not self._enabled or self._subscriber is None:
577
+ if self._subscriber is None:
530
578
  return {
531
579
  "is_active": False,
532
580
  "last_value": None,
533
581
  "time_since_last": None,
534
582
  "timeout_seconds": self._timeout_seconds,
535
- "enabled": False,
583
+ "enabled": self._enabled,
536
584
  "paused": False,
537
585
  }
538
586
 
539
- last_value = self._subscriber.get_latest_data()
540
- time_since_last = self._subscriber.get_time_since_last_data()
587
+ last_value = self._get_latest_heartbeat_data()
588
+ time_since_last = self._get_time_since_last_data()
541
589
 
542
590
  with self._paused_lock:
543
591
  paused = self._paused
544
592
 
545
593
  return {
546
- "is_active": self._subscriber.is_active(),
594
+ "is_active": self._is_subscriber_active(),
547
595
  "last_value": last_value,
548
596
  "time_since_last": time_since_last,
549
597
  "timeout_seconds": self._timeout_seconds,
550
- "enabled": True,
598
+ "enabled": self._enabled,
551
599
  "paused": paused,
552
600
  }
553
601
 
@@ -557,9 +605,9 @@ class Heartbeat:
557
605
  Returns:
558
606
  True if heartbeat is active, False otherwise.
559
607
  """
560
- if not self._enabled or self._subscriber is None:
608
+ if self._subscriber is None:
561
609
  return False
562
- return self._subscriber.is_active()
610
+ return self._is_subscriber_active()
563
611
 
564
612
  @staticmethod
565
613
  def _format_uptime(seconds: float) -> str:
@@ -609,8 +657,6 @@ class Heartbeat:
609
657
 
610
658
  def shutdown(self) -> None:
611
659
  """Shuts down the heartbeat monitor and stops monitoring thread."""
612
- if not self._enabled:
613
- return
614
660
  self._shutdown_event.set()
615
661
  if self._monitor_thread and self._monitor_thread.is_alive():
616
662
  self._monitor_thread.join(timeout=2.0) # Extended timeout
@@ -627,47 +673,174 @@ class Heartbeat:
627
673
  table.add_column("Parameter", style="cyan")
628
674
  table.add_column("Value")
629
675
 
630
- # Enabled status
631
- enabled = status.get("enabled", True)
632
- if not enabled:
633
- table.add_row("Status", "[yellow]DISABLED[/]")
634
- table.add_row(
635
- "Reason", f"[yellow]Disabled via {DISABLE_HEARTBEAT_ENV_VAR}[/]"
636
- )
637
- console = Console()
638
- console.print(table)
639
- return
676
+ # Mode: Enabled/Disabled and Paused state
677
+ mode_parts = []
678
+ if not status["enabled"]:
679
+ mode_parts.append("[yellow]Exit Disabled[/]")
680
+ if status["paused"]:
681
+ mode_parts.append("[yellow]Paused[/]")
682
+ if not mode_parts:
683
+ mode_parts.append("[green]Active[/]")
684
+ table.add_row("Mode", " | ".join(mode_parts))
685
+
686
+ # Signal status
687
+ active_style = "green" if status["is_active"] else "red"
688
+ table.add_row(
689
+ "Signal",
690
+ f"[{active_style}]{'Receiving' if status['is_active'] else 'No Signal'}[/]",
691
+ )
640
692
 
641
- # Paused status
642
- paused = status.get("paused", False)
643
- if paused:
644
- table.add_row("Status", "[yellow]PAUSED[/]")
645
- else:
646
- # Active status
647
- active_style = "bold dark_green" if status["is_active"] else "bold red"
648
- table.add_row("Signal Active", f"[{active_style}]{status['is_active']}[/]")
649
-
650
- # Last heartbeat value (robot uptime)
651
- last_value = status["last_value"]
652
- if last_value is not None:
653
- uptime_str = self._format_uptime(last_value)
654
- table.add_row("Robot server uptime", f"[blue]{uptime_str}[/]")
655
- else:
656
- table.add_row("Robot server uptime", "[red]No data[/]")
693
+ # Robot uptime
694
+ if status["last_value"] is not None:
695
+ uptime_str = self._format_uptime(status["last_value"])
696
+ table.add_row("Robot Uptime", f"[blue]{uptime_str}[/]")
657
697
 
658
698
  # Time since last heartbeat
659
- time_since = status["time_since_last"]
660
- timeout_seconds = status["timeout_seconds"]
661
- if time_since is not None and isinstance(timeout_seconds, (int, float)):
699
+ if status["time_since_last"] is not None:
700
+ time_since = float(status["time_since_last"])
701
+ timeout = status["timeout_seconds"]
702
+ timeout = float(timeout) if timeout is not None else 1.0
662
703
  time_style = (
663
- "bold red" if time_since > timeout_seconds * 0.8 else "bold dark_green"
704
+ "red"
705
+ if time_since > timeout
706
+ else "yellow"
707
+ if time_since > timeout * 0.5
708
+ else "green"
664
709
  )
665
- table.add_row("Time Since Last", f"[{time_style}]{time_since:.2f}s[/]")
666
- else:
667
- table.add_row("Time Since Last", "[yellow]N/A[/]")
710
+ table.add_row("Last Heartbeat", f"[{time_style}]{time_since:.1f}s ago[/]")
668
711
 
669
712
  # Timeout setting
670
- table.add_row("Timeout", f"[blue]{timeout_seconds}s[/]")
713
+ table.add_row("Timeout", f"[blue]{status['timeout_seconds']}s[/]")
671
714
 
672
- console = Console()
673
- console.print(table)
715
+ Console().print(table)
716
+
717
+
718
+ class ServerLogSubscriber:
719
+ """Server log subscriber that monitors and displays server log messages.
720
+
721
+ This class subscribes to the "logs" topic and handles incoming log messages
722
+ from the robot server. It provides formatted display of server logs with
723
+ proper error handling and validation.
724
+
725
+ The server sends log information via the "logs" topic as JSON with format:
726
+ {"timestamp": "ISO8601", "message": "text", "source": "robot_server"}
727
+
728
+ Attributes:
729
+ _zenoh_session: Zenoh session for communication.
730
+ _log_subscriber: Zenoh subscriber for log messages.
731
+ """
732
+
733
+ def __init__(self) -> None:
734
+ """Initialize the ServerLogSubscriber."""
735
+ # DexComm will handle the communication
736
+ self._log_subscriber = None
737
+ self._initialize()
738
+
739
+ def _initialize(self) -> None:
740
+ """Initialize the log subscriber with error handling."""
741
+
742
+ def log_handler(payload):
743
+ """Handle incoming log messages from the server."""
744
+ try:
745
+ log_data = self._parse_log_payload(payload)
746
+ if log_data:
747
+ self._display_server_log(log_data)
748
+ except Exception as e:
749
+ logger.warning(f"Failed to process server log: {e}")
750
+
751
+ try:
752
+ # Subscribe to server logs topic using DexComm
753
+ self._log_subscriber = Subscriber(
754
+ topic="logs", callback=log_handler, config=get_zenoh_config_path()
755
+ )
756
+ logger.debug("Server log subscriber initialized")
757
+ except Exception as e:
758
+ logger.warning(f"Failed to initialize server log subscriber: {e}")
759
+ self._log_subscriber = None
760
+
761
+ def _parse_log_payload(self, payload) -> dict[str, str] | None:
762
+ """Parse log payload and return structured data.
763
+
764
+ Args:
765
+ payload: Raw payload from Zenoh sample.
766
+
767
+ Returns:
768
+ Parsed log data as dictionary or None if parsing fails.
769
+ """
770
+ try:
771
+ if hasattr(payload, "to_bytes"):
772
+ # Handle zenoh-style payload
773
+ payload_str = payload.to_bytes().decode("utf-8")
774
+ else:
775
+ # Handle raw bytes from DexComm
776
+ payload_str = (
777
+ payload.decode("utf-8")
778
+ if isinstance(payload, bytes)
779
+ else str(payload)
780
+ )
781
+
782
+ if not payload_str.strip():
783
+ logger.debug("Received empty log payload")
784
+ return None
785
+
786
+ log_data = json.loads(payload_str)
787
+
788
+ if not isinstance(log_data, dict):
789
+ logger.warning(
790
+ f"Invalid log data format: expected dict, got {type(log_data)}"
791
+ )
792
+ return None
793
+
794
+ return log_data
795
+ except (json.JSONDecodeError, UnicodeDecodeError) as e:
796
+ logger.warning(f"Failed to parse log payload: {e}")
797
+ return None
798
+
799
+ def _display_server_log(self, log_data: dict[str, str]) -> None:
800
+ """Display formatted server log message.
801
+
802
+ Args:
803
+ log_data: Parsed log data dictionary.
804
+ """
805
+ # Extract log information with safe defaults
806
+ timestamp = log_data.get("timestamp", "")
807
+ message = log_data.get("message", "")
808
+ source = log_data.get("source", "unknown")
809
+
810
+ # Validate critical fields
811
+ if not message:
812
+ logger.debug("Received log with empty message")
813
+ return
814
+
815
+ # Log the server message with clear identification
816
+ logger.info(f"[SERVER_LOG] [{timestamp}] [{source}] {message}")
817
+
818
+ def is_active(self) -> bool:
819
+ """Check if the log subscriber is active.
820
+
821
+ Returns:
822
+ True if subscriber is active, False otherwise.
823
+ """
824
+ return self._log_subscriber is not None
825
+
826
+ def shutdown(self) -> None:
827
+ """Clean up the log subscriber and release resources."""
828
+ if self._log_subscriber is not None:
829
+ try:
830
+ self._log_subscriber.shutdown()
831
+ self._log_subscriber = None
832
+ except Exception as e:
833
+ logger.error(f"Error cleaning up log subscriber: {e}")
834
+
835
+ def get_status(self) -> dict[str, Any]:
836
+ """Get the current status of the log subscriber.
837
+
838
+ Returns:
839
+ Dictionary containing status information:
840
+ - is_active: Whether the subscriber is active
841
+ - topic: The topic being subscribed to
842
+ """
843
+ return {
844
+ "is_active": self.is_active(),
845
+ "topic": "logs",
846
+ }