fix: align modules with physical rover API

This commit is contained in:
2026-09-10 22:06:08 +03:00
parent 0a8894f3b2
commit cb5e751d15
14 changed files with 474 additions and 96 deletions
+1 -1
View File
@@ -33,7 +33,7 @@ export function isCommand(value: unknown): value is Command {
case 'set_goal':
return isPose(command.pose)
case 'lift':
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.1)
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05)
case 'gripper':
return command.robot === 'RMC1' && (command.state === 'open' || command.state === 'closed')
default:
+16 -2
View File
@@ -8,6 +8,7 @@ stderr: diagnostics
import json
import math
import os
import queue
import sys
import threading
@@ -81,6 +82,7 @@ class FmsBridge(Node):
"type": "state",
"timestamp": int(time.time() * 1000),
"mode": "ros",
"fieldSize": max(2, min(10, int(os.environ.get("MVCH_FIELD_SIZE", "5")))),
"bridgeOnline": True,
"bridgeError": None,
"map": None,
@@ -154,8 +156,20 @@ class FmsBridge(Node):
self.create_subscription(OccupancyGrid, "/map", self.on_map, map_qos)
self.create_subscription(OccupancyGrid, "/RMC1/map", self.on_map, map_qos)
# Первый топик физического RMC2, второй оставлен для Webots.
self.create_subscription(
String, "/RMC2/camera_bottom/aruco_id", self.on_aruco, sensor_qos
)
self.create_subscription(String, "/RMC2/aruco_id", self.on_aruco, sensor_qos)
self.create_subscription(String, "/RMC2/lift_status", self.on_lift_status, 10)
lift_qos = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
depth=1,
)
self.create_subscription(
String, "/RMC2/lift_status", self.on_lift_status, lift_qos
)
self.lift_publisher = self.create_publisher(Float64, "/RMC2/lift", 10)
self.gripper_publisher = self.create_publisher(
JointTrajectory,
@@ -335,7 +349,7 @@ class FmsBridge(Node):
self.state["robots"][robot]["goal"] = None
self.state["robots"][robot]["plan"] = []
elif kind == "lift" and robot == "RMC2" and not emergency:
height = 0.1 if float(command.get("height", 0)) >= 0.05 else 0.0
height = 0.05 if float(command.get("height", 0)) >= 0.025 else 0.0
self.lift_publisher.publish(Float64(data=height))
with self.lock:
self.state["robots"][robot]["liftStatus"] = "command: up" if height else "command: down"
+3 -3
View File
@@ -5,12 +5,12 @@ import { applyMockCommand, initialState, tickMock } from './state'
describe('command validation', () => {
test('accepts valid commands', () => {
expect(isCommand({ type: 'manual', robot: 'RMC1', x: 0.4, y: 0.2, yaw: 0 })).toBe(true)
expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.1 })).toBe(true)
expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.05 })).toBe(true)
expect(isCommand({ type: 'gripper', robot: 'RMC1', state: 'closed' })).toBe(true)
})
test('rejects wrong robot tools and non-finite coordinates', () => {
expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.1 })).toBe(false)
expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.05 })).toBe(false)
expect(isCommand({ type: 'gripper', robot: 'RMC2', state: 'open' })).toBe(false)
expect(isCommand({ type: 'set_goal', robot: 'RMC1', pose: { x: Number.NaN, y: 0, yaw: 0 } })).toBe(false)
})
@@ -23,7 +23,7 @@ describe('mock state', () => {
tickMock(state, 1)
expect(state.robots.RMC2.pose.x).toBeCloseTo(0.5)
applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.1 })
applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.05 })
applyMockCommand(state, { type: 'emergency', robot: 'RMC2', active: true })
expect(state.robots.RMC2.velocity.x).toBe(0)
expect(state.robots.RMC2.liftStatus).toBe('up')
+1
View File
@@ -22,6 +22,7 @@ export function initialState(mode: 'ros' | 'mock'): FmsState {
type: 'state',
timestamp: Date.now(),
mode,
fieldSize: mode === 'mock' ? 6 : 5,
bridgeOnline: mode === 'mock',
bridgeError: null,
map: mockMap,