petal-user-journey-coordinator 0.1.6__tar.gz → 0.1.8__tar.gz
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.
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/PKG-INFO +1 -1
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/pyproject.toml +1 -1
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/src/petal_user_journey_coordinator/controllers.py +83 -96
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/src/petal_user_journey_coordinator/plugin.py +94 -48
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/README.md +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/src/petal_user_journey_coordinator/__init__.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/src/petal_user_journey_coordinator/data_model.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/__init__.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/dummy_trajectory_test.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/fix_expected_results.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/generate_trajectory_test_data.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/generated_trajectory_test_data.json +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_generated_trajectories.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_petal_user_journey_coordinator.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_plotting.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_trajectory_simple.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_trajectory_verification.py +0 -0
- {petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/trajectory_test_data.json +0 -0
|
@@ -810,22 +810,10 @@ class ESCCalibrationController(BaseTimeoutController):
|
|
|
810
810
|
self.logger.error(f"[{message_id}] Unknown throttle type: {throttle_type}")
|
|
811
811
|
return False
|
|
812
812
|
|
|
813
|
-
|
|
814
|
-
|
|
815
|
-
|
|
816
|
-
|
|
817
|
-
command_msg = self.mavlink_proxy.master.mav.command_long_encode(
|
|
818
|
-
1, # TODO: investigate best practice
|
|
819
|
-
1, # TODO: investigate best practice
|
|
820
|
-
mavutil.mavlink.MAV_CMD_ACTUATOR_TEST,
|
|
821
|
-
0, # confirmation
|
|
822
|
-
param1, # Motor value (0-1 or NaN)
|
|
823
|
-
timeout, # Timeout in seconds
|
|
824
|
-
0, # Reserved
|
|
825
|
-
0, # Reserved
|
|
826
|
-
param5, # Motor mapping (110x)
|
|
827
|
-
0, # Reserved
|
|
828
|
-
0 # Reserved
|
|
813
|
+
command_msg = self.mavlink_proxy.build_motor_value_command(
|
|
814
|
+
motor_idx=motor_idx,
|
|
815
|
+
motor_value=param1,
|
|
816
|
+
timeout=timeout
|
|
829
817
|
)
|
|
830
818
|
|
|
831
819
|
# Send the command
|
|
@@ -833,7 +821,7 @@ class ESCCalibrationController(BaseTimeoutController):
|
|
|
833
821
|
|
|
834
822
|
action = "maximum throttle" if throttle_type == "maximum" else \
|
|
835
823
|
"minimum throttle" if throttle_type == "minimum" else "stopped"
|
|
836
|
-
self.logger.info(f"[{message_id}] Motor {motor_idx}: {action} (
|
|
824
|
+
self.logger.info(f"[{message_id}] Motor {motor_idx}: {action} (motor value={param1}, timeout={timeout}s)")
|
|
837
825
|
|
|
838
826
|
# Small delay between motor commands
|
|
839
827
|
await asyncio.sleep(0.1)
|
|
@@ -1019,22 +1007,11 @@ class ESCForceRunAllController(BaseTimeoutController):
|
|
|
1019
1007
|
"""Send MAV_CMD_ACTUATOR_TEST command to all motors."""
|
|
1020
1008
|
try:
|
|
1021
1009
|
for motor_idx in range(1, self.motor_count + 1):
|
|
1022
|
-
|
|
1023
|
-
|
|
1024
|
-
|
|
1025
|
-
|
|
1026
|
-
|
|
1027
|
-
1, # TODO: investigate best practice
|
|
1028
|
-
1, # TODO: investigate best practice
|
|
1029
|
-
mavutil.mavlink.MAV_CMD_ACTUATOR_TEST,
|
|
1030
|
-
0, # confirmation
|
|
1031
|
-
motor_value, # Motor value (0-1 or NaN)
|
|
1032
|
-
timeout, # Timeout in seconds
|
|
1033
|
-
0, # Reserved
|
|
1034
|
-
0, # Reserved
|
|
1035
|
-
param5, # Motor mapping (110x)
|
|
1036
|
-
0, # Reserved
|
|
1037
|
-
0 # Reserved
|
|
1010
|
+
|
|
1011
|
+
command_msg = self.mavlink_proxy.build_motor_value_command(
|
|
1012
|
+
motor_idx=motor_idx,
|
|
1013
|
+
motor_value=motor_value,
|
|
1014
|
+
timeout=timeout
|
|
1038
1015
|
)
|
|
1039
1016
|
|
|
1040
1017
|
# Send the command
|
|
@@ -1056,21 +1033,11 @@ class ESCForceRunAllController(BaseTimeoutController):
|
|
|
1056
1033
|
nan_value = float('nan')
|
|
1057
1034
|
|
|
1058
1035
|
for motor_idx in range(1, self.motor_count + 1):
|
|
1059
|
-
|
|
1060
|
-
|
|
1061
|
-
|
|
1062
|
-
|
|
1063
|
-
|
|
1064
|
-
1, # TODO: investigate best practice
|
|
1065
|
-
mavutil.mavlink.MAV_CMD_ACTUATOR_TEST,
|
|
1066
|
-
0, # confirmation
|
|
1067
|
-
nan_value, # Motor value (0-1 or NaN)
|
|
1068
|
-
0.0, # Timeout in seconds
|
|
1069
|
-
0, # Reserved
|
|
1070
|
-
0, # Reserved
|
|
1071
|
-
param5, # Motor mapping (110x)
|
|
1072
|
-
0, # Reserved
|
|
1073
|
-
0 # Reserved
|
|
1036
|
+
|
|
1037
|
+
command_msg = self.mavlink_proxy.build_motor_value_command(
|
|
1038
|
+
motor_idx=motor_idx,
|
|
1039
|
+
motor_value=nan_value,
|
|
1040
|
+
timeout=0.0
|
|
1074
1041
|
)
|
|
1075
1042
|
|
|
1076
1043
|
# Send the command
|
|
@@ -1250,22 +1217,11 @@ class ESCForceRunSingleController(BaseTimeoutController):
|
|
|
1250
1217
|
async def _send_actuator_test_single_motor(self, motor_value: float, timeout: float) -> bool:
|
|
1251
1218
|
"""Send MAV_CMD_ACTUATOR_TEST command to single motor."""
|
|
1252
1219
|
try:
|
|
1253
|
-
|
|
1254
|
-
|
|
1255
|
-
|
|
1256
|
-
|
|
1257
|
-
|
|
1258
|
-
1, # TODO: investigate best practice
|
|
1259
|
-
1, # TODO: investigate best practice
|
|
1260
|
-
mavutil.mavlink.MAV_CMD_ACTUATOR_TEST,
|
|
1261
|
-
0, # confirmation
|
|
1262
|
-
motor_value, # Motor value (0-1 or NaN)
|
|
1263
|
-
timeout, # Timeout in seconds
|
|
1264
|
-
0, # Reserved
|
|
1265
|
-
0, # Reserved
|
|
1266
|
-
param5, # Motor mapping (110x)
|
|
1267
|
-
0, # Reserved
|
|
1268
|
-
0 # Reserved
|
|
1220
|
+
|
|
1221
|
+
command_msg = self.mavlink_proxy.build_motor_value_command(
|
|
1222
|
+
motor_idx=self.target_motor,
|
|
1223
|
+
motor_value=motor_value,
|
|
1224
|
+
timeout=timeout
|
|
1269
1225
|
)
|
|
1270
1226
|
|
|
1271
1227
|
# Send the command
|
|
@@ -1284,23 +1240,13 @@ class ESCForceRunSingleController(BaseTimeoutController):
|
|
|
1284
1240
|
try:
|
|
1285
1241
|
# Use NaN to stop motor
|
|
1286
1242
|
nan_value = float('nan')
|
|
1287
|
-
param5 = float(1100 + self.target_motor)
|
|
1288
1243
|
|
|
1289
|
-
|
|
1290
|
-
|
|
1291
|
-
|
|
1292
|
-
|
|
1293
|
-
mavutil.mavlink.MAV_CMD_ACTUATOR_TEST,
|
|
1294
|
-
0, # confirmation
|
|
1295
|
-
nan_value, # Motor value (0-1 or NaN)
|
|
1296
|
-
0.0, # Timeout in seconds
|
|
1297
|
-
0, # Reserved
|
|
1298
|
-
0, # Reserved
|
|
1299
|
-
param5, # Motor mapping (110x)
|
|
1300
|
-
0, # Reserved
|
|
1301
|
-
0 # Reserved
|
|
1244
|
+
command_msg = self.mavlink_proxy.build_motor_value_command(
|
|
1245
|
+
motor_idx=self.target_motor,
|
|
1246
|
+
motor_value=nan_value,
|
|
1247
|
+
timeout=0.0
|
|
1302
1248
|
)
|
|
1303
|
-
|
|
1249
|
+
|
|
1304
1250
|
# Send the command
|
|
1305
1251
|
self.mavlink_proxy.send("mav", command_msg)
|
|
1306
1252
|
|
|
@@ -1447,7 +1393,15 @@ class BasePubSubController(ABC):
|
|
|
1447
1393
|
3. Stop publishing when receiving petal-user-journey-coordinator/unsubscribe_<stream_name>
|
|
1448
1394
|
"""
|
|
1449
1395
|
|
|
1450
|
-
def __init__(
|
|
1396
|
+
def __init__(
|
|
1397
|
+
self,
|
|
1398
|
+
stream_name: str,
|
|
1399
|
+
mqtt_proxy: MQTTProxy,
|
|
1400
|
+
mavlink_proxy: MavLinkExternalProxy,
|
|
1401
|
+
logger: logging.Logger,
|
|
1402
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
1403
|
+
):
|
|
1404
|
+
self.petal_name = petal_name
|
|
1451
1405
|
self.stream_name = stream_name
|
|
1452
1406
|
self.mqtt_proxy = mqtt_proxy
|
|
1453
1407
|
self.mavlink_proxy = mavlink_proxy
|
|
@@ -1544,7 +1498,7 @@ class BasePubSubController(ABC):
|
|
|
1544
1498
|
|
|
1545
1499
|
mqtt_message = {
|
|
1546
1500
|
"messageId": str(uuid.uuid4()),
|
|
1547
|
-
"command": f"/
|
|
1501
|
+
"command": f"/{self.petal_name}/publish_{self.stream_name}",
|
|
1548
1502
|
"timestamp": datetime.now().isoformat(),
|
|
1549
1503
|
"payload": publish_payload.model_dump()
|
|
1550
1504
|
}
|
|
@@ -1568,8 +1522,14 @@ class BasePubSubController(ABC):
|
|
|
1568
1522
|
class RCChannelsController(BasePubSubController):
|
|
1569
1523
|
"""Controller for streaming RC_CHANNELS MAVLink data."""
|
|
1570
1524
|
|
|
1571
|
-
def __init__(
|
|
1572
|
-
|
|
1525
|
+
def __init__(
|
|
1526
|
+
self,
|
|
1527
|
+
mqtt_proxy: MQTTProxy,
|
|
1528
|
+
mavlink_proxy: MavLinkExternalProxy,
|
|
1529
|
+
logger: logging.Logger,
|
|
1530
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
1531
|
+
):
|
|
1532
|
+
super().__init__("rc_value_stream", mqtt_proxy, mavlink_proxy, logger, petal_name=petal_name)
|
|
1573
1533
|
self._latest_sample = None
|
|
1574
1534
|
self._sample_lock = threading.Lock()
|
|
1575
1535
|
|
|
@@ -1628,9 +1588,10 @@ class PositionChannelsController(BasePubSubController):
|
|
|
1628
1588
|
corner_exclusion_radius: float,
|
|
1629
1589
|
max_matching_distance: float,
|
|
1630
1590
|
corner_points: List[Dict[str, float]],
|
|
1631
|
-
reference_trajectory: List[Dict[str, float]]
|
|
1591
|
+
reference_trajectory: List[Dict[str, float]],
|
|
1592
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
1632
1593
|
):
|
|
1633
|
-
super().__init__("real_time_pose", mqtt_proxy, mavlink_proxy, logger)
|
|
1594
|
+
super().__init__("real_time_pose", mqtt_proxy, mavlink_proxy, logger, petal_name=petal_name)
|
|
1634
1595
|
self._latest_position = None
|
|
1635
1596
|
self._latest_attitude = None
|
|
1636
1597
|
self._sample_lock = threading.Lock()
|
|
@@ -1916,8 +1877,14 @@ def _classify_kill_transition(text: str) -> Optional[bool]:
|
|
|
1916
1877
|
class KillSwitchController(BasePubSubController):
|
|
1917
1878
|
"""Controller for streaming kill switch status from PX4 STATUSTEXT messages with automatic parameter configuration."""
|
|
1918
1879
|
|
|
1919
|
-
def __init__(
|
|
1920
|
-
|
|
1880
|
+
def __init__(
|
|
1881
|
+
self,
|
|
1882
|
+
mqtt_proxy: MQTTProxy,
|
|
1883
|
+
mavlink_proxy: MavLinkExternalProxy,
|
|
1884
|
+
logger: logging.Logger,
|
|
1885
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
1886
|
+
):
|
|
1887
|
+
super().__init__("ks_status_stream", mqtt_proxy, mavlink_proxy, logger, petal_name=petal_name)
|
|
1921
1888
|
|
|
1922
1889
|
# Kill switch state tracking
|
|
1923
1890
|
self.is_killed: Optional[bool] = None # None = unknown
|
|
@@ -2007,8 +1974,14 @@ class KillSwitchController(BasePubSubController):
|
|
|
2007
1974
|
class MultiFunctionalSwitchAController(BasePubSubController):
|
|
2008
1975
|
"""Controller for streaming Multi-functional Switch A data from MAVLink messages."""
|
|
2009
1976
|
|
|
2010
|
-
def __init__(
|
|
2011
|
-
|
|
1977
|
+
def __init__(
|
|
1978
|
+
self,
|
|
1979
|
+
mqtt_proxy: MQTTProxy,
|
|
1980
|
+
mavlink_proxy: MavLinkExternalProxy,
|
|
1981
|
+
logger: logging.Logger,
|
|
1982
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
1983
|
+
):
|
|
1984
|
+
super().__init__("mfs_a_status_stream", mqtt_proxy, mavlink_proxy, logger, petal_name=petal_name)
|
|
2012
1985
|
|
|
2013
1986
|
# Multi-functional Switch A state tracking
|
|
2014
1987
|
self.latest_mfs_a_data: Optional[Dict[str, Any]] = None
|
|
@@ -2061,8 +2034,14 @@ class MultiFunctionalSwitchAController(BasePubSubController):
|
|
|
2061
2034
|
class MultiFunctionalSwitchBController(BasePubSubController):
|
|
2062
2035
|
"""Controller for streaming Multi-functional Switch B data from MAVLink messages."""
|
|
2063
2036
|
|
|
2064
|
-
def __init__(
|
|
2065
|
-
|
|
2037
|
+
def __init__(
|
|
2038
|
+
self,
|
|
2039
|
+
mqtt_proxy: MQTTProxy,
|
|
2040
|
+
mavlink_proxy: MavLinkExternalProxy,
|
|
2041
|
+
logger: logging.Logger,
|
|
2042
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
2043
|
+
):
|
|
2044
|
+
super().__init__("mfs_b_status_stream", mqtt_proxy, mavlink_proxy, logger, petal_name=petal_name)
|
|
2066
2045
|
|
|
2067
2046
|
# Multi-functional Switch B state tracking
|
|
2068
2047
|
self.latest_mfs_b_data: Optional[Dict[str, Any]] = None
|
|
@@ -2124,8 +2103,10 @@ class TrajectoryVerificationController:
|
|
|
2124
2103
|
rectangle_a: float = 2.0,
|
|
2125
2104
|
rectangle_b: float = 2.0,
|
|
2126
2105
|
points_per_edge: int = 10,
|
|
2127
|
-
corner_exclusion_radius: float = 0.2
|
|
2106
|
+
corner_exclusion_radius: float = 0.2,
|
|
2107
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
2128
2108
|
):
|
|
2109
|
+
self.petal_name = petal_name
|
|
2129
2110
|
self.mqtt_proxy = mqtt_proxy
|
|
2130
2111
|
self.logger = logger
|
|
2131
2112
|
self.is_active = False
|
|
@@ -2730,7 +2711,7 @@ class TrajectoryVerificationController:
|
|
|
2730
2711
|
|
|
2731
2712
|
mqtt_message = {
|
|
2732
2713
|
"messageId": str(uuid.uuid4()),
|
|
2733
|
-
"command": "/
|
|
2714
|
+
"command": f"/{self.petal_name}/verify_pos_yaw_directions_results",
|
|
2734
2715
|
"timestamp": datetime.now().isoformat(),
|
|
2735
2716
|
"payload": results_payload
|
|
2736
2717
|
}
|
|
@@ -2758,7 +2739,13 @@ class WifiOptitrackConnectivityController:
|
|
|
2758
2739
|
Controller for connecting to WiFi and verifying OptiTrack server connectivity.
|
|
2759
2740
|
"""
|
|
2760
2741
|
|
|
2761
|
-
def __init__(
|
|
2742
|
+
def __init__(
|
|
2743
|
+
self,
|
|
2744
|
+
mqtt_proxy: "MQTTProxy",
|
|
2745
|
+
logger: logging.Logger,
|
|
2746
|
+
petal_name: str = "petal-user-journey-coordinator"
|
|
2747
|
+
):
|
|
2748
|
+
self.petal_name = petal_name
|
|
2762
2749
|
self.mqtt_proxy = mqtt_proxy
|
|
2763
2750
|
self.logger = logger
|
|
2764
2751
|
self.is_active = False
|
|
@@ -3115,7 +3102,7 @@ class WifiOptitrackConnectivityController:
|
|
|
3115
3102
|
mqtt_message = {
|
|
3116
3103
|
"messageId": message_id,
|
|
3117
3104
|
"deviceId": getattr(self.mqtt_proxy, 'device_id', 'unknown'),
|
|
3118
|
-
"command": "
|
|
3105
|
+
"command": f"{self.petal_name}/acknowledge",
|
|
3119
3106
|
"timestamp": datetime.now().isoformat(),
|
|
3120
3107
|
"payload": response_payload.model_dump()
|
|
3121
3108
|
}
|
|
@@ -3387,7 +3374,7 @@ class WifiOptitrackConnectivityController:
|
|
|
3387
3374
|
mqtt_message = {
|
|
3388
3375
|
"messageId": message_id,
|
|
3389
3376
|
"deviceId": getattr(self.mqtt_proxy, 'device_id', 'unknown'),
|
|
3390
|
-
"command": "
|
|
3377
|
+
"command": f"{self.petal_name}/set_static_ip_address_ack",
|
|
3391
3378
|
"timestamp": datetime.now().isoformat(),
|
|
3392
3379
|
"payload": response_payload.model_dump()
|
|
3393
3380
|
}
|
|
@@ -188,11 +188,12 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
188
188
|
rectangle_a=self.rectangle_a,
|
|
189
189
|
rectangle_b=self.rectangle_b,
|
|
190
190
|
points_per_edge=self.points_per_edge,
|
|
191
|
-
corner_exclusion_radius=self.corner_exclusion_radius
|
|
191
|
+
corner_exclusion_radius=self.corner_exclusion_radius,
|
|
192
|
+
petal_name=self.name
|
|
192
193
|
)
|
|
193
194
|
|
|
194
195
|
# Initialize WiFi OptiTrack connectivity controller
|
|
195
|
-
self._wifi_optitrack_controller = WifiOptitrackConnectivityController(self._mqtt_proxy, logger)
|
|
196
|
+
self._wifi_optitrack_controller = WifiOptitrackConnectivityController(self._mqtt_proxy, logger, petal_name=self.name)
|
|
196
197
|
|
|
197
198
|
# Initialize operation controllers
|
|
198
199
|
self._operation_controllers = {
|
|
@@ -375,7 +376,12 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
375
376
|
|
|
376
377
|
# Initialize pub/sub controllers now that topic_base is available
|
|
377
378
|
self._pubsub_controllers: Dict[str, BasePubSubController] = {
|
|
378
|
-
"rc_value_stream": RCChannelsController(
|
|
379
|
+
"rc_value_stream": RCChannelsController(
|
|
380
|
+
self._mqtt_proxy,
|
|
381
|
+
self._mavlink_proxy,
|
|
382
|
+
logger,
|
|
383
|
+
petal_name=self.name
|
|
384
|
+
),
|
|
379
385
|
"pose_value_stream": PositionChannelsController(
|
|
380
386
|
mqtt_proxy=self._mqtt_proxy,
|
|
381
387
|
mavlink_proxy=self._mavlink_proxy,
|
|
@@ -386,11 +392,27 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
386
392
|
corner_exclusion_radius=self.corner_exclusion_radius,
|
|
387
393
|
max_matching_distance=self._trajectory_verification.max_matching_distance,
|
|
388
394
|
corner_points=self._trajectory_verification.corner_points, # Pass corner points here
|
|
389
|
-
reference_trajectory=self._trajectory_verification.reference_trajectory # Pass reference trajectory here
|
|
395
|
+
reference_trajectory=self._trajectory_verification.reference_trajectory, # Pass reference trajectory here
|
|
396
|
+
petal_name=self.name
|
|
397
|
+
),
|
|
398
|
+
"ks_status_stream": KillSwitchController(
|
|
399
|
+
self._mqtt_proxy,
|
|
400
|
+
self._mavlink_proxy,
|
|
401
|
+
logger,
|
|
402
|
+
petal_name=self.name
|
|
403
|
+
),
|
|
404
|
+
"mfs_a_status_stream": MultiFunctionalSwitchAController(
|
|
405
|
+
self._mqtt_proxy,
|
|
406
|
+
self._mavlink_proxy,
|
|
407
|
+
logger,
|
|
408
|
+
petal_name=self.name
|
|
390
409
|
),
|
|
391
|
-
"
|
|
392
|
-
|
|
393
|
-
|
|
410
|
+
"mfs_b_status_stream": MultiFunctionalSwitchBController(
|
|
411
|
+
self._mqtt_proxy,
|
|
412
|
+
self._mavlink_proxy,
|
|
413
|
+
logger,
|
|
414
|
+
petal_name=self.name
|
|
415
|
+
)
|
|
394
416
|
}
|
|
395
417
|
|
|
396
418
|
# Initialize command handlers registry
|
|
@@ -421,45 +443,44 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
421
443
|
# "Update": self._test_unregister_all_handlers,
|
|
422
444
|
|
|
423
445
|
# Timeout operation commands
|
|
424
|
-
"
|
|
425
|
-
"
|
|
426
|
-
"
|
|
446
|
+
f"{self.name}/esc_calibration": self._esc_calibration_message_handler,
|
|
447
|
+
f"{self.name}/esc_force_run_all": self._esc_force_run_all_message_handler,
|
|
448
|
+
f"{self.name}/esc_force_run_single": self._esc_force_run_single_message_handler,
|
|
427
449
|
|
|
428
450
|
# Parameter configuration commands
|
|
429
|
-
"
|
|
430
|
-
"
|
|
431
|
-
"
|
|
432
|
-
"
|
|
433
|
-
"
|
|
434
|
-
"
|
|
435
|
-
"
|
|
436
|
-
"
|
|
437
|
-
"
|
|
438
|
-
"
|
|
451
|
+
f"{self.name}/geometry": self._geometry_message_handler,
|
|
452
|
+
f"{self.name}/gps_module": self._gps_module_message_handler,
|
|
453
|
+
f"{self.name}/dist_module": self._dist_module_message_handler,
|
|
454
|
+
f"{self.name}/oflow_module": self._oflow_module_message_handler,
|
|
455
|
+
f"{self.name}/gps_spatial_offset": self._gps_spatial_offset_message_handler,
|
|
456
|
+
f"{self.name}/distance_spatial_offset": self._distance_spatial_offset_message_handler,
|
|
457
|
+
f"{self.name}/optical_flow_spatial_offset": self._optical_flow_spatial_offset_message_handler,
|
|
458
|
+
f"{self.name}/esc_update_calibration_limits": self._esc_update_calibration_limits_message_handler,
|
|
459
|
+
f"{self.name}/bulk_set_parameters": self._bulk_set_parameter_message_handler,
|
|
460
|
+
f"{self.name}/bulk_get_parameters": self._bulk_get_parameter_message_handler,
|
|
439
461
|
|
|
440
462
|
# Pub/Sub stream commands
|
|
441
|
-
"
|
|
442
|
-
"
|
|
443
|
-
"
|
|
444
|
-
"
|
|
445
|
-
"
|
|
446
|
-
"
|
|
447
|
-
"
|
|
448
|
-
"
|
|
449
|
-
"
|
|
450
|
-
"
|
|
451
|
-
"
|
|
463
|
+
f"{self.name}/subscribe_rc_value_stream": self._subscribe_rc_value_stream_handler,
|
|
464
|
+
f"{self.name}/unsubscribe_rc_value_stream": self._unsubscribe_rc_value_stream_handler,
|
|
465
|
+
f"{self.name}/subscribe_pose_value_stream": self._subscribe_pose_value_stream_handler,
|
|
466
|
+
f"{self.name}/unsubscribe_pose_value_stream": self._unsubscribe_pose_value_stream_handler,
|
|
467
|
+
f"{self.name}/subscribe_ks_status_stream": self._subscribe_ks_status_stream_handler,
|
|
468
|
+
f"{self.name}/unsubscribe_ks_status_stream": self._unsubscribe_ks_status_stream_handler,
|
|
469
|
+
f"{self.name}/subscribe_mfs_a_status_stream": self._subscribe_mfs_a_status_stream_handler,
|
|
470
|
+
f"{self.name}/unsubscribe_mfs_a_status_stream": self._unsubscribe_mfs_a_status_stream_handler,
|
|
471
|
+
f"{self.name}/subscribe_mfs_b_status_stream": self._subscribe_mfs_b_status_stream_handler,
|
|
472
|
+
f"{self.name}/unsubscribe_mfs_b_status_stream": self._unsubscribe_mfs_b_status_stream_handler,
|
|
473
|
+
f"{self.name}/unsubscribeall": self._unregister_all_handlers,
|
|
452
474
|
|
|
453
475
|
# Trajectory verification commands
|
|
454
|
-
"
|
|
455
|
-
"
|
|
476
|
+
f"{self.name}/verify_pos_yaw_directions": self._verify_pos_yaw_directions_handler,
|
|
477
|
+
f"{self.name}/verify_pos_yaw_directions_complete": self._verify_pos_yaw_directions_complete_handler,
|
|
456
478
|
|
|
457
479
|
# WiFi OptiTrack connectivity commands
|
|
458
|
-
"
|
|
459
|
-
"
|
|
460
|
-
|
|
480
|
+
f"{self.name}/connect_to_wifi_and_verify_optitrack": self._connect_to_wifi_and_verify_optitrack_handler,
|
|
481
|
+
f"{self.name}/set_static_ip_address": self._set_static_ip_address_handler,
|
|
461
482
|
# Reboot command
|
|
462
|
-
"
|
|
483
|
+
f"{self.name}/reboot_autopilot": self._reboot_px4_message_handler
|
|
463
484
|
}
|
|
464
485
|
|
|
465
486
|
async def _master_command_handler(self, topic: str, message: Dict[str, Any]):
|
|
@@ -488,6 +509,10 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
488
509
|
handler = self._command_handlers[command]
|
|
489
510
|
await handler(topic, message)
|
|
490
511
|
else:
|
|
512
|
+
# if command does not start with petal-flight-log/, ignore it
|
|
513
|
+
if not command.startswith(f"{self.name}/"):
|
|
514
|
+
logger.debug(f"Ignoring command not meant for this petal: {command}")
|
|
515
|
+
return
|
|
491
516
|
error_msg = f"Unknown command: {command}"
|
|
492
517
|
logger.error(error_msg)
|
|
493
518
|
|
|
@@ -756,7 +781,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
756
781
|
"waitResponse": True,
|
|
757
782
|
"messageId": f"test-pose-subscribe-{datetime.now().timestamp()}",
|
|
758
783
|
"deviceId": message.get("deviceId", "test-device"),
|
|
759
|
-
"command": "
|
|
784
|
+
"command": f"{self.name}/subscribe_pose_value_stream",
|
|
760
785
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
761
786
|
"payload": {
|
|
762
787
|
"subscribed_stream_id": "real_time_pose",
|
|
@@ -773,7 +798,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
773
798
|
"waitResponse": True,
|
|
774
799
|
"messageId": f"test-verify-start-{datetime.now().timestamp()}",
|
|
775
800
|
"deviceId": message.get("deviceId", "test-device"),
|
|
776
|
-
"command": "
|
|
801
|
+
"command": f"{self.name}/verify_pos_yaw_directions",
|
|
777
802
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
778
803
|
"payload": {
|
|
779
804
|
"start": True
|
|
@@ -790,7 +815,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
790
815
|
"waitResponse": True,
|
|
791
816
|
"messageId": f"test-verify-complete-{datetime.now().timestamp()}",
|
|
792
817
|
"deviceId": message.get("deviceId", "test-device"),
|
|
793
|
-
"command": "
|
|
818
|
+
"command": f"{self.name}/verify_pos_yaw_directions_complete",
|
|
794
819
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
795
820
|
"payload": {}
|
|
796
821
|
}
|
|
@@ -801,7 +826,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
801
826
|
"waitResponse": True,
|
|
802
827
|
"messageId": f"test-pose-unsubscribe-{datetime.now().timestamp()}",
|
|
803
828
|
"deviceId": message.get("deviceId", "test-device"),
|
|
804
|
-
"command": "
|
|
829
|
+
"command": f"{self.name}/unsubscribe_pose_value_stream",
|
|
805
830
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
806
831
|
"payload": {
|
|
807
832
|
"unsubscribed_stream_id": "real_time_pose"
|
|
@@ -821,7 +846,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
821
846
|
"waitResponse": True,
|
|
822
847
|
"messageId": f"test-wifi-optitrack-{datetime.now().timestamp()}",
|
|
823
848
|
"deviceId": message.get("deviceId", "test-device"),
|
|
824
|
-
"command": "
|
|
849
|
+
"command": f"{self.name}/connect_to_wifi_and_verify_optitrack",
|
|
825
850
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
826
851
|
"payload": {
|
|
827
852
|
"positioning_system_network_wifi_ssid": "Rob-Lab-C00060",
|
|
@@ -847,7 +872,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
847
872
|
"waitResponse": True,
|
|
848
873
|
"messageId": f"test-static-ip-{datetime.now().timestamp()}",
|
|
849
874
|
"deviceId": message.get("deviceId", "test-device"),
|
|
850
|
-
"command": "
|
|
875
|
+
"command": f"{self.name}/set_static_ip_address",
|
|
851
876
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
852
877
|
"payload": {
|
|
853
878
|
"positioning_system_network_wifi_subnet": "255.255.255.0",
|
|
@@ -870,12 +895,12 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
870
895
|
# List of streams to subscribe to for testing
|
|
871
896
|
test_subscriptions = [
|
|
872
897
|
{
|
|
873
|
-
"command": "
|
|
898
|
+
"command": f"{self.name}/subscribe_rc_value_stream",
|
|
874
899
|
"stream_id": "px4_rc_raw",
|
|
875
900
|
"data_rate_hz": 20.0
|
|
876
901
|
},
|
|
877
902
|
{
|
|
878
|
-
"command": "
|
|
903
|
+
"command": f"{self.name}/subscribe_pose_value_stream",
|
|
879
904
|
"stream_id": "real_time_pose",
|
|
880
905
|
"data_rate_hz": 10.0
|
|
881
906
|
},
|
|
@@ -925,7 +950,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
925
950
|
"waitResponse": True,
|
|
926
951
|
"messageId": f"test-unsubscribe-all-{datetime.now().timestamp()}",
|
|
927
952
|
"deviceId": message.get("deviceId", "test-device"),
|
|
928
|
-
"command": "
|
|
953
|
+
"command": f"{self.name}/unsubscribeall",
|
|
929
954
|
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
930
955
|
"payload": {}
|
|
931
956
|
}
|
|
@@ -1686,7 +1711,18 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
1686
1711
|
results[pname] = confirmed[pname]
|
|
1687
1712
|
# check that the set value matches the requested value
|
|
1688
1713
|
confirmed_value = results[pname].get("value")
|
|
1689
|
-
|
|
1714
|
+
requested_value = parameter.parameter_value
|
|
1715
|
+
|
|
1716
|
+
# Check for equality, handling floating point precision issues
|
|
1717
|
+
is_match = False
|
|
1718
|
+
if isinstance(confirmed_value, (float, int)) and isinstance(requested_value, (float, int)):
|
|
1719
|
+
# Use 1e-5 relative tolerance to handle float32/float64 mismatch
|
|
1720
|
+
# This should be enough for values like 0.2 vs 0.20000000298...
|
|
1721
|
+
is_match = math.isclose(confirmed_value, requested_value, rel_tol=1e-5)
|
|
1722
|
+
else:
|
|
1723
|
+
is_match = confirmed_value == requested_value
|
|
1724
|
+
|
|
1725
|
+
if is_match:
|
|
1690
1726
|
results[pname]["success"] = True
|
|
1691
1727
|
else:
|
|
1692
1728
|
results[pname]["success"] = False
|
|
@@ -2564,7 +2600,17 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2564
2600
|
results[pname] = confirmed[pname]
|
|
2565
2601
|
# check that the set value matches the requested value
|
|
2566
2602
|
confirmed_value = results[pname].get("value")
|
|
2567
|
-
|
|
2603
|
+
requested_value = parameter.parameter_value
|
|
2604
|
+
|
|
2605
|
+
# Check for equality, handling floating point precision issues
|
|
2606
|
+
is_match = False
|
|
2607
|
+
if isinstance(confirmed_value, (float, int)) and isinstance(requested_value, (float, int)):
|
|
2608
|
+
# Use 1e-5 relative tolerance to handle float32/float64 mismatch
|
|
2609
|
+
is_match = math.isclose(confirmed_value, requested_value, rel_tol=1e-5)
|
|
2610
|
+
else:
|
|
2611
|
+
is_match = confirmed_value == requested_value
|
|
2612
|
+
|
|
2613
|
+
if is_match:
|
|
2568
2614
|
results[pname]["success"] = True
|
|
2569
2615
|
else:
|
|
2570
2616
|
results[pname]["success"] = False
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
{petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/__init__.py
RENAMED
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
{petal_user_journey_coordinator-0.1.6 → petal_user_journey_coordinator-0.1.8}/tests/test_plotting.py
RENAMED
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|