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.
- dexcontrol/__init__.py +2 -0
- dexcontrol/config/core/arm.py +5 -1
- dexcontrol/config/core/chassis.py +9 -4
- dexcontrol/config/core/hand.py +2 -1
- dexcontrol/config/core/head.py +7 -8
- dexcontrol/config/core/misc.py +14 -1
- dexcontrol/config/core/torso.py +8 -4
- dexcontrol/config/sensors/cameras/__init__.py +2 -1
- dexcontrol/config/sensors/cameras/luxonis_camera.py +51 -0
- dexcontrol/config/sensors/cameras/rgb_camera.py +1 -1
- dexcontrol/config/sensors/cameras/zed_camera.py +2 -2
- dexcontrol/config/sensors/vega_sensors.py +9 -1
- dexcontrol/config/vega.py +34 -3
- dexcontrol/core/arm.py +103 -58
- dexcontrol/core/chassis.py +146 -115
- dexcontrol/core/component.py +83 -20
- dexcontrol/core/hand.py +74 -39
- dexcontrol/core/head.py +41 -28
- dexcontrol/core/misc.py +256 -25
- dexcontrol/core/robot_query_interface.py +440 -0
- dexcontrol/core/torso.py +28 -10
- dexcontrol/proto/dexcontrol_msg_pb2.py +27 -37
- dexcontrol/proto/dexcontrol_msg_pb2.pyi +111 -126
- dexcontrol/proto/dexcontrol_query_pb2.py +39 -35
- dexcontrol/proto/dexcontrol_query_pb2.pyi +41 -4
- dexcontrol/robot.py +266 -409
- dexcontrol/sensors/__init__.py +2 -1
- dexcontrol/sensors/camera/__init__.py +2 -0
- dexcontrol/sensors/camera/luxonis_camera.py +169 -0
- dexcontrol/sensors/camera/zed_camera.py +17 -8
- dexcontrol/sensors/imu/chassis_imu.py +5 -1
- dexcontrol/sensors/imu/zed_imu.py +3 -2
- dexcontrol/sensors/lidar/rplidar.py +1 -0
- dexcontrol/sensors/manager.py +3 -0
- dexcontrol/utils/constants.py +3 -0
- dexcontrol/utils/error_code.py +236 -0
- dexcontrol/utils/os_utils.py +183 -1
- dexcontrol/utils/pb_utils.py +0 -22
- dexcontrol/utils/subscribers/lidar.py +1 -0
- dexcontrol/utils/trajectory_utils.py +17 -5
- dexcontrol/utils/viz_utils.py +86 -11
- dexcontrol/utils/zenoh_utils.py +288 -2
- {dexcontrol-0.2.10.dist-info → dexcontrol-0.3.0.dist-info}/METADATA +15 -2
- dexcontrol-0.3.0.dist-info/RECORD +76 -0
- dexcontrol-0.2.10.dist-info/RECORD +0 -72
- {dexcontrol-0.2.10.dist-info → dexcontrol-0.3.0.dist-info}/WHEEL +0 -0
- {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
|
|
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.
|
|
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.
|
|
75
|
+
control_msg = dexcontrol_msg_pb2.MotorPosCommand()
|
|
68
76
|
joint_pos_array = self._convert_joint_cmd_to_array(joint_pos)
|
|
69
|
-
control_msg.
|
|
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.
|
|
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.
|
|
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, "
|
|
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 (
|
|
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, "
|
|
114
|
-
joint_vel: Float[np.ndarray, "
|
|
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 (
|
|
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 (
|
|
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.
|
|
160
|
-
control_msg.
|
|
161
|
-
control_msg.
|
|
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
|
-
|
|
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, "
|
|
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(
|
|
263
|
+
return np.zeros(3, dtype=np.float32)
|
|
255
264
|
|
|
256
|
-
|
|
257
|
-
|
|
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(
|
|
272
|
+
return np.full(3, joint_vel, dtype=np.float32)
|
|
262
273
|
|
|
263
|
-
# Convert to array and clip to
|
|
264
|
-
return self._convert_joint_cmd_to_array(
|
|
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":
|
|
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
|
-
|
|
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
|
-
|
|
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.
|
|
295
|
-
|
|
296
|
-
self._monitor_thread.
|
|
297
|
-
|
|
298
|
-
|
|
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
|
-
|
|
310
|
-
|
|
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
|
-
|
|
313
|
-
|
|
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}]{
|
|
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
|
-
|
|
391
|
-
#
|
|
392
|
-
|
|
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
|
-
|
|
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("
|
|
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
|
+
}
|