petal-user-journey-coordinator 0.1.5__py3-none-any.whl → 0.1.7__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.
- petal_user_journey_coordinator/controllers.py +83 -96
- petal_user_journey_coordinator/data_model.py +104 -24
- petal_user_journey_coordinator/plugin.py +649 -135
- {petal_user_journey_coordinator-0.1.5.dist-info → petal_user_journey_coordinator-0.1.7.dist-info}/METADATA +1 -1
- petal_user_journey_coordinator-0.1.7.dist-info/RECORD +8 -0
- petal_user_journey_coordinator-0.1.5.dist-info/RECORD +0 -8
- {petal_user_journey_coordinator-0.1.5.dist-info → petal_user_journey_coordinator-0.1.7.dist-info}/WHEEL +0 -0
- {petal_user_journey_coordinator-0.1.5.dist-info → petal_user_journey_coordinator-0.1.7.dist-info}/entry_points.txt +0 -0
|
@@ -6,7 +6,7 @@ import asyncio
|
|
|
6
6
|
import math
|
|
7
7
|
import numpy as np
|
|
8
8
|
from typing import Dict, Any, List, Union, Optional, Callable
|
|
9
|
-
from datetime import datetime
|
|
9
|
+
from datetime import datetime, timezone
|
|
10
10
|
import threading
|
|
11
11
|
from enum import Enum
|
|
12
12
|
import time
|
|
@@ -21,6 +21,7 @@ from petal_app_manager.proxies import (
|
|
|
21
21
|
RedisProxy
|
|
22
22
|
)
|
|
23
23
|
from petal_app_manager import Config
|
|
24
|
+
from petal_app_manager.models import MQTTMessage
|
|
24
25
|
|
|
25
26
|
import json, math
|
|
26
27
|
from pymavlink import mavutil
|
|
@@ -66,7 +67,7 @@ from .controllers import (
|
|
|
66
67
|
|
|
67
68
|
from .data_model import (
|
|
68
69
|
# generic payloads
|
|
69
|
-
|
|
70
|
+
BulkParameterSetRequest,
|
|
70
71
|
SubscribePayload,
|
|
71
72
|
UnsubscribePayload,
|
|
72
73
|
|
|
@@ -89,9 +90,15 @@ from .data_model import (
|
|
|
89
90
|
MavlinkParametersResponseModel,
|
|
90
91
|
RotorCountParameter,
|
|
91
92
|
DistanceModulePayload,
|
|
92
|
-
OpticalFlowModulePayload
|
|
93
|
+
OpticalFlowModulePayload,
|
|
94
|
+
BulkParameterSetRequest,
|
|
95
|
+
BulkParameterGetRequest,
|
|
96
|
+
BulkParameterResponse
|
|
93
97
|
)
|
|
94
98
|
|
|
99
|
+
from petal_app_manager.models.mavlink import (
|
|
100
|
+
RebootAutopilotResponse
|
|
101
|
+
)
|
|
95
102
|
|
|
96
103
|
class OperationMode(Enum):
|
|
97
104
|
"""Enumeration of operation modes"""
|
|
@@ -181,11 +188,12 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
181
188
|
rectangle_a=self.rectangle_a,
|
|
182
189
|
rectangle_b=self.rectangle_b,
|
|
183
190
|
points_per_edge=self.points_per_edge,
|
|
184
|
-
corner_exclusion_radius=self.corner_exclusion_radius
|
|
191
|
+
corner_exclusion_radius=self.corner_exclusion_radius,
|
|
192
|
+
petal_name=self.name
|
|
185
193
|
)
|
|
186
194
|
|
|
187
195
|
# Initialize WiFi OptiTrack connectivity controller
|
|
188
|
-
self._wifi_optitrack_controller = WifiOptitrackConnectivityController(self._mqtt_proxy, logger)
|
|
196
|
+
self._wifi_optitrack_controller = WifiOptitrackConnectivityController(self._mqtt_proxy, logger, petal_name=self.name)
|
|
189
197
|
|
|
190
198
|
# Initialize operation controllers
|
|
191
199
|
self._operation_controllers = {
|
|
@@ -267,7 +275,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
267
275
|
self._active_handlers[stream_name] = {
|
|
268
276
|
"stream_id": stream_id,
|
|
269
277
|
"rate_hz": rate_hz,
|
|
270
|
-
"started_at": datetime.now().isoformat(),
|
|
278
|
+
"started_at": datetime.now(timezone.utc).isoformat(),
|
|
271
279
|
"controller": self._pubsub_controllers.get(stream_name)
|
|
272
280
|
}
|
|
273
281
|
logger.info(f"Tracking subscription for {stream_name} (ID: {stream_id}, Rate: {rate_hz} Hz)")
|
|
@@ -302,7 +310,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
302
310
|
"status": "success",
|
|
303
311
|
"message": "No active handlers to unregister",
|
|
304
312
|
"unsubscribed_streams": [],
|
|
305
|
-
"timestamp": datetime.now().isoformat()
|
|
313
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
306
314
|
}
|
|
307
315
|
|
|
308
316
|
unsubscribed_streams = []
|
|
@@ -336,7 +344,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
336
344
|
"status": "success" if not failed_streams else "partial_success",
|
|
337
345
|
"message": f"Unsubscribed from {len(unsubscribed_streams)} streams",
|
|
338
346
|
"unsubscribed_streams": unsubscribed_streams,
|
|
339
|
-
"timestamp": datetime.now().isoformat()
|
|
347
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
340
348
|
}
|
|
341
349
|
|
|
342
350
|
if failed_streams:
|
|
@@ -368,7 +376,12 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
368
376
|
|
|
369
377
|
# Initialize pub/sub controllers now that topic_base is available
|
|
370
378
|
self._pubsub_controllers: Dict[str, BasePubSubController] = {
|
|
371
|
-
"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
|
+
),
|
|
372
385
|
"pose_value_stream": PositionChannelsController(
|
|
373
386
|
mqtt_proxy=self._mqtt_proxy,
|
|
374
387
|
mavlink_proxy=self._mavlink_proxy,
|
|
@@ -379,11 +392,27 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
379
392
|
corner_exclusion_radius=self.corner_exclusion_radius,
|
|
380
393
|
max_matching_distance=self._trajectory_verification.max_matching_distance,
|
|
381
394
|
corner_points=self._trajectory_verification.corner_points, # Pass corner points here
|
|
382
|
-
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
|
|
383
403
|
),
|
|
384
|
-
"
|
|
385
|
-
|
|
386
|
-
|
|
404
|
+
"mfs_a_status_stream": MultiFunctionalSwitchAController(
|
|
405
|
+
self._mqtt_proxy,
|
|
406
|
+
self._mavlink_proxy,
|
|
407
|
+
logger,
|
|
408
|
+
petal_name=self.name
|
|
409
|
+
),
|
|
410
|
+
"mfs_b_status_stream": MultiFunctionalSwitchBController(
|
|
411
|
+
self._mqtt_proxy,
|
|
412
|
+
self._mavlink_proxy,
|
|
413
|
+
logger,
|
|
414
|
+
petal_name=self.name
|
|
415
|
+
)
|
|
387
416
|
}
|
|
388
417
|
|
|
389
418
|
# Initialize command handlers registry
|
|
@@ -414,40 +443,44 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
414
443
|
# "Update": self._test_unregister_all_handlers,
|
|
415
444
|
|
|
416
445
|
# Timeout operation commands
|
|
417
|
-
"
|
|
418
|
-
"
|
|
419
|
-
"
|
|
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,
|
|
420
449
|
|
|
421
450
|
# Parameter configuration commands
|
|
422
|
-
"
|
|
423
|
-
"
|
|
424
|
-
"
|
|
425
|
-
"
|
|
426
|
-
"
|
|
427
|
-
"
|
|
428
|
-
"
|
|
429
|
-
"
|
|
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,
|
|
430
461
|
|
|
431
462
|
# Pub/Sub stream commands
|
|
432
|
-
"
|
|
433
|
-
"
|
|
434
|
-
"
|
|
435
|
-
"
|
|
436
|
-
"
|
|
437
|
-
"
|
|
438
|
-
"
|
|
439
|
-
"
|
|
440
|
-
"
|
|
441
|
-
"
|
|
442
|
-
"
|
|
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,
|
|
443
474
|
|
|
444
475
|
# Trajectory verification commands
|
|
445
|
-
"
|
|
446
|
-
"
|
|
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,
|
|
447
478
|
|
|
448
479
|
# WiFi OptiTrack connectivity commands
|
|
449
|
-
"
|
|
450
|
-
"
|
|
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,
|
|
482
|
+
# Reboot command
|
|
483
|
+
f"{self.name}/reboot_autopilot": self._reboot_px4_message_handler
|
|
451
484
|
}
|
|
452
485
|
|
|
453
486
|
async def _master_command_handler(self, topic: str, message: Dict[str, Any]):
|
|
@@ -476,6 +509,10 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
476
509
|
handler = self._command_handlers[command]
|
|
477
510
|
await handler(topic, message)
|
|
478
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
|
|
479
516
|
error_msg = f"Unknown command: {command}"
|
|
480
517
|
logger.error(error_msg)
|
|
481
518
|
|
|
@@ -518,13 +555,9 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
518
555
|
except Exception as e:
|
|
519
556
|
logger.error(f"Failed to send error response: {e}")
|
|
520
557
|
|
|
558
|
+
@http_action(method="POST", path="/test/esc-calibration")
|
|
521
559
|
async def _test_esc_calibration_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
522
560
|
"""Test handler for ESC calibration with enhanced workflow."""
|
|
523
|
-
# allow only one call
|
|
524
|
-
if getattr(self, "_test_esc_calibration_called", False):
|
|
525
|
-
logger.warning("ESC calibration test has already been called.")
|
|
526
|
-
return
|
|
527
|
-
self._test_esc_calibration_called = True
|
|
528
561
|
|
|
529
562
|
# Test Step 1: Initialize and configure ESC calibration
|
|
530
563
|
test_payload = {
|
|
@@ -583,6 +616,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
583
616
|
|
|
584
617
|
logger.info("✅ ESC calibration test sequence completed!")
|
|
585
618
|
|
|
619
|
+
@http_action(method="POST", path="/test/geometry")
|
|
586
620
|
async def _test_geometry_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
587
621
|
# intercept payload
|
|
588
622
|
test_payload = {
|
|
@@ -592,6 +626,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
592
626
|
# Use the dynamically created handler directly
|
|
593
627
|
await self._rotor_count_message_handler(topic, message)
|
|
594
628
|
|
|
629
|
+
@http_action(method="POST", path="/test/dist-module")
|
|
595
630
|
async def _test_dist_module_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
596
631
|
"""Test handler for distance module configuration."""
|
|
597
632
|
# Test with LiDAR Lite v3
|
|
@@ -601,6 +636,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
601
636
|
message["payload"] = test_payload
|
|
602
637
|
await self._dist_module_message_handler(topic, message)
|
|
603
638
|
|
|
639
|
+
@http_action(method="POST", path="/test/oflow-module")
|
|
604
640
|
async def _test_oflow_module_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
605
641
|
"""Test handler for optical flow module configuration."""
|
|
606
642
|
# Test with ARK Flow
|
|
@@ -610,6 +646,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
610
646
|
message["payload"] = test_payload
|
|
611
647
|
await self._oflow_module_message_handler(topic, message)
|
|
612
648
|
|
|
649
|
+
@http_action(method="POST", path="/test/subscribe-rc-value-stream")
|
|
613
650
|
async def _test_subscribe_rc_value_stream_handler(self, topic: str, message: Dict[str, Any]):
|
|
614
651
|
# intercept payload
|
|
615
652
|
test_payload = {
|
|
@@ -630,6 +667,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
630
667
|
|
|
631
668
|
await self._unsubscribe_rc_value_stream_handler(topic, message)
|
|
632
669
|
|
|
670
|
+
@http_action(method="POST", path="/test/subscribe-real-time-pose")
|
|
633
671
|
async def _test_subscribe_real_time_pose_handler(self, topic: str, message: Dict[str, Any]):
|
|
634
672
|
# intercept payload
|
|
635
673
|
test_payload = {
|
|
@@ -650,6 +688,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
650
688
|
|
|
651
689
|
await self._unsubscribe_real_time_pose_handler(topic, message)
|
|
652
690
|
|
|
691
|
+
@http_action(method="POST", path="/test/kill-switch-stream")
|
|
653
692
|
async def _test_kill_switch_stream_handler(self, topic: str, message: Dict[str, Any]):
|
|
654
693
|
"""Test handler for kill switch stream."""
|
|
655
694
|
logger.info("Running kill switch stream test")
|
|
@@ -675,6 +714,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
675
714
|
|
|
676
715
|
logger.info("Kill switch stream test completed")
|
|
677
716
|
|
|
717
|
+
@http_action(method="POST", path="/test/mfs-a-stream")
|
|
678
718
|
async def _test_mfs_a_stream_handler(self, topic: str, message: Dict[str, Any]):
|
|
679
719
|
"""Test handler for Multi-functional Switch A stream."""
|
|
680
720
|
logger.info("Running Multi-functional Switch A stream test")
|
|
@@ -700,6 +740,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
700
740
|
|
|
701
741
|
logger.info("Multi-functional Switch A stream test completed")
|
|
702
742
|
|
|
743
|
+
@http_action(method="POST", path="/test/mfs-b-stream")
|
|
703
744
|
async def _test_mfs_b_stream_handler(self, topic: str, message: Dict[str, Any]):
|
|
704
745
|
"""Test handler for Multi-functional Switch B stream."""
|
|
705
746
|
logger.info("Running Multi-functional Switch B stream test")
|
|
@@ -725,14 +766,10 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
725
766
|
|
|
726
767
|
logger.info("Multi-functional Switch B stream test completed")
|
|
727
768
|
|
|
769
|
+
@http_action(method="POST", path="/test/verify-pos-yaw-directions")
|
|
728
770
|
async def _test_verify_pos_yaw_directions_handler(self, topic: str, message: Dict[str, Any]):
|
|
729
771
|
"""Test handler for trajectory verification with the new command structure."""
|
|
730
772
|
|
|
731
|
-
if getattr(self, "_test_verification_trajectory", False):
|
|
732
|
-
logger.warning("Trajectory verification test has already been called.")
|
|
733
|
-
return
|
|
734
|
-
self._test_verification_trajectory = True
|
|
735
|
-
|
|
736
773
|
logger.info("Running trajectory verification test with new command structure")
|
|
737
774
|
|
|
738
775
|
# Configure trajectory collection rate (optional - demonstrates the feature)
|
|
@@ -744,8 +781,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
744
781
|
"waitResponse": True,
|
|
745
782
|
"messageId": f"test-pose-subscribe-{datetime.now().timestamp()}",
|
|
746
783
|
"deviceId": message.get("deviceId", "test-device"),
|
|
747
|
-
"command": "
|
|
748
|
-
"timestamp": datetime.now().isoformat(),
|
|
784
|
+
"command": f"{self.name}/subscribe_pose_value_stream",
|
|
785
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
749
786
|
"payload": {
|
|
750
787
|
"subscribed_stream_id": "real_time_pose",
|
|
751
788
|
"data_rate_hz": 10.0
|
|
@@ -761,8 +798,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
761
798
|
"waitResponse": True,
|
|
762
799
|
"messageId": f"test-verify-start-{datetime.now().timestamp()}",
|
|
763
800
|
"deviceId": message.get("deviceId", "test-device"),
|
|
764
|
-
"command": "
|
|
765
|
-
"timestamp": datetime.now().isoformat(),
|
|
801
|
+
"command": f"{self.name}/verify_pos_yaw_directions",
|
|
802
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
766
803
|
"payload": {
|
|
767
804
|
"start": True
|
|
768
805
|
}
|
|
@@ -778,8 +815,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
778
815
|
"waitResponse": True,
|
|
779
816
|
"messageId": f"test-verify-complete-{datetime.now().timestamp()}",
|
|
780
817
|
"deviceId": message.get("deviceId", "test-device"),
|
|
781
|
-
"command": "
|
|
782
|
-
"timestamp": datetime.now().isoformat(),
|
|
818
|
+
"command": f"{self.name}/verify_pos_yaw_directions_complete",
|
|
819
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
783
820
|
"payload": {}
|
|
784
821
|
}
|
|
785
822
|
await self._master_command_handler(topic, verify_complete_message)
|
|
@@ -789,8 +826,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
789
826
|
"waitResponse": True,
|
|
790
827
|
"messageId": f"test-pose-unsubscribe-{datetime.now().timestamp()}",
|
|
791
828
|
"deviceId": message.get("deviceId", "test-device"),
|
|
792
|
-
"command": "
|
|
793
|
-
"timestamp": datetime.now().isoformat(),
|
|
829
|
+
"command": f"{self.name}/unsubscribe_pose_value_stream",
|
|
830
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
794
831
|
"payload": {
|
|
795
832
|
"unsubscribed_stream_id": "real_time_pose"
|
|
796
833
|
}
|
|
@@ -799,14 +836,9 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
799
836
|
|
|
800
837
|
logger.info("Trajectory verification test completed")
|
|
801
838
|
|
|
839
|
+
@http_action(method="POST", path="/test/connect-to-wifi-and-verify-optitrack")
|
|
802
840
|
async def _test_connect_to_wifi_and_verify_optitrack_handler(self, topic: str, message: Dict[str, Any]):
|
|
803
|
-
"""Test handler for WiFi and OptiTrack connectivity verification."""
|
|
804
|
-
# allow only one call
|
|
805
|
-
if getattr(self, "_test_wifi", False):
|
|
806
|
-
logger.warning("Wifi test has already been called.")
|
|
807
|
-
return
|
|
808
|
-
self._test_wifi = True
|
|
809
|
-
|
|
841
|
+
"""Test handler for WiFi and OptiTrack connectivity verification."""
|
|
810
842
|
logger.info("Running WiFi and OptiTrack connectivity verification test")
|
|
811
843
|
|
|
812
844
|
# Create test message with proper command structure
|
|
@@ -814,8 +846,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
814
846
|
"waitResponse": True,
|
|
815
847
|
"messageId": f"test-wifi-optitrack-{datetime.now().timestamp()}",
|
|
816
848
|
"deviceId": message.get("deviceId", "test-device"),
|
|
817
|
-
"command": "
|
|
818
|
-
"timestamp": datetime.now().isoformat(),
|
|
849
|
+
"command": f"{self.name}/connect_to_wifi_and_verify_optitrack",
|
|
850
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
819
851
|
"payload": {
|
|
820
852
|
"positioning_system_network_wifi_ssid": "Rob-Lab-C00060",
|
|
821
853
|
"positioning_system_network_wifi_pass": "kuri@1234!!",
|
|
@@ -830,14 +862,9 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
830
862
|
await self._master_command_handler(topic, test_message)
|
|
831
863
|
logger.info("WiFi and OptiTrack connectivity test completed")
|
|
832
864
|
|
|
865
|
+
@http_action(method="POST", path="/test/set-static-ip-address")
|
|
833
866
|
async def _test_set_static_ip_address_handler(self, topic: str, message: Dict[str, Any]):
|
|
834
867
|
"""Test handler for static IP address configuration."""
|
|
835
|
-
# allow only one call
|
|
836
|
-
if getattr(self, "_test_static_ip", False):
|
|
837
|
-
logger.warning("Static IP test has already been called.")
|
|
838
|
-
return
|
|
839
|
-
self._test_static_ip = True
|
|
840
|
-
|
|
841
868
|
logger.info("Running static IP address configuration test")
|
|
842
869
|
|
|
843
870
|
# Create test message with proper command structure
|
|
@@ -845,8 +872,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
845
872
|
"waitResponse": True,
|
|
846
873
|
"messageId": f"test-static-ip-{datetime.now().timestamp()}",
|
|
847
874
|
"deviceId": message.get("deviceId", "test-device"),
|
|
848
|
-
"command": "
|
|
849
|
-
"timestamp": datetime.now().isoformat(),
|
|
875
|
+
"command": f"{self.name}/set_static_ip_address",
|
|
876
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
850
877
|
"payload": {
|
|
851
878
|
"positioning_system_network_wifi_subnet": "255.255.255.0",
|
|
852
879
|
"positioning_system_network_server_ip_address": "10.0.0.27"
|
|
@@ -857,48 +884,23 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
857
884
|
await self._master_command_handler(topic, test_message)
|
|
858
885
|
logger.info("Static IP address configuration test completed")
|
|
859
886
|
|
|
860
|
-
|
|
861
|
-
"""
|
|
862
|
-
Helper method to create properly formatted command messages for testing.
|
|
863
|
-
|
|
864
|
-
Args:
|
|
865
|
-
command: The command to execute (e.g., "petal-user-journey-coordinator/verify_pos_yaw_directions")
|
|
866
|
-
payload: The payload data for the command
|
|
867
|
-
|
|
868
|
-
Returns:
|
|
869
|
-
Properly formatted MQTT message dict
|
|
870
|
-
"""
|
|
871
|
-
return {
|
|
872
|
-
"waitResponse": True,
|
|
873
|
-
"messageId": f"test-{command.replace('/', '-')}-{datetime.now().timestamp()}",
|
|
874
|
-
"deviceId": "test-device",
|
|
875
|
-
"command": command,
|
|
876
|
-
"timestamp": datetime.now().isoformat(),
|
|
877
|
-
"payload": payload
|
|
878
|
-
}
|
|
879
|
-
|
|
887
|
+
@http_action(method="POST", path="/test/unregister-all-handlers")
|
|
880
888
|
async def _test_unregister_all_handlers(self, topic: str, message: Dict[str, Any]):
|
|
881
889
|
"""
|
|
882
890
|
Test handler that subscribes to multiple streams and then tests unsubscribe all functionality.
|
|
883
891
|
This demonstrates the complete workflow of subscription tracking and bulk unsubscribe.
|
|
884
892
|
"""
|
|
885
|
-
# Allow only one call
|
|
886
|
-
if getattr(self, "_test_unsubscribe_all", False):
|
|
887
|
-
logger.warning("Unsubscribe all test has already been called.")
|
|
888
|
-
return
|
|
889
|
-
self._test_unsubscribe_all = True
|
|
890
|
-
|
|
891
893
|
logger.info("Running unsubscribe all functionality test")
|
|
892
894
|
|
|
893
895
|
# List of streams to subscribe to for testing
|
|
894
896
|
test_subscriptions = [
|
|
895
897
|
{
|
|
896
|
-
"command": "
|
|
898
|
+
"command": f"{self.name}/subscribe_rc_value_stream",
|
|
897
899
|
"stream_id": "px4_rc_raw",
|
|
898
900
|
"data_rate_hz": 20.0
|
|
899
901
|
},
|
|
900
902
|
{
|
|
901
|
-
"command": "
|
|
903
|
+
"command": f"{self.name}/subscribe_pose_value_stream",
|
|
902
904
|
"stream_id": "real_time_pose",
|
|
903
905
|
"data_rate_hz": 10.0
|
|
904
906
|
},
|
|
@@ -917,7 +919,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
917
919
|
"messageId": f"test-subscribe-{subscription['stream_id']}-{datetime.now().timestamp()}",
|
|
918
920
|
"deviceId": message.get("deviceId", "test-device"),
|
|
919
921
|
"command": subscription["command"],
|
|
920
|
-
"timestamp": datetime.now().isoformat(),
|
|
922
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
921
923
|
"payload": {
|
|
922
924
|
"subscribed_stream_id": subscription["stream_id"],
|
|
923
925
|
"data_rate_hz": subscription["data_rate_hz"]
|
|
@@ -948,8 +950,8 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
948
950
|
"waitResponse": True,
|
|
949
951
|
"messageId": f"test-unsubscribe-all-{datetime.now().timestamp()}",
|
|
950
952
|
"deviceId": message.get("deviceId", "test-device"),
|
|
951
|
-
"command": "
|
|
952
|
-
"timestamp": datetime.now().isoformat(),
|
|
953
|
+
"command": f"{self.name}/unsubscribeall",
|
|
954
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
953
955
|
"payload": {}
|
|
954
956
|
}
|
|
955
957
|
|
|
@@ -1622,6 +1624,387 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
1622
1624
|
response_data={"status": "error", "message": str(e)}
|
|
1623
1625
|
)
|
|
1624
1626
|
|
|
1627
|
+
@http_action(method="POST", path="/mqtt/bulk_set_parameters")
|
|
1628
|
+
async def _bulk_set_parameter_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
1629
|
+
"""
|
|
1630
|
+
Handle bulk set parameter MQTT messages.
|
|
1631
|
+
|
|
1632
|
+
This handler processes requests to set multiple parameters in bulk,
|
|
1633
|
+
ensuring no active operations are in progress before applying changes.
|
|
1634
|
+
"""
|
|
1635
|
+
try:
|
|
1636
|
+
# Parse base MQTT message
|
|
1637
|
+
mqtt_msg = MQTTMessage(**message)
|
|
1638
|
+
message_id = mqtt_msg.messageId
|
|
1639
|
+
|
|
1640
|
+
# Check if controller is in emergency mode
|
|
1641
|
+
for controller in self._active_controllers.values():
|
|
1642
|
+
if controller is not None and controller.is_active:
|
|
1643
|
+
error_msg = "Bulk parameter configuration blocked - Active operation in progress"
|
|
1644
|
+
logger.warning(f"[{message_id}] {error_msg}")
|
|
1645
|
+
if mqtt_msg.waitResponse:
|
|
1646
|
+
await self._mqtt_proxy.send_command_response(
|
|
1647
|
+
message_id=message_id,
|
|
1648
|
+
response_data={
|
|
1649
|
+
"status": "error",
|
|
1650
|
+
"message": error_msg,
|
|
1651
|
+
"error_code": "OPERATION_ACTIVE"
|
|
1652
|
+
}
|
|
1653
|
+
)
|
|
1654
|
+
return
|
|
1655
|
+
|
|
1656
|
+
# Process payload using the mavlink proxy bulk set parameter helper
|
|
1657
|
+
payload = BulkParameterSetRequest(**mqtt_msg.payload)
|
|
1658
|
+
parameters = payload.parameters
|
|
1659
|
+
|
|
1660
|
+
if not parameters:
|
|
1661
|
+
error_msg = "No parameters provided for bulk set"
|
|
1662
|
+
logger.error(f"[{message_id}] {error_msg}")
|
|
1663
|
+
if mqtt_msg.waitResponse:
|
|
1664
|
+
await self._mqtt_proxy.send_command_response(
|
|
1665
|
+
message_id=message_id,
|
|
1666
|
+
response_data={
|
|
1667
|
+
"status": "error",
|
|
1668
|
+
"message": error_msg,
|
|
1669
|
+
"error_code": "NO_PARAMETERS_PROVIDED"
|
|
1670
|
+
}
|
|
1671
|
+
)
|
|
1672
|
+
return
|
|
1673
|
+
|
|
1674
|
+
logger.info(f"Setting bulk PX4 parameters for {self.name}")
|
|
1675
|
+
|
|
1676
|
+
results = {}
|
|
1677
|
+
set_param_dict = {}
|
|
1678
|
+
for parameter in parameters:
|
|
1679
|
+
set_param_dict[parameter.parameter_name] = (
|
|
1680
|
+
parameter.parameter_value,
|
|
1681
|
+
parameter.parameter_type
|
|
1682
|
+
)
|
|
1683
|
+
|
|
1684
|
+
confirmed = await self._mavlink_proxy.set_params_bulk_lossy(
|
|
1685
|
+
set_param_dict,
|
|
1686
|
+
max_in_flight=6,
|
|
1687
|
+
resend_interval=0.8,
|
|
1688
|
+
max_retries=5,
|
|
1689
|
+
timeout_total=10.0,
|
|
1690
|
+
)
|
|
1691
|
+
|
|
1692
|
+
if not confirmed:
|
|
1693
|
+
error_msg = "No parameters were confirmed after bulk set"
|
|
1694
|
+
logger.error(f"[{message_id}] {error_msg}")
|
|
1695
|
+
if mqtt_msg.waitResponse:
|
|
1696
|
+
await self._mqtt_proxy.send_command_response(
|
|
1697
|
+
message_id=message_id,
|
|
1698
|
+
response_data={
|
|
1699
|
+
"status": "error",
|
|
1700
|
+
"message": error_msg,
|
|
1701
|
+
"error_code": "NO_PARAMETERS_CONFIRMED"
|
|
1702
|
+
}
|
|
1703
|
+
)
|
|
1704
|
+
return
|
|
1705
|
+
|
|
1706
|
+
# check that all requested parameters were confirmed
|
|
1707
|
+
success = True
|
|
1708
|
+
for parameter in parameters:
|
|
1709
|
+
pname = parameter.parameter_name
|
|
1710
|
+
if pname in confirmed:
|
|
1711
|
+
results[pname] = confirmed[pname]
|
|
1712
|
+
# check that the set value matches the requested value
|
|
1713
|
+
confirmed_value = results[pname].get("value")
|
|
1714
|
+
if confirmed_value == parameter.parameter_value:
|
|
1715
|
+
results[pname]["success"] = True
|
|
1716
|
+
else:
|
|
1717
|
+
results[pname]["success"] = False
|
|
1718
|
+
results[pname]["error"] = f"Parameter value mismatch: requested {parameter.parameter_value}, got {confirmed_value}"
|
|
1719
|
+
success = False
|
|
1720
|
+
else:
|
|
1721
|
+
results[pname] = {
|
|
1722
|
+
"name": pname,
|
|
1723
|
+
"error": "Parameter value could not be retrieved after set",
|
|
1724
|
+
"success": False
|
|
1725
|
+
}
|
|
1726
|
+
success = False
|
|
1727
|
+
|
|
1728
|
+
payload = {
|
|
1729
|
+
"results": results,
|
|
1730
|
+
"success": success,
|
|
1731
|
+
# timestamp must be string
|
|
1732
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
1733
|
+
}
|
|
1734
|
+
|
|
1735
|
+
payload = BulkParameterResponse(**payload)
|
|
1736
|
+
sanitized_response = _json_safe(payload.model_dump())
|
|
1737
|
+
|
|
1738
|
+
logger.info(f"[{message_id}] Successfully processed bulk parameter configuration")
|
|
1739
|
+
|
|
1740
|
+
# Send response if requested
|
|
1741
|
+
if mqtt_msg.waitResponse:
|
|
1742
|
+
if not success:
|
|
1743
|
+
logger.warning(f"[{message_id}] Some parameters could not be set successfully")
|
|
1744
|
+
await self._mqtt_proxy.send_command_response(
|
|
1745
|
+
message_id=message_id,
|
|
1746
|
+
response_data={
|
|
1747
|
+
"status": "error",
|
|
1748
|
+
"message": f"Bulk parameter set completed - some parameters failed",
|
|
1749
|
+
"data": sanitized_response
|
|
1750
|
+
}
|
|
1751
|
+
)
|
|
1752
|
+
else:
|
|
1753
|
+
await self._mqtt_proxy.send_command_response(
|
|
1754
|
+
message_id=message_id,
|
|
1755
|
+
response_data={
|
|
1756
|
+
"status": "success",
|
|
1757
|
+
"message": f"Bulk parameter set completed - {len(results)} parameters processed",
|
|
1758
|
+
"data": sanitized_response
|
|
1759
|
+
}
|
|
1760
|
+
)
|
|
1761
|
+
|
|
1762
|
+
except ValidationError as ve:
|
|
1763
|
+
error_msg = f"Invalid bulk parameter payload: {ve}"
|
|
1764
|
+
logger.error(f"Bulk parameter config validation error: {error_msg}")
|
|
1765
|
+
if mqtt_msg.waitResponse:
|
|
1766
|
+
await self._mqtt_proxy.send_command_response(
|
|
1767
|
+
message_id=message_id,
|
|
1768
|
+
response_data={
|
|
1769
|
+
"status": "error",
|
|
1770
|
+
"message": error_msg,
|
|
1771
|
+
"error_code": "VALIDATION_ERROR"
|
|
1772
|
+
}
|
|
1773
|
+
)
|
|
1774
|
+
|
|
1775
|
+
except Exception as e:
|
|
1776
|
+
error_msg = f"Bulk parameter handler error: {str(e)}"
|
|
1777
|
+
logger.error(f"Unexpected bulk parameter error: {error_msg}")
|
|
1778
|
+
if mqtt_msg.waitResponse:
|
|
1779
|
+
await self._mqtt_proxy.send_command_response(
|
|
1780
|
+
message_id=message_id,
|
|
1781
|
+
response_data={
|
|
1782
|
+
"status": "error",
|
|
1783
|
+
"message": error_msg,
|
|
1784
|
+
"error_code": "HANDLER_ERROR"
|
|
1785
|
+
}
|
|
1786
|
+
)
|
|
1787
|
+
|
|
1788
|
+
@http_action(method="POST", path="/mqtt/bulk_get_parameters")
|
|
1789
|
+
async def _bulk_get_parameter_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
1790
|
+
"""
|
|
1791
|
+
Handle bulk get parameter MQTT messages.
|
|
1792
|
+
|
|
1793
|
+
This handler processes requests to get multiple parameters in bulk,
|
|
1794
|
+
ensuring no active operations are in progress before retrieving values.
|
|
1795
|
+
"""
|
|
1796
|
+
try:
|
|
1797
|
+
# Parse base MQTT message
|
|
1798
|
+
mqtt_msg = MQTTMessage(**message)
|
|
1799
|
+
message_id = mqtt_msg.messageId
|
|
1800
|
+
|
|
1801
|
+
# Check if controller is in emergency mode
|
|
1802
|
+
for controller in self._active_controllers.values():
|
|
1803
|
+
if controller is not None and controller.is_active:
|
|
1804
|
+
error_msg = "Bulk parameter retrieval blocked - Active operation in progress"
|
|
1805
|
+
logger.warning(f"[{message_id}] {error_msg}")
|
|
1806
|
+
if mqtt_msg.waitResponse:
|
|
1807
|
+
await self._mqtt_proxy.send_command_response(
|
|
1808
|
+
message_id=message_id,
|
|
1809
|
+
response_data={
|
|
1810
|
+
"status": "error",
|
|
1811
|
+
"message": error_msg,
|
|
1812
|
+
"error_code": "OPERATION_ACTIVE"
|
|
1813
|
+
}
|
|
1814
|
+
)
|
|
1815
|
+
return
|
|
1816
|
+
|
|
1817
|
+
# Process payload using the mavlink proxy bulk get parameter helper
|
|
1818
|
+
payload = BulkParameterGetRequest(**mqtt_msg.payload)
|
|
1819
|
+
parameter_names = payload.parameter_names
|
|
1820
|
+
|
|
1821
|
+
if not parameter_names:
|
|
1822
|
+
error_msg = "No parameter names provided for bulk get"
|
|
1823
|
+
logger.error(f"[{message_id}] {error_msg}")
|
|
1824
|
+
if mqtt_msg.waitResponse:
|
|
1825
|
+
await self._mqtt_proxy.send_command_response(
|
|
1826
|
+
message_id=message_id,
|
|
1827
|
+
response_data={
|
|
1828
|
+
"status": "error",
|
|
1829
|
+
"message": error_msg,
|
|
1830
|
+
"error_code": "NO_PARAMETER_NAMES_PROVIDED"
|
|
1831
|
+
}
|
|
1832
|
+
)
|
|
1833
|
+
return
|
|
1834
|
+
|
|
1835
|
+
logger.info(f"Getting bulk PX4 parameters for {self.name}")
|
|
1836
|
+
|
|
1837
|
+
parameters = await self._mavlink_proxy.get_params_bulk_lossy(
|
|
1838
|
+
names=parameter_names,
|
|
1839
|
+
max_in_flight=6,
|
|
1840
|
+
resend_interval=0.8,
|
|
1841
|
+
max_retries=5,
|
|
1842
|
+
timeout_total=10.0,
|
|
1843
|
+
inter_send_delay=0.05,
|
|
1844
|
+
)
|
|
1845
|
+
|
|
1846
|
+
if not parameters:
|
|
1847
|
+
error_msg = "No parameters were confirmed after bulk get"
|
|
1848
|
+
logger.error(f"[{message_id}] {error_msg}")
|
|
1849
|
+
if mqtt_msg.waitResponse:
|
|
1850
|
+
await self._mqtt_proxy.send_command_response(
|
|
1851
|
+
message_id=message_id,
|
|
1852
|
+
response_data={
|
|
1853
|
+
"status": "error",
|
|
1854
|
+
"message": error_msg,
|
|
1855
|
+
"error_code": "NO_PARAMETERS_CONFIRMED"
|
|
1856
|
+
}
|
|
1857
|
+
)
|
|
1858
|
+
|
|
1859
|
+
success = True
|
|
1860
|
+
for parameter in parameter_names:
|
|
1861
|
+
if parameter not in parameters:
|
|
1862
|
+
parameters[parameter] = {
|
|
1863
|
+
"name": parameter,
|
|
1864
|
+
"error": "Parameter value could not be retrieved",
|
|
1865
|
+
"success": False
|
|
1866
|
+
}
|
|
1867
|
+
success = False
|
|
1868
|
+
else:
|
|
1869
|
+
parameters[parameter]["success"] = True
|
|
1870
|
+
|
|
1871
|
+
payload = {
|
|
1872
|
+
"results": parameters,
|
|
1873
|
+
"success": success,
|
|
1874
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
1875
|
+
}
|
|
1876
|
+
|
|
1877
|
+
payload = BulkParameterResponse(**payload)
|
|
1878
|
+
sanitized_response = _json_safe(payload.model_dump())
|
|
1879
|
+
|
|
1880
|
+
logger.info(f"[{message_id}] Successfully processed bulk parameter retrieval")
|
|
1881
|
+
|
|
1882
|
+
# Send response if requested
|
|
1883
|
+
if mqtt_msg.waitResponse:
|
|
1884
|
+
if not success:
|
|
1885
|
+
logger.warning(f"[{message_id}] Some parameters could not be retrieved successfully")
|
|
1886
|
+
await self._mqtt_proxy.send_command_response(
|
|
1887
|
+
message_id=message_id,
|
|
1888
|
+
response_data={
|
|
1889
|
+
"status": "error",
|
|
1890
|
+
"message": f"Bulk parameter get completed - some parameters failed",
|
|
1891
|
+
"data": sanitized_response
|
|
1892
|
+
}
|
|
1893
|
+
)
|
|
1894
|
+
else:
|
|
1895
|
+
await self._mqtt_proxy.send_command_response(
|
|
1896
|
+
message_id=message_id,
|
|
1897
|
+
response_data={
|
|
1898
|
+
"status": "success",
|
|
1899
|
+
"message": f"Bulk parameter get completed - {len(parameters)} parameters processed",
|
|
1900
|
+
"data": sanitized_response
|
|
1901
|
+
}
|
|
1902
|
+
)
|
|
1903
|
+
|
|
1904
|
+
except ValidationError as ve:
|
|
1905
|
+
error_msg = f"Invalid bulk parameter get payload: {ve}"
|
|
1906
|
+
logger.error(f"Bulk parameter get validation error: {error_msg}")
|
|
1907
|
+
if mqtt_msg.waitResponse:
|
|
1908
|
+
await self._mqtt_proxy.send_command_response(
|
|
1909
|
+
message_id=message_id,
|
|
1910
|
+
response_data={
|
|
1911
|
+
"status": "error",
|
|
1912
|
+
"message": error_msg,
|
|
1913
|
+
"error_code": "VALIDATION_ERROR"
|
|
1914
|
+
}
|
|
1915
|
+
)
|
|
1916
|
+
|
|
1917
|
+
except Exception as e:
|
|
1918
|
+
error_msg = f"Bulk parameter get handler error: {str(e)}"
|
|
1919
|
+
logger.error(f"Unexpected bulk parameter get error: {error_msg}")
|
|
1920
|
+
if mqtt_msg.waitResponse:
|
|
1921
|
+
await self._mqtt_proxy.send_command_response(
|
|
1922
|
+
message_id=message_id,
|
|
1923
|
+
response_data={
|
|
1924
|
+
"status": "error",
|
|
1925
|
+
"message": error_msg,
|
|
1926
|
+
"error_code": "HANDLER_ERROR"
|
|
1927
|
+
}
|
|
1928
|
+
)
|
|
1929
|
+
|
|
1930
|
+
@http_action(method="POST", path="/mqtt/reboot_px4")
|
|
1931
|
+
async def _reboot_px4_message_handler(self, topic: str, message: Dict[str, Any]):
|
|
1932
|
+
"""Handle reboot PX4 MQTT messages."""
|
|
1933
|
+
try:
|
|
1934
|
+
# Parse base MQTT message
|
|
1935
|
+
mqtt_msg = MQTTMessage(**message)
|
|
1936
|
+
message_id = mqtt_msg.messageId
|
|
1937
|
+
|
|
1938
|
+
# Check if controller is in emergency mode
|
|
1939
|
+
for controller in self._active_controllers.values():
|
|
1940
|
+
if controller is not None and controller.is_active:
|
|
1941
|
+
error_msg = "PX4 reboot blocked - Active operation in progress"
|
|
1942
|
+
logger.warning(f"[{message_id}] {error_msg}")
|
|
1943
|
+
if mqtt_msg.waitResponse:
|
|
1944
|
+
await self._mqtt_proxy.send_command_response(
|
|
1945
|
+
message_id=message_id,
|
|
1946
|
+
response_data={
|
|
1947
|
+
"status": "error",
|
|
1948
|
+
"message": error_msg,
|
|
1949
|
+
"error_code": "OPERATION_ACTIVE"
|
|
1950
|
+
}
|
|
1951
|
+
)
|
|
1952
|
+
|
|
1953
|
+
return
|
|
1954
|
+
|
|
1955
|
+
logger.info(f"Restarting PX4 for {self.name} petal")
|
|
1956
|
+
reboot_response = await self._mavlink_proxy.reboot_autopilot(
|
|
1957
|
+
reboot_onboard_computer=False,
|
|
1958
|
+
timeout=5.0
|
|
1959
|
+
)
|
|
1960
|
+
|
|
1961
|
+
if not reboot_response.success:
|
|
1962
|
+
error_msg = "PX4 reboot command failed or timed out"
|
|
1963
|
+
logger.error(f"[{message_id}] {error_msg}")
|
|
1964
|
+
if mqtt_msg.waitResponse:
|
|
1965
|
+
await self._mqtt_proxy.send_command_response(
|
|
1966
|
+
message_id=message_id,
|
|
1967
|
+
response_data={
|
|
1968
|
+
"status": "error",
|
|
1969
|
+
"message": error_msg,
|
|
1970
|
+
"error_code": "REBOOT_FAILED",
|
|
1971
|
+
"data": _json_safe(reboot_response.model_dump())
|
|
1972
|
+
}
|
|
1973
|
+
)
|
|
1974
|
+
return
|
|
1975
|
+
|
|
1976
|
+
logger.info(f"[{message_id}] PX4 reboot command successful")
|
|
1977
|
+
|
|
1978
|
+
if mqtt_msg.waitResponse:
|
|
1979
|
+
await self._mqtt_proxy.send_command_response(
|
|
1980
|
+
message_id=message_id,
|
|
1981
|
+
response_data={
|
|
1982
|
+
"status": "success",
|
|
1983
|
+
"message": "PX4 reboot command successful",
|
|
1984
|
+
"data": _json_safe(reboot_response.model_dump())
|
|
1985
|
+
}
|
|
1986
|
+
)
|
|
1987
|
+
except ValidationError as ve:
|
|
1988
|
+
error_msg = f"Invalid PX4 reboot payload: {ve}"
|
|
1989
|
+
logger.error(f"PX4 reboot validation error: {error_msg}")
|
|
1990
|
+
if mqtt_msg.waitResponse:
|
|
1991
|
+
await self._mqtt_proxy.send_command_response(
|
|
1992
|
+
message_id=message_id,
|
|
1993
|
+
response_data={
|
|
1994
|
+
"status": "error",
|
|
1995
|
+
"message": error_msg,
|
|
1996
|
+
"error_code": "VALIDATION_ERROR"
|
|
1997
|
+
}
|
|
1998
|
+
)
|
|
1999
|
+
except Exception as e:
|
|
2000
|
+
logger.error(f"Error handling PX4 reboot message: {e}")
|
|
2001
|
+
if mqtt_msg and mqtt_msg.waitResponse:
|
|
2002
|
+
await self._mqtt_proxy.send_command_response(
|
|
2003
|
+
message_id=mqtt_msg.messageId,
|
|
2004
|
+
response_data={"status": "error", "message": str(e)}
|
|
2005
|
+
)
|
|
2006
|
+
return
|
|
2007
|
+
|
|
1625
2008
|
def _create_parameter_message_handler(self, handler_key: str, config_type: str):
|
|
1626
2009
|
"""
|
|
1627
2010
|
Factory method to create parameter message handlers with minimal boilerplate.
|
|
@@ -1983,11 +2366,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
1983
2366
|
|
|
1984
2367
|
return status
|
|
1985
2368
|
|
|
1986
|
-
@http_action(
|
|
1987
|
-
method="GET",
|
|
1988
|
-
path="/health",
|
|
1989
|
-
description="Health check endpoint for this petal"
|
|
1990
|
-
)
|
|
2369
|
+
@http_action(method="GET", path="/health")
|
|
1991
2370
|
async def health_check(self):
|
|
1992
2371
|
"""
|
|
1993
2372
|
Health check endpoint that reports proxy requirements and petal status.
|
|
@@ -1999,7 +2378,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
1999
2378
|
health_info = {
|
|
2000
2379
|
"petal_name": self.name,
|
|
2001
2380
|
"petal_version": self.version,
|
|
2002
|
-
"timestamp": datetime.now().isoformat(),
|
|
2381
|
+
"timestamp": datetime.now(timezone.utc).isoformat(),
|
|
2003
2382
|
"status": "healthy",
|
|
2004
2383
|
"required_proxies": self.get_required_proxies(),
|
|
2005
2384
|
"optional_proxies": self.get_optional_proxies(),
|
|
@@ -2008,11 +2387,7 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2008
2387
|
|
|
2009
2388
|
return health_info
|
|
2010
2389
|
|
|
2011
|
-
@http_action(
|
|
2012
|
-
method="GET",
|
|
2013
|
-
path="/px4-parameter",
|
|
2014
|
-
description="get a specific PX4 parameter"
|
|
2015
|
-
)
|
|
2390
|
+
@http_action(method="GET", path="/px4-parameter")
|
|
2016
2391
|
async def get_px4_parameter(self, data: ParameterRequestModel) -> MavlinkParameterResponseModel:
|
|
2017
2392
|
"""
|
|
2018
2393
|
Get a specific PX4 parameter value.
|
|
@@ -2035,17 +2410,13 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2035
2410
|
return {
|
|
2036
2411
|
"parameter_name": parameter_name,
|
|
2037
2412
|
"parameter_value": parameter_value,
|
|
2038
|
-
"timestamp": datetime.now().isoformat()
|
|
2413
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2039
2414
|
}
|
|
2040
2415
|
except TimeoutError as exc:
|
|
2041
2416
|
logger.error(f"Timeout while waiting for PX4 parameter: {str(exc)}")
|
|
2042
2417
|
raise HTTPException(status_code=504, detail=str(exc))
|
|
2043
2418
|
|
|
2044
|
-
@http_action(
|
|
2045
|
-
method="GET",
|
|
2046
|
-
path="/px4-parameters",
|
|
2047
|
-
description="get a specific PX4 parameter"
|
|
2048
|
-
)
|
|
2419
|
+
@http_action(method="GET", path="/px4-parameters")
|
|
2049
2420
|
async def get_all_parameters(self) -> MavlinkParametersResponseModel:
|
|
2050
2421
|
"""
|
|
2051
2422
|
Get a specific PX4 parameter value.
|
|
@@ -2066,18 +2437,14 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2066
2437
|
)
|
|
2067
2438
|
payload = {
|
|
2068
2439
|
"parameters": parameters,
|
|
2069
|
-
"timestamp": datetime.now().isoformat()
|
|
2440
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2070
2441
|
}
|
|
2071
2442
|
return _json_safe(payload) # ← sanitize before FastAPI encodes
|
|
2072
2443
|
except TimeoutError as exc:
|
|
2073
2444
|
logger.error(f"Timeout while waiting for PX4 parameter: {str(exc)}")
|
|
2074
2445
|
raise HTTPException(status_code=504, detail=str(exc))
|
|
2075
2446
|
|
|
2076
|
-
@http_action(
|
|
2077
|
-
method="POST",
|
|
2078
|
-
path="/px4-parameter",
|
|
2079
|
-
description="set a specific PX4 parameter"
|
|
2080
|
-
)
|
|
2447
|
+
@http_action(method="POST", path="/px4-parameter")
|
|
2081
2448
|
async def set_px4_parameter(self, data: ParameterBaseModel) -> ParameterResponseModel:
|
|
2082
2449
|
"""
|
|
2083
2450
|
Get a specific PX4 parameter value.
|
|
@@ -2111,18 +2478,14 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2111
2478
|
payload = {
|
|
2112
2479
|
"parameter_name": parameter_name,
|
|
2113
2480
|
"parameter_value": parameter_value,
|
|
2114
|
-
"timestamp": datetime.now().isoformat()
|
|
2481
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2115
2482
|
}
|
|
2116
2483
|
return _json_safe(payload) # ← sanitize before FastAPI encodes
|
|
2117
2484
|
except TimeoutError as exc:
|
|
2118
2485
|
logger.error(f"Timeout while waiting for PX4 parameter: {str(exc)}")
|
|
2119
2486
|
raise HTTPException(status_code=504, detail=str(exc))
|
|
2120
2487
|
|
|
2121
|
-
@http_action(
|
|
2122
|
-
method="POST",
|
|
2123
|
-
path="/rotor-count",
|
|
2124
|
-
description="set a specific PX4 parameter"
|
|
2125
|
-
)
|
|
2488
|
+
@http_action(method="POST", path="/rotor-count")
|
|
2126
2489
|
async def set_rotor_count(self, data: RotorCountParameter) -> ParameterResponseModel:
|
|
2127
2490
|
"""
|
|
2128
2491
|
Get a specific PX4 parameter value.
|
|
@@ -2159,9 +2522,160 @@ class PetalUserJourneyCoordinator(Petal):
|
|
|
2159
2522
|
payload = {
|
|
2160
2523
|
"parameter_name": parameter_name,
|
|
2161
2524
|
"parameter_value": parameter_value,
|
|
2162
|
-
"timestamp": datetime.now().isoformat()
|
|
2525
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2163
2526
|
}
|
|
2164
2527
|
return _json_safe(payload) # ← sanitize before FastAPI encodes
|
|
2165
2528
|
except TimeoutError as exc:
|
|
2166
2529
|
logger.error(f"Timeout while waiting for PX4 parameter: {str(exc)}")
|
|
2167
2530
|
raise HTTPException(status_code=504, detail=str(exc))
|
|
2531
|
+
|
|
2532
|
+
@http_action(method="POST", path="/bulk-px4-parameters")
|
|
2533
|
+
async def set_bulk_px4_parameters(self, data: BulkParameterSetRequest) -> BulkParameterResponse:
|
|
2534
|
+
"""
|
|
2535
|
+
Set multiple PX4 parameters in bulk.
|
|
2536
|
+
"""
|
|
2537
|
+
|
|
2538
|
+
parameters = data.parameters
|
|
2539
|
+
|
|
2540
|
+
if not parameters:
|
|
2541
|
+
logger.error("No parameters provided for bulk set")
|
|
2542
|
+
raise HTTPException(
|
|
2543
|
+
status_code=400,
|
|
2544
|
+
detail="No parameters provided",
|
|
2545
|
+
headers={"source": "bulk_px4_parameters"}
|
|
2546
|
+
)
|
|
2547
|
+
|
|
2548
|
+
logger.info(f"Setting bulk PX4 parameters for {self.name}")
|
|
2549
|
+
|
|
2550
|
+
results = {}
|
|
2551
|
+
try:
|
|
2552
|
+
set_param_dict = {}
|
|
2553
|
+
for parameter in parameters:
|
|
2554
|
+
set_param_dict[parameter.parameter_name] = (
|
|
2555
|
+
parameter.parameter_value,
|
|
2556
|
+
parameter.parameter_type
|
|
2557
|
+
)
|
|
2558
|
+
|
|
2559
|
+
confirmed = await self._mavlink_proxy.set_params_bulk_lossy(
|
|
2560
|
+
set_param_dict,
|
|
2561
|
+
max_in_flight=6,
|
|
2562
|
+
resend_interval=0.8,
|
|
2563
|
+
max_retries=5,
|
|
2564
|
+
timeout_total=10.0,
|
|
2565
|
+
)
|
|
2566
|
+
|
|
2567
|
+
# confirmed[pname] = {
|
|
2568
|
+
# "name": pname,
|
|
2569
|
+
# "value": decoded_value,
|
|
2570
|
+
# "raw": float(pkt.param_value),
|
|
2571
|
+
# "type": pkt.param_type,
|
|
2572
|
+
# "count": pkt.param_count,
|
|
2573
|
+
# "index": pkt.param_index,
|
|
2574
|
+
# }
|
|
2575
|
+
|
|
2576
|
+
if not confirmed:
|
|
2577
|
+
logger.error("No parameters were set in bulk PX4 parameter set")
|
|
2578
|
+
raise HTTPException(
|
|
2579
|
+
status_code=500,
|
|
2580
|
+
detail="No parameters were set",
|
|
2581
|
+
headers={"source": "bulk_px4_parameters"}
|
|
2582
|
+
)
|
|
2583
|
+
|
|
2584
|
+
# check that all requested parameters were confirmed
|
|
2585
|
+
success = True
|
|
2586
|
+
for parameter in parameters:
|
|
2587
|
+
pname = parameter.parameter_name
|
|
2588
|
+
if pname in confirmed:
|
|
2589
|
+
results[pname] = confirmed[pname]
|
|
2590
|
+
# check that the set value matches the requested value
|
|
2591
|
+
confirmed_value = results[pname].get("value")
|
|
2592
|
+
if confirmed_value == parameter.parameter_value:
|
|
2593
|
+
results[pname]["success"] = True
|
|
2594
|
+
else:
|
|
2595
|
+
results[pname]["success"] = False
|
|
2596
|
+
results[pname]["error"] = f"Parameter value mismatch: requested {parameter.parameter_value}, got {confirmed_value}"
|
|
2597
|
+
success = False
|
|
2598
|
+
else:
|
|
2599
|
+
results[pname] = {
|
|
2600
|
+
"name": pname,
|
|
2601
|
+
"error": "Parameter value could not be retrieved after set",
|
|
2602
|
+
"success": False
|
|
2603
|
+
}
|
|
2604
|
+
success = False
|
|
2605
|
+
|
|
2606
|
+
payload = {
|
|
2607
|
+
"results": results,
|
|
2608
|
+
"success": success,
|
|
2609
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2610
|
+
}
|
|
2611
|
+
|
|
2612
|
+
payload = BulkParameterResponse(**payload)
|
|
2613
|
+
return _json_safe(payload.model_dump()) # ← sanitize before FastAPI encodes
|
|
2614
|
+
|
|
2615
|
+
except TimeoutError as exc:
|
|
2616
|
+
logger.error(f"Timeout while setting bulk PX4 parameters: {str(exc)}")
|
|
2617
|
+
raise HTTPException(status_code=504, detail=str(exc))
|
|
2618
|
+
except Exception as e:
|
|
2619
|
+
logger.error(f"Error while setting bulk PX4 parameters: {str(e)}")
|
|
2620
|
+
raise HTTPException(status_code=500, detail=str(e))
|
|
2621
|
+
|
|
2622
|
+
@http_action(method="GET", path="/bulk-px4-parameters")
|
|
2623
|
+
async def get_bulk_px4_parameters(self, data: BulkParameterGetRequest) -> BulkParameterResponse:
|
|
2624
|
+
"""
|
|
2625
|
+
Get multiple PX4 parameters in bulk.
|
|
2626
|
+
"""
|
|
2627
|
+
parameter_names = data.parameter_names
|
|
2628
|
+
|
|
2629
|
+
if not parameter_names:
|
|
2630
|
+
logger.error("No parameter names provided for bulk get")
|
|
2631
|
+
raise HTTPException(
|
|
2632
|
+
status_code=400,
|
|
2633
|
+
detail="No parameter names provided",
|
|
2634
|
+
headers={"source": "bulk_px4_parameters"}
|
|
2635
|
+
)
|
|
2636
|
+
|
|
2637
|
+
logger.info(f"Getting bulk PX4 parameters for {self.name}")
|
|
2638
|
+
|
|
2639
|
+
try:
|
|
2640
|
+
|
|
2641
|
+
parameters = await self._mavlink_proxy.get_params_bulk_lossy(
|
|
2642
|
+
names=parameter_names,
|
|
2643
|
+
timeout_total=6.0,
|
|
2644
|
+
max_retries=4,
|
|
2645
|
+
max_in_flight=6,
|
|
2646
|
+
resend_interval=0.8,
|
|
2647
|
+
inter_send_delay=0.02,
|
|
2648
|
+
)
|
|
2649
|
+
|
|
2650
|
+
if not parameters:
|
|
2651
|
+
logger.error("No parameters were retrieved in bulk PX4 parameter get")
|
|
2652
|
+
raise HTTPException(
|
|
2653
|
+
status_code=500,
|
|
2654
|
+
detail="No parameters were retrieved",
|
|
2655
|
+
headers={"source": "bulk_px4_parameters"}
|
|
2656
|
+
)
|
|
2657
|
+
|
|
2658
|
+
success = True
|
|
2659
|
+
for parameter in parameter_names:
|
|
2660
|
+
if parameter not in parameters:
|
|
2661
|
+
parameters[parameter] = {
|
|
2662
|
+
"name": parameter,
|
|
2663
|
+
"error": "Parameter value could not be retrieved",
|
|
2664
|
+
"success": False
|
|
2665
|
+
}
|
|
2666
|
+
success = False
|
|
2667
|
+
else:
|
|
2668
|
+
parameters[parameter]["success"] = True
|
|
2669
|
+
|
|
2670
|
+
|
|
2671
|
+
payload = {
|
|
2672
|
+
"success": success,
|
|
2673
|
+
"results": parameters,
|
|
2674
|
+
"timestamp": datetime.now(timezone.utc).isoformat()
|
|
2675
|
+
}
|
|
2676
|
+
parameters = BulkParameterResponse(**payload)
|
|
2677
|
+
return _json_safe(parameters.model_dump()) # ← sanitize before FastAPI encodes
|
|
2678
|
+
except TimeoutError as exc:
|
|
2679
|
+
logger.error(f"Timeout while getting bulk PX4 parameters: {str(exc)}")
|
|
2680
|
+
raise HTTPException(status_code=504, detail=str(exc))
|
|
2681
|
+
|