diff --git a/workspace/src/application_pkg/application_pkg/web_node.py b/workspace/src/application_pkg/application_pkg/web_node.py index 2554bfc..a0fd2b4 100755 --- a/workspace/src/application_pkg/application_pkg/web_node.py +++ b/workspace/src/application_pkg/application_pkg/web_node.py @@ -21,13 +21,26 @@ 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._laser_publisher = self.create_publisher( + String, '/laser/command', 10 + ) + self._gimbal_pid_config_pub = self.create_publisher( + String, '/web/gimbal_pid_config', 10 + ) + self._chassis_mode_pub = self.create_publisher( + String, '/chassis/mode', 10 + ) + self._chassis_pid_config_pub = self.create_publisher( + String, '/web/chassis_pid_config', 10 ) - self.declare_parameter('pid_config_file', 'data/pid_config.json') - self._pid_config_file = self.get_parameter('pid_config_file').value - self._pid_config = self._load_pid_config() + self.declare_parameter('gimbal_pid_config_file', 'data/gimbal_pid_config.json') + self._gimbal_pid_config_file = self.get_parameter('gimbal_pid_config_file').value + self._gimbal_pid_config = self._load_gimbal_pid_config() + + self.declare_parameter('chassis_pid_config_file', 'data/chassis_pid_config.json') + self._chassis_pid_config_file = self.get_parameter('chassis_pid_config_file').value + self._chassis_pid_config = self._load_chassis_pid_config() self._app = Microdot() self._setup_routes() @@ -42,7 +55,7 @@ class WebNode(Node): def _setup_routes(self): app = self._app - @app.route('/api/mode/switch', methods=['POST']) + @app.route('/api/gimbal/mode/switch', methods=['POST']) async def mode_switch(request): try: data = request.json @@ -59,6 +72,23 @@ class WebNode(Node): self._mode_publisher.publish(msg) return {'code': 0, 'message': 'ok'} + @app.route('/api/chassis/mode/switch', methods=['POST']) + async def chassis_mode_switch(request): + try: + data = request.json + except Exception: + return {'code': -1, 'message': 'invalid json'}, 400 + + mode = data.get('mode', '') + valid_modes = ('auto', 'manual', 'debug') + if mode not in valid_modes: + return {'code': -1, 'message': f'unknown mode: {mode}'}, 400 + + msg = String() + msg.data = mode + self._chassis_mode_pub.publish(msg) + return {'code': 0, 'message': 'ok'} + @app.route('/api/laser', methods=['POST']) async def laser(request): try: @@ -68,21 +98,21 @@ class WebNode(Node): action = data.get('action', '') if action == 'on': - mode = 'laser_on' + cmd = 'on' elif action == 'off': - mode = 'laser_off' + cmd = 'off' else: return {'code': -1, 'message': 'action must be on or off'}, 400 msg = String() - msg.data = mode - self._mode_publisher.publish(msg) + msg.data = cmd + self._laser_publisher.publish(msg) return {'code': 0, 'message': 'ok'} - @app.route('/api/pid/config', methods=['GET', 'POST']) - async def pid_config(request): + @app.route('/api/gimbal/pidconfig', methods=['GET', 'POST']) + async def gimbal_pid_config(request): if request.method == 'GET': - return self._pid_config + return self._gimbal_pid_config if request.method == 'POST': try: @@ -90,14 +120,37 @@ class WebNode(Node): except Exception: return {'code': -1, 'message': 'invalid json'}, 400 - if not self._validate_pid_config(data): + if not self._validate_gimbal_pid_config(data): return {'code': -1, 'message': 'invalid pid config schema'}, 400 - self._pid_config = data + self._gimbal_pid_config = data + self._save_gimbal_pid_config() msg = String() msg.data = json.dumps(data) - self._pid_config_publisher.publish(msg) - self.get_logger().info('PID config updated and published') + self._gimbal_pid_config_pub.publish(msg) + self.get_logger().info('Gimbal PID config updated and published') + return {'code': 0, 'message': 'ok'} + + @app.route('/api/chassis/pidconfig', methods=['GET', 'POST']) + async def chassis_pid_config(request): + if request.method == 'GET': + return self._chassis_pid_config + + if request.method == 'POST': + try: + data = request.json + except Exception: + return {'code': -1, 'message': 'invalid json'}, 400 + + if not self._validate_chassis_pid_config(data): + return {'code': -1, 'message': 'invalid pid config schema'}, 400 + + self._chassis_pid_config = data + self._save_chassis_pid_config() + msg = String() + msg.data = json.dumps(data) + self._chassis_pid_config_pub.publish(msg) + self.get_logger().info('Chassis PID config updated and published') return {'code': 0, 'message': 'ok'} def _run_server(self): @@ -107,7 +160,7 @@ class WebNode(Node): pass @staticmethod - def _pid_default(): + def _gimbal_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}, @@ -115,32 +168,60 @@ class WebNode(Node): 'angle_pitch': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, } - def _load_pid_config(self): - if os.path.exists(self._pid_config_file): + @staticmethod + def _chassis_pid_default(): + return { + 'speed': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + 'angle': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + 'position': {'kp': 0.0, 'ki': 0.0, 'kd': 0.0}, + } + + def _load_gimbal_pid_config(self): + return self._load_pid_config_file( + self._gimbal_pid_config_file, + self._gimbal_pid_default(), + self._validate_gimbal_pid_config, + ) + + def _load_chassis_pid_config(self): + return self._load_pid_config_file( + self._chassis_pid_config_file, + self._chassis_pid_default(), + self._validate_chassis_pid_config, + ) + + def _load_pid_config_file(self, path, default, validator): + if os.path.exists(path): try: - with open(self._pid_config_file, 'r') as f: + with open(path, '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}') + if validator(data): + self.get_logger().info(f'Loaded PID config from {path}') 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() + return default - def _save_pid_config(self): + def _save_gimbal_pid_config(self): + self._save_pid_config_file( + self._gimbal_pid_config_file, self._gimbal_pid_config) + + def _save_chassis_pid_config(self): + self._save_pid_config_file( + self._chassis_pid_config_file, self._chassis_pid_config) + + def _save_pid_config_file(self, path, config): try: - os.makedirs(os.path.dirname(self._pid_config_file), exist_ok=True) - 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}') + os.makedirs(os.path.dirname(path), exist_ok=True) + with open(path, 'w') as f: + json.dump(config, f, indent=2) + self.get_logger().info(f'PID config saved to {path}') except OSError as e: self.get_logger().error(f'Failed to save PID config: {e}') @staticmethod - def _validate_pid_config(data): + def _validate_gimbal_pid_config(data): required = ('visual_yaw', 'visual_pitch', 'angle_yaw', 'angle_pitch') pid_keys = ('kp', 'ki', 'kd') for group in required: @@ -152,8 +233,22 @@ class WebNode(Node): return False return True + @staticmethod + def _validate_chassis_pid_config(data): + required = ('speed', 'angle', 'position') + 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() + self._save_gimbal_pid_config() + self._save_chassis_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 1dd32d1..3fbc149 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,17 +45,20 @@ class GimbalNode(Node): Float32MultiArray, '/vision/target', self._target_callback, 10 ) self._pid_config_sub = self.create_subscription( - String, '/web/pid_config', self._pid_config_callback, 10 + String, '/web/gimbal_pid_config', self._pid_config_callback, 10 ) self._mode_sub = self.create_subscription( String, '/gimbal/mode', self._mode_callback, 10 ) + self._laser_sub = self.create_subscription( + String, '/laser/command', self._laser_callback, 10 + ) self._current_mode = 'debug' self.get_logger().info( 'GimbalNode ready - subs: /cmd_gimbal, /vision/target, ' - '/web/pid_config, /gimbal/mode' + '/web/gimbal_pid_config, /gimbal/mode, /laser/command' ) def _write_frame(self, data: bytearray): @@ -141,10 +144,6 @@ class GimbalNode(Node): cmd = CMD_MANUAL elif mode == 'debug': cmd = CMD_DEBUG - elif mode == 'laser_on': - cmd = CMD_LASER_ON - elif mode == 'laser_off': - cmd = CMD_LASER_OFF else: self.get_logger().warn(f'Unknown mode command: {mode}') return @@ -155,6 +154,21 @@ class GimbalNode(Node): data.append(_checksum(data)) self._write_frame(data) + def _laser_callback(self, msg: String): + cmd_str = msg.data.strip().lower() + if cmd_str == 'on': + cmd = CMD_LASER_ON + elif cmd_str == 'off': + cmd = CMD_LASER_OFF + else: + self.get_logger().warn(f'Unknown laser command: {cmd_str}') + return + + self.get_logger().info(f'Sending laser command: 0xA7 cmd=0x{cmd:02X}') + data = bytearray([0xA7, cmd]) + data.append(_checksum(data)) + self._write_frame(data) + def destroy_node(self): if self._ser and self._ser.is_open: self._ser.close()