diff --git a/workspace/src/application_pkg/application_pkg/web_node.py b/workspace/src/application_pkg/application_pkg/web_node.py index 24a13f8..bdd5ee7 100755 --- a/workspace/src/application_pkg/application_pkg/web_node.py +++ b/workspace/src/application_pkg/application_pkg/web_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import json -import asyncio +import os import threading import socket @@ -21,6 +21,13 @@ class WebNode(Node): self._mode_publisher = self.create_publisher( String, '/gimbal/mode', 10 ) + self._pid_config_publisher = self.create_publisher( + String, '/web/pid_config', 10 + ) + + self.declare_parameter('pid_config_file', 'pid_config.json') + self._pid_config_file = self.get_parameter('pid_config_file').value + self._pid_config = self._load_pid_config() self._app = Microdot() self._setup_routes() @@ -72,13 +79,80 @@ class WebNode(Node): self._mode_publisher.publish(msg) return {'code': 0, 'message': 'ok'} + @app.route('/api/pid/config', methods=['GET', 'POST']) + async def pid_config(request): + if request.method == 'GET': + return self._pid_config + + if request.method == 'POST': + try: + data = request.json + except Exception: + return {'code': -1, 'message': 'invalid json'}, 400 + + if not self._validate_pid_config(data): + return {'code': -1, 'message': 'invalid pid config schema'}, 400 + + self._pid_config = data + msg = String() + msg.data = json.dumps(data) + self._pid_config_publisher.publish(msg) + self.get_logger().info('PID config updated and published') + return {'code': 0, 'message': 'ok'} + def _run_server(self): try: self._app.run(host='0.0.0.0', port=self._port, debug=False) except RuntimeError: pass + @staticmethod + def _pid_default(): + return { + 'visual_yaw': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + 'visual_pitch': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + 'angle_yaw': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + 'angle_pitch': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + } + + def _load_pid_config(self): + if os.path.exists(self._pid_config_file): + try: + with open(self._pid_config_file, 'r') as f: + data = json.load(f) + if self._validate_pid_config(data): + self.get_logger().info( + f'Loaded PID config from {self._pid_config_file}') + return data + except (json.JSONDecodeError, OSError) as e: + self.get_logger().warn( + f'Failed to load PID config: {e}, using defaults') + return self._pid_default() + + def _save_pid_config(self): + try: + with open(self._pid_config_file, 'w') as f: + json.dump(self._pid_config, f, indent=2) + self.get_logger().info( + f'PID config saved to {self._pid_config_file}') + except OSError as e: + self.get_logger().error(f'Failed to save PID config: {e}') + + @staticmethod + def _validate_pid_config(data): + required = ('visual_yaw', 'visual_pitch', 'angle_yaw', 'angle_pitch') + pid_keys = ('kp', 'ki', 'kd') + for group in required: + entry = data.get(group) + if not isinstance(entry, dict): + return False + for k in pid_keys: + if not isinstance(entry.get(k), (int, float)): + return False + return True + def destroy_node(self): + self._save_pid_config() try: self._app.shutdown() except Exception: diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py index f625027..1dd32d1 100644 --- a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py +++ b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py @@ -45,7 +45,7 @@ class GimbalNode(Node): Float32MultiArray, '/vision/target', self._target_callback, 10 ) self._pid_config_sub = self.create_subscription( - String, '/pid_config', self._pid_config_callback, 10 + String, '/web/pid_config', self._pid_config_callback, 10 ) self._mode_sub = self.create_subscription( String, '/gimbal/mode', self._mode_callback, 10 @@ -55,7 +55,7 @@ class GimbalNode(Node): self.get_logger().info( 'GimbalNode ready - subs: /cmd_gimbal, /vision/target, ' - '/pid_config, /gimbal/mode' + '/web/pid_config, /gimbal/mode' ) def _write_frame(self, data: bytearray):