fix: align modules with physical rover API
This commit is contained in:
@@ -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:
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user