AimBot/Vision
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: persist gimbal PID config on POST, separate laser to /laser/command topic, add chassis HTTP API endpoints

Browse Source 
- web_node: call _save_gimbal_pid_config() after POST /api/gimbal/pidconfig
- web_node: publish laser commands to /laser/command (was /gimbal/mode)
- web_node: add POST /api/chassis/mode/switch, GET/POST /api/chassis/pidconfig
- web_node: add chassis PID config file management (data/chassis_pid_config.json)
- gimbal_node: subscribe to /laser/command separately from /gimbal/mode
This commit is contained in:
dev01
2026-06-07 13:30:57 +08:00
parent 5752e76576
commit 30565c87cc
2 changed files with 148 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
workspace/src/application_pkg/application_pkg/web_node.py
+128 -33
View File
@@ -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:
workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
+20 -6
View File
@@ -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()