dexcontrol 0.2.10__py3-none-any.whl → 0.3.0__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.

Potentially problematic release.


This version of dexcontrol might be problematic. Click here for more details.

Files changed (47) hide show
  1. dexcontrol/__init__.py +2 -0
  2. dexcontrol/config/core/arm.py +5 -1
  3. dexcontrol/config/core/chassis.py +9 -4
  4. dexcontrol/config/core/hand.py +2 -1
  5. dexcontrol/config/core/head.py +7 -8
  6. dexcontrol/config/core/misc.py +14 -1
  7. dexcontrol/config/core/torso.py +8 -4
  8. dexcontrol/config/sensors/cameras/__init__.py +2 -1
  9. dexcontrol/config/sensors/cameras/luxonis_camera.py +51 -0
  10. dexcontrol/config/sensors/cameras/rgb_camera.py +1 -1
  11. dexcontrol/config/sensors/cameras/zed_camera.py +2 -2
  12. dexcontrol/config/sensors/vega_sensors.py +9 -1
  13. dexcontrol/config/vega.py +34 -3
  14. dexcontrol/core/arm.py +103 -58
  15. dexcontrol/core/chassis.py +146 -115
  16. dexcontrol/core/component.py +83 -20
  17. dexcontrol/core/hand.py +74 -39
  18. dexcontrol/core/head.py +41 -28
  19. dexcontrol/core/misc.py +256 -25
  20. dexcontrol/core/robot_query_interface.py +440 -0
  21. dexcontrol/core/torso.py +28 -10
  22. dexcontrol/proto/dexcontrol_msg_pb2.py +27 -37
  23. dexcontrol/proto/dexcontrol_msg_pb2.pyi +111 -126
  24. dexcontrol/proto/dexcontrol_query_pb2.py +39 -35
  25. dexcontrol/proto/dexcontrol_query_pb2.pyi +41 -4
  26. dexcontrol/robot.py +266 -409
  27. dexcontrol/sensors/__init__.py +2 -1
  28. dexcontrol/sensors/camera/__init__.py +2 -0
  29. dexcontrol/sensors/camera/luxonis_camera.py +169 -0
  30. dexcontrol/sensors/camera/zed_camera.py +17 -8
  31. dexcontrol/sensors/imu/chassis_imu.py +5 -1
  32. dexcontrol/sensors/imu/zed_imu.py +3 -2
  33. dexcontrol/sensors/lidar/rplidar.py +1 -0
  34. dexcontrol/sensors/manager.py +3 -0
  35. dexcontrol/utils/constants.py +3 -0
  36. dexcontrol/utils/error_code.py +236 -0
  37. dexcontrol/utils/os_utils.py +183 -1
  38. dexcontrol/utils/pb_utils.py +0 -22
  39. dexcontrol/utils/subscribers/lidar.py +1 -0
  40. dexcontrol/utils/trajectory_utils.py +17 -5
  41. dexcontrol/utils/viz_utils.py +86 -11
  42. dexcontrol/utils/zenoh_utils.py +288 -2
  43. {dexcontrol-0.2.10.dist-info → dexcontrol-0.3.0.dist-info}/METADATA +15 -2
  44. dexcontrol-0.3.0.dist-info/RECORD +76 -0
  45. dexcontrol-0.2.10.dist-info/RECORD +0 -72
  46. {dexcontrol-0.2.10.dist-info → dexcontrol-0.3.0.dist-info}/WHEEL +0 -0
  47. {dexcontrol-0.2.10.dist-info → dexcontrol-0.3.0.dist-info}/licenses/LICENSE +0 -0
dexcontrol/core/hand.py CHANGED
@@ -14,17 +14,24 @@ This module provides the Hand class for controlling a robotic hand through Zenoh
14
14
  communication. It handles joint position control and state monitoring.
15
15
  """
16
16
 
17
- from typing import Any
17
+ from enum import Enum
18
+ from typing import Any, cast
18
19
 
19
20
  import numpy as np
20
21
  import zenoh
21
22
  from jaxtyping import Float
22
23
 
23
24
  from dexcontrol.config.core.hand import HandConfig
24
- from dexcontrol.core.component import RobotJointComponent
25
+ from dexcontrol.core.component import RobotComponent, RobotJointComponent
25
26
  from dexcontrol.proto import dexcontrol_msg_pb2
26
27
 
27
28
 
29
+ class HandType(Enum):
30
+ UNKNOWN = "UNKNOWN"
31
+ HandF5D6_V1 = "HandF5D6_V1"
32
+ HandF5D6_V2 = "HandF5D6_V2"
33
+
34
+
28
35
  class Hand(RobotJointComponent):
29
36
  """Robot hand control class.
30
37
 
@@ -36,6 +43,7 @@ class Hand(RobotJointComponent):
36
43
  self,
37
44
  configs: HandConfig,
38
45
  zenoh_session: zenoh.Session,
46
+ hand_type: HandType = HandType.HandF5D6_V1,
39
47
  ) -> None:
40
48
  """Initialize the hand controller.
41
49
 
@@ -47,7 +55,7 @@ class Hand(RobotJointComponent):
47
55
  super().__init__(
48
56
  state_sub_topic=configs.state_sub_topic,
49
57
  control_pub_topic=configs.control_pub_topic,
50
- state_message_type=dexcontrol_msg_pb2.HandState,
58
+ state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
51
59
  zenoh_session=zenoh_session,
52
60
  joint_name=configs.joint_name,
53
61
  )
@@ -64,45 +72,11 @@ class Hand(RobotJointComponent):
64
72
  Args:
65
73
  joint_pos: Joint positions as list or numpy array.
66
74
  """
67
- control_msg = dexcontrol_msg_pb2.HandCommand()
75
+ control_msg = dexcontrol_msg_pb2.MotorPosCommand()
68
76
  joint_pos_array = self._convert_joint_cmd_to_array(joint_pos)
69
- control_msg.joint_pos.extend(joint_pos_array.tolist())
77
+ control_msg.pos.extend(joint_pos_array.tolist())
70
78
  self._publish_control(control_msg)
71
79
 
72
- def set_joint_pos(
73
- self,
74
- joint_pos: Float[np.ndarray, " N"] | list[float] | dict[str, float],
75
- relative: bool = False,
76
- wait_time: float = 0.0,
77
- wait_kwargs: dict[str, float] | None = None,
78
- exit_on_reach: bool = False,
79
- exit_on_reach_kwargs: dict[str, Any] | None = None,
80
- ) -> None:
81
- """Send joint position control commands to the hand.
82
-
83
- Args:
84
- joint_pos: Joint positions as either:
85
- - List of joint values [j1, j2, ..., jN]
86
- - Numpy array with shape (N,)
87
- - Dictionary mapping joint names to position values
88
- relative: If True, the joint positions are relative to the current position.
89
- wait_time: Time to wait after setting the joint positions.
90
- wait_kwargs: Optional parameters for trajectory generation (not used in Hand).
91
- exit_on_reach: If True, the function will exit when the joint positions are reached.
92
- exit_on_reach_kwargs: Optional parameters for exit when the joint positions are reached.
93
-
94
- Raises:
95
- ValueError: If joint_pos dictionary contains invalid joint names.
96
- """
97
- super().set_joint_pos(
98
- joint_pos,
99
- relative,
100
- wait_time,
101
- wait_kwargs,
102
- exit_on_reach,
103
- exit_on_reach_kwargs,
104
- )
105
-
106
80
  def open_hand(
107
81
  self,
108
82
  wait_time: float = 0.0,
@@ -152,6 +126,37 @@ class HandF5D6(Hand):
152
126
  the F5D6 hand model.
153
127
  """
154
128
 
129
+ def __init__(
130
+ self,
131
+ configs: HandConfig,
132
+ zenoh_session: zenoh.Session,
133
+ hand_type: HandType = HandType.HandF5D6_V1,
134
+ ) -> None:
135
+ super().__init__(configs, zenoh_session)
136
+
137
+ # Initialize touch sensor for F5D6_V2 hands
138
+ self._hand_type = hand_type
139
+ if self._hand_type == HandType.HandF5D6_V2:
140
+ self._touch_sensor = HandF5D6TouchSensor(
141
+ configs.touch_sensor_sub_topic, zenoh_session
142
+ )
143
+ elif self._hand_type == HandType.HandF5D6_V1:
144
+ self._touch_sensor = None
145
+ else:
146
+ raise ValueError(f"Invalid hand type: {self._hand_type}")
147
+
148
+ def get_finger_tip_force(self) -> Float[np.ndarray, "5"]:
149
+ """Get the force at the finger tips.
150
+
151
+ Returns:
152
+ Array of force values at the finger tips.
153
+ """
154
+ if self._touch_sensor is None:
155
+ raise ValueError(
156
+ f"Touch sensor not available for this hand type: {self._hand_type}"
157
+ )
158
+ return self._touch_sensor.get_fingertip_touch_net_force()
159
+
155
160
  def close_hand(
156
161
  self,
157
162
  wait_time: float = 0.0,
@@ -241,3 +246,33 @@ class HandF5D6(Hand):
241
246
  0
242
247
  ] * (1 - ratio)
243
248
  return intermediate_pos
249
+
250
+
251
+ class HandF5D6TouchSensor(RobotComponent):
252
+ """Wrench sensor reader for the robot arm.
253
+
254
+ This class provides methods to read wrench sensor data through Zenoh communication.
255
+ """
256
+
257
+ def __init__(self, state_sub_topic: str, zenoh_session: zenoh.Session) -> None:
258
+ """Initialize the wrench sensor reader.
259
+
260
+ Args:
261
+ state_sub_topic: Topic to subscribe to for wrench sensor data.
262
+ zenoh_session: Active Zenoh communication session for message passing.
263
+ """
264
+ super().__init__(
265
+ state_sub_topic=state_sub_topic,
266
+ zenoh_session=zenoh_session,
267
+ state_message_type=dexcontrol_msg_pb2.HandTouchSensorState,
268
+ )
269
+
270
+ def get_fingertip_touch_net_force(self) -> Float[np.ndarray, "5"]:
271
+ """Get the complete wrench sensor state.
272
+
273
+ Returns:
274
+ Dictionary containing wrench values and button states.
275
+ """
276
+ state = self._get_state()
277
+ hand_touch_state = cast(dexcontrol_msg_pb2.HandTouchSensorState, state)
278
+ return np.array(hand_touch_state.force)
dexcontrol/core/head.py CHANGED
@@ -55,27 +55,26 @@ class Head(RobotJointComponent):
55
55
  super().__init__(
56
56
  state_sub_topic=configs.state_sub_topic,
57
57
  control_pub_topic=configs.control_pub_topic,
58
- state_message_type=dexcontrol_msg_pb2.HeadState,
58
+ state_message_type=dexcontrol_msg_pb2.MotorStateWithTorque,
59
59
  zenoh_session=zenoh_session,
60
60
  joint_name=configs.joint_name,
61
+ joint_limit=configs.joint_limit
62
+ if hasattr(configs, "joint_limit")
63
+ else None,
64
+ joint_vel_limit=configs.joint_vel_limit
65
+ if hasattr(configs, "joint_vel_limit")
66
+ else None,
61
67
  pose_pool=configs.pose_pool,
62
68
  )
63
69
 
64
70
  self.mode_querier: Final[zenoh.Querier] = zenoh_session.declare_querier(
65
71
  resolve_key_name(configs.set_mode_query), timeout=2.0
66
72
  )
67
- self.default_vel: Final[float] = configs.default_vel
68
- self.max_vel: Final[float] = configs.max_vel
69
- assert self.max_vel > self.default_vel, (
70
- "max_vel must be greater than default_vel"
71
- )
72
- self._joint_limit: Float[np.ndarray, "3 2"] = np.stack(
73
- [configs.joint_limit_lower, configs.joint_limit_upper], axis=1
74
- )
73
+ assert self._joint_vel_limit is not None, "joint_vel_limit is not set"
75
74
 
76
75
  def set_joint_pos(
77
76
  self,
78
- joint_pos: Float[np.ndarray, "2"] | list[float] | dict[str, float],
77
+ joint_pos: Float[np.ndarray, "3"] | list[float] | dict[str, float],
79
78
  relative: bool = False,
80
79
  wait_time: float = 0.0,
81
80
  wait_kwargs: dict[str, float] | None = None,
@@ -87,7 +86,7 @@ class Head(RobotJointComponent):
87
86
  Args:
88
87
  joint_pos: Joint positions as either:
89
88
  - List of joint values [j1, j2]
90
- - Numpy array with shape (2,), in radians
89
+ - Numpy array with shape (3,), in radians
91
90
  - Dictionary mapping joint names to position values
92
91
  relative: If True, the joint positions are relative to the current position.
93
92
  wait_time: Time to wait after sending command in seconds. If 0, returns
@@ -110,8 +109,8 @@ class Head(RobotJointComponent):
110
109
 
111
110
  def set_joint_pos_vel(
112
111
  self,
113
- joint_pos: Float[np.ndarray, "2"] | list[float] | dict[str, float],
114
- joint_vel: Float[np.ndarray, "2"]
112
+ joint_pos: Float[np.ndarray, "3"] | list[float] | dict[str, float],
113
+ joint_vel: Float[np.ndarray, "3"]
115
114
  | list[float]
116
115
  | dict[str, float]
117
116
  | float
@@ -126,11 +125,11 @@ class Head(RobotJointComponent):
126
125
  Args:
127
126
  joint_pos: Joint positions as either:
128
127
  - List of joint values [j1, j2]
129
- - Numpy array with shape (2,), in radians
128
+ - Numpy array with shape (3,), in radians
130
129
  - Dictionary mapping joint names to position values
131
130
  joint_vel: Optional joint velocities as either:
132
131
  - List of joint values [v1, v2]
133
- - Numpy array with shape (2,), in rad/s
132
+ - Numpy array with shape (3,), in rad/s
134
133
  - Dictionary mapping joint names to velocity values
135
134
  - Single float value to be applied to all joints
136
135
  If None, velocities are calculated based on default velocity setting.
@@ -155,10 +154,19 @@ class Head(RobotJointComponent):
155
154
  joint_pos = self._convert_joint_cmd_to_array(joint_pos)
156
155
  joint_vel = self._process_joint_velocities(joint_vel, joint_pos)
157
156
 
157
+ if self._joint_limit is not None:
158
+ joint_pos = np.clip(
159
+ joint_pos, self._joint_limit[:, 0], self._joint_limit[:, 1]
160
+ )
161
+ if self._joint_vel_limit is not None:
162
+ joint_vel = np.clip(
163
+ joint_vel, -self._joint_vel_limit, self._joint_vel_limit
164
+ )
165
+
158
166
  # Create and send control message
159
- control_msg = dexcontrol_msg_pb2.HeadCommand()
160
- control_msg.joint_pos.extend(joint_pos.tolist())
161
- control_msg.joint_vel.extend(joint_vel.tolist())
167
+ control_msg = dexcontrol_msg_pb2.MotorPosVelCommand()
168
+ control_msg.pos.extend(joint_pos.tolist())
169
+ control_msg.vel.extend(joint_vel.tolist())
162
170
  self._publish_control(control_msg)
163
171
 
164
172
  # Wait if specified
@@ -194,15 +202,16 @@ class Head(RobotJointComponent):
194
202
 
195
203
  for reply in replies:
196
204
  if reply.ok is not None and reply.ok.payload is not None:
197
- logger.info(reply.ok.payload.to_string())
205
+ # TODO: handle the reply message of head mode
206
+ pass
198
207
  time.sleep(0.5)
199
208
 
200
- def get_joint_limit(self) -> Float[np.ndarray, "3 2"]:
209
+ def get_joint_limit(self) -> Float[np.ndarray, "3 2"] | None:
201
210
  """Get the joint limits of the head.
202
211
 
203
212
  Returns:
204
213
  Array of joint limits with shape (3, 2), where the first column contains
205
- lower limits and the second column contains upper limits.
214
+ lower limits and the second column contains upper limits, or None if not configured.
206
215
  """
207
216
  return self._joint_limit
208
217
 
@@ -229,7 +238,7 @@ class Head(RobotJointComponent):
229
238
 
230
239
  def _process_joint_velocities(
231
240
  self,
232
- joint_vel: Float[np.ndarray, "2"]
241
+ joint_vel: Float[np.ndarray, "3"]
233
242
  | list[float]
234
243
  | dict[str, float]
235
244
  | float
@@ -251,14 +260,18 @@ class Head(RobotJointComponent):
251
260
  motion_norm = np.linalg.norm(joint_motion)
252
261
 
253
262
  if motion_norm < 1e-6: # Avoid division by zero
254
- return np.zeros(2, dtype=np.float32)
263
+ return np.zeros(3, dtype=np.float32)
255
264
 
256
- # Scale velocities by default velocity
257
- return (joint_motion / motion_norm) * self.default_vel
265
+ default_vel = (
266
+ 2.5 if self._joint_vel_limit is None else np.min(self._joint_vel_limit)
267
+ )
268
+ return (joint_motion / motion_norm) * default_vel
258
269
 
259
270
  if isinstance(joint_vel, (int, float)):
260
271
  # Single value - apply to all joints
261
- return np.full(2, joint_vel, dtype=np.float32)
272
+ return np.full(3, joint_vel, dtype=np.float32)
262
273
 
263
- # Convert to array and clip to max velocity
264
- return self._convert_joint_cmd_to_array(joint_vel, clip_value=self.max_vel)
274
+ # Convert to array and clip to velocity limits
275
+ return self._convert_joint_cmd_to_array(
276
+ joint_vel, clip_value=self._joint_vel_limit
277
+ )
dexcontrol/core/misc.py CHANGED
@@ -11,13 +11,14 @@
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
23
  import zenoh
23
24
  from google.protobuf.message import Message
@@ -95,6 +96,14 @@ class Battery(RobotComponent):
95
96
  - power: Power consumption in Watts
96
97
  """
97
98
  state = self._get_state()
99
+ if state is None:
100
+ return {
101
+ "percentage": 0.0,
102
+ "temperature": 0.0,
103
+ "current": 0.0,
104
+ "voltage": 0.0,
105
+ "power": 0.0,
106
+ }
98
107
  return {
99
108
  "percentage": float(state.percentage),
100
109
  "temperature": float(state.temperature),
@@ -111,6 +120,11 @@ class Battery(RobotComponent):
111
120
  table.add_column("Parameter", style="cyan")
112
121
  table.add_column("Value")
113
122
 
123
+ if state is None:
124
+ table.add_row("Status", "[red]No battery data available[/]")
125
+ self._console.print(table)
126
+ return
127
+
114
128
  battery_style = self._get_battery_level_style(state.percentage)
115
129
  table.add_row("Battery Level", f"[{battery_style}]{state.percentage:.1f}%[/]")
116
130
 
@@ -120,7 +134,7 @@ class Battery(RobotComponent):
120
134
  power = state.current * state.voltage
121
135
  power_style = self._get_power_style(power)
122
136
  table.add_row(
123
- "Power",
137
+ "Power Consumption",
124
138
  f"[{power_style}]{power:.2f}W[/] ([blue]{state.current:.2f}A[/] "
125
139
  f"× [blue]{state.voltage:.2f}V[/])",
126
140
  )
@@ -213,10 +227,14 @@ class EStop(RobotComponent):
213
227
  configs: EStop configuration containing subscription topics.
214
228
  zenoh_session: Active Zenoh session for communication.
215
229
  """
230
+ self._enabled = configs.enabled
216
231
  super().__init__(
217
232
  configs.state_sub_topic, zenoh_session, dexcontrol_msg_pb2.EStopState
218
233
  )
219
234
  self._estop_query_name = configs.estop_query_name
235
+ if not self._enabled:
236
+ logger.warning("EStop monitoring is DISABLED via configuration")
237
+ return
220
238
  self._shutdown_event = threading.Event()
221
239
  self._monitor_thread = threading.Thread(target=self._estop_monitor, daemon=True)
222
240
  self._monitor_thread.start()
@@ -264,18 +282,40 @@ class EStop(RobotComponent):
264
282
  - software_estop_enabled: Software EStop enabled
265
283
  """
266
284
  state = self._get_state()
285
+ state = cast(dexcontrol_msg_pb2.EStopState, state)
286
+ if state is None:
287
+ return {
288
+ "button_pressed": False,
289
+ "software_estop_enabled": False,
290
+ }
291
+ button_pressed = (
292
+ state.left_button_pressed
293
+ or state.right_button_pressed
294
+ or state.waist_button_pressed
295
+ or state.wireless_button_pressed
296
+ )
267
297
  return {
268
- "button_pressed": state.button_pressed,
298
+ "button_pressed": button_pressed,
269
299
  "software_estop_enabled": state.software_estop_enabled,
270
300
  }
271
301
 
272
302
  def is_button_pressed(self) -> bool:
273
303
  """Checks if the EStop button is pressed."""
274
- return self._get_state().button_pressed
304
+ state = self._get_state()
305
+ state = cast(dexcontrol_msg_pb2.EStopState, state)
306
+ button_pressed = (
307
+ state.left_button_pressed
308
+ or state.right_button_pressed
309
+ or state.waist_button_pressed
310
+ or state.wireless_button_pressed
311
+ )
312
+ return button_pressed
275
313
 
276
314
  def is_software_estop_enabled(self) -> bool:
277
315
  """Checks if the software EStop is enabled."""
278
- return self._get_state().software_estop_enabled
316
+ state = self._get_state()
317
+ state = cast(dexcontrol_msg_pb2.EStopState, state)
318
+ return state.software_estop_enabled
279
319
 
280
320
  def activate(self) -> None:
281
321
  """Activates the software emergency stop (E-Stop)."""
@@ -291,30 +331,29 @@ class EStop(RobotComponent):
291
331
 
292
332
  def shutdown(self) -> None:
293
333
  """Shuts down the EStop component and stops monitoring thread."""
294
- self._shutdown_event.set()
295
- if self._monitor_thread and self._monitor_thread.is_alive():
296
- self._monitor_thread.join(timeout=2.0) # Extended timeout
297
- if self._monitor_thread.is_alive():
298
- logger.warning("EStop monitor thread did not terminate cleanly")
334
+ if self._enabled:
335
+ self._shutdown_event.set()
336
+ if self._monitor_thread and self._monitor_thread.is_alive():
337
+ self._monitor_thread.join(timeout=2.0) # Extended timeout
338
+ if self._monitor_thread.is_alive():
339
+ logger.warning("EStop monitor thread did not terminate cleanly")
299
340
  super().shutdown()
300
341
 
301
342
  def show(self) -> None:
302
343
  """Displays the current EStop status as a formatted table with color indicators."""
303
- state = self._get_state()
304
-
305
344
  table = Table(title="E-Stop Status")
306
345
  table.add_column("Parameter", style="cyan")
307
346
  table.add_column("Value")
308
347
 
309
- button_style = "bold red" if state.button_pressed else "bold dark_green"
310
- table.add_row("Button Pressed", f"[{button_style}]{state.button_pressed}[/]")
348
+ button_pressed = self.is_button_pressed()
349
+ button_style = "bold red" if button_pressed else "bold dark_green"
350
+ table.add_row("Button Pressed", f"[{button_style}]{button_pressed}[/]")
311
351
 
312
- software_style = (
313
- "bold red" if state.software_estop_enabled else "bold dark_green"
314
- )
352
+ if_software_estop_enabled = self.is_software_estop_enabled()
353
+ software_style = "bold red" if if_software_estop_enabled else "bold dark_green"
315
354
  table.add_row(
316
355
  "Software E-Stop Enabled",
317
- f"[{software_style}]{state.software_estop_enabled}[/]",
356
+ f"[{software_style}]{if_software_estop_enabled}[/]",
318
357
  )
319
358
 
320
359
  console = Console()
@@ -387,9 +426,12 @@ class Heartbeat:
387
426
  Returns:
388
427
  Decoded heartbeat timestamp value in seconds.
389
428
  """
390
- timestamp = int.from_bytes(data.to_bytes(), byteorder="little")
391
- # convert it from microseconds to seconds
392
- return timestamp / 1000000.0
429
+ # Decode UTF-8 string and convert to float
430
+ # Publisher sends: str(self.timecount_now).encode() in milliseconds
431
+ timestamp_str = data.to_bytes().decode("utf-8")
432
+ timestamp_ms = float(timestamp_str)
433
+ # Convert from milliseconds to seconds
434
+ return timestamp_ms / 1000.0
393
435
 
394
436
  def _heartbeat_monitor(self) -> None:
395
437
  """Background thread that continuously monitors heartbeat signal."""
@@ -527,6 +569,52 @@ class Heartbeat:
527
569
  return False
528
570
  return self._subscriber.is_active()
529
571
 
572
+ @staticmethod
573
+ def _format_uptime(seconds: float) -> str:
574
+ """Convert seconds to human-readable uptime format with high resolution.
575
+
576
+ Args:
577
+ seconds: Total seconds of uptime.
578
+
579
+ Returns:
580
+ Human-readable string like "1mo 2d 3h 45m 12s 345ms".
581
+ """
582
+ # Calculate months (assuming 30 days per month)
583
+ months = int(seconds // (86400 * 30))
584
+ remaining = seconds % (86400 * 30)
585
+
586
+ # Calculate days
587
+ days = int(remaining // 86400)
588
+ remaining = remaining % 86400
589
+
590
+ # Calculate hours
591
+ hours = int(remaining // 3600)
592
+ remaining = remaining % 3600
593
+
594
+ # Calculate minutes
595
+ minutes = int(remaining // 60)
596
+ remaining = remaining % 60
597
+
598
+ # Calculate seconds and milliseconds
599
+ secs = int(remaining)
600
+ milliseconds = int((remaining - secs) * 1000)
601
+
602
+ parts = []
603
+ if months > 0:
604
+ parts.append(f"{months}mo")
605
+ if days > 0:
606
+ parts.append(f"{days}d")
607
+ if hours > 0:
608
+ parts.append(f"{hours}h")
609
+ if minutes > 0:
610
+ parts.append(f"{minutes}m")
611
+ if secs > 0:
612
+ parts.append(f"{secs}s")
613
+ if milliseconds > 0 or not parts:
614
+ parts.append(f"{milliseconds}ms")
615
+
616
+ return " ".join(parts)
617
+
530
618
  def shutdown(self) -> None:
531
619
  """Shuts down the heartbeat monitor and stops monitoring thread."""
532
620
  if not self._enabled:
@@ -567,12 +655,13 @@ class Heartbeat:
567
655
  active_style = "bold dark_green" if status["is_active"] else "bold red"
568
656
  table.add_row("Signal Active", f"[{active_style}]{status['is_active']}[/]")
569
657
 
570
- # Last heartbeat value
658
+ # Last heartbeat value (robot uptime)
571
659
  last_value = status["last_value"]
572
660
  if last_value is not None:
573
- table.add_row("Last Value", f"[blue]{last_value:.6f}s[/]")
661
+ uptime_str = self._format_uptime(last_value)
662
+ table.add_row("Robot Driver Uptime", f"[blue]{uptime_str}[/]")
574
663
  else:
575
- table.add_row("Last Value", "[red]No data[/]")
664
+ table.add_row("Robot Driver Uptime", "[red]No data[/]")
576
665
 
577
666
  # Time since last heartbeat
578
667
  time_since = status["time_since_last"]
@@ -590,3 +679,145 @@ class Heartbeat:
590
679
 
591
680
  console = Console()
592
681
  console.print(table)
682
+
683
+
684
+ class ServerLogSubscriber:
685
+ """Server log subscriber that monitors and displays server log messages.
686
+
687
+ This class subscribes to the "logs" topic and handles incoming log messages
688
+ from the robot server. It provides formatted display of server logs with
689
+ proper error handling and validation.
690
+
691
+ The server sends log information via the "logs" topic as JSON with format:
692
+ {"timestamp": "ISO8601", "message": "text", "source": "robot_server"}
693
+
694
+ Attributes:
695
+ _zenoh_session: Zenoh session for communication.
696
+ _log_subscriber: Zenoh subscriber for log messages.
697
+ """
698
+
699
+ def __init__(self, zenoh_session: zenoh.Session) -> None:
700
+ """Initialize the ServerLogSubscriber.
701
+
702
+ Args:
703
+ zenoh_session: Active Zenoh session for communication.
704
+ """
705
+ self._zenoh_session = zenoh_session
706
+ self._log_subscriber = None
707
+ self._initialize()
708
+
709
+ def _initialize(self) -> None:
710
+ """Initialize the log subscriber with error handling."""
711
+
712
+ def log_handler(sample):
713
+ """Handle incoming log messages from the server."""
714
+ if not self._is_valid_log_sample(sample):
715
+ return
716
+
717
+ try:
718
+ log_data = self._parse_log_payload(sample.payload)
719
+ if log_data:
720
+ self._display_server_log(log_data)
721
+ except Exception as e:
722
+ logger.warning(f"Failed to process server log: {e}")
723
+
724
+ try:
725
+ # Subscribe to server logs topic
726
+ self._log_subscriber = self._zenoh_session.declare_subscriber(
727
+ "logs", log_handler
728
+ )
729
+ logger.debug("Server log subscriber initialized")
730
+ except Exception as e:
731
+ logger.warning(f"Failed to initialize server log subscriber: {e}")
732
+ self._log_subscriber = None
733
+
734
+ def _is_valid_log_sample(self, sample) -> bool:
735
+ """Check if log sample is valid.
736
+
737
+ Args:
738
+ sample: Zenoh sample to validate.
739
+
740
+ Returns:
741
+ True if sample is valid, False otherwise.
742
+ """
743
+ if sample is None or sample.payload is None:
744
+ logger.debug("Received empty log sample")
745
+ return False
746
+ return True
747
+
748
+ def _parse_log_payload(self, payload) -> dict[str, str] | None:
749
+ """Parse log payload and return structured data.
750
+
751
+ Args:
752
+ payload: Raw payload from Zenoh sample.
753
+
754
+ Returns:
755
+ Parsed log data as dictionary or None if parsing fails.
756
+ """
757
+ try:
758
+ payload_str = payload.to_bytes().decode("utf-8")
759
+ if not payload_str.strip():
760
+ logger.debug("Received empty log payload")
761
+ return None
762
+
763
+ log_data = json.loads(payload_str)
764
+
765
+ if not isinstance(log_data, dict):
766
+ logger.warning(
767
+ f"Invalid log data format: expected dict, got {type(log_data)}"
768
+ )
769
+ return None
770
+
771
+ return log_data
772
+ except (json.JSONDecodeError, UnicodeDecodeError) as e:
773
+ logger.warning(f"Failed to parse log payload: {e}")
774
+ return None
775
+
776
+ def _display_server_log(self, log_data: dict[str, str]) -> None:
777
+ """Display formatted server log message.
778
+
779
+ Args:
780
+ log_data: Parsed log data dictionary.
781
+ """
782
+ # Extract log information with safe defaults
783
+ timestamp = log_data.get("timestamp", "")
784
+ message = log_data.get("message", "")
785
+ source = log_data.get("source", "unknown")
786
+
787
+ # Validate critical fields
788
+ if not message:
789
+ logger.debug("Received log with empty message")
790
+ return
791
+
792
+ # Log the server message with clear identification
793
+ logger.info(f"[SERVER_LOG] [{timestamp}] [{source}] {message}")
794
+
795
+ def is_active(self) -> bool:
796
+ """Check if the log subscriber is active.
797
+
798
+ Returns:
799
+ True if subscriber is active, False otherwise.
800
+ """
801
+ return self._log_subscriber is not None
802
+
803
+ def shutdown(self) -> None:
804
+ """Clean up the log subscriber and release resources."""
805
+ if self._log_subscriber is not None:
806
+ try:
807
+ self._log_subscriber.undeclare()
808
+ self._log_subscriber = None
809
+ except Exception as e:
810
+ logger.error(f"Error cleaning up log subscriber: {e}")
811
+
812
+ def get_status(self) -> dict[str, Any]:
813
+ """Get the current status of the log subscriber.
814
+
815
+ Returns:
816
+ Dictionary containing status information:
817
+ - is_active: Whether the subscriber is active
818
+ - topic: The topic being subscribed to
819
+ """
820
+ return {
821
+ "is_active": self.is_active(),
822
+ "topic": "logs",
823
+ }