mavxa/chvt-bak
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: align modules with physical rover API

Browse Source
This commit is contained in:
mavxa
2026-09-10 22:06:08 +03:00
parent 0a8894f3b2
commit cb5e751d15
14 changed files with 474 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
module4/README.md
+13 -3
View File
@@ -7,7 +7,7 @@ Tailwind CSS 4, Bun и небольшой ROS 2-мост на Python.
- выбор RMC1 или RMC2;
- положение, скорость и история движения обоих роверов;
- ArUco-граф 6 × 6 с координатами маркеров 0–35;
- ArUco-граф 5 × 5 для физического поля или 6 × 6 для Webots;
- карта занятости RMC1 из `/map`;
- облако точек `/RMC*/scan`, повернутое и перенесённое вместе с ровером;
- план Nav2 из `/RMC*/plan` и текущая целевая точка;
@@ -54,6 +54,13 @@ ros2 launch ar_nav_ros2 bringup_launch.py namespace:=/RMC1 robot_type:=rmc1
использует простой контроллер по одометрии. Он доводит оба ровера до x/y/yaw, но
не строит обход сложных препятствий, поэтому Nav2 для RMC1 предпочтительнее.
Для показанной в PDF физической площадки ничего задавать не нужно: используется
сетка 5 × 5, ID 0–24. В Webots перед запуском моста задайте его сетку 6 × 6:
```bash
export MVCH_FIELD_SIZE=6
```
3. В третьем, также ROS-sourced терминале запустить FMS:
```bash
@@ -94,8 +101,11 @@ WebSocket и компоновки интерфейса.
| План | `/RMC1/plan` | `/RMC2/plan` |
| Рабочий инструмент | `.../gripper_trajectory_controller/joint_trajectory` | `/RMC2/lift` |
Дополнительно используются `/map`, `/RMC2/aruco_id` и
`/RMC2/lift_status`.
Дополнительно используются `/map`, реальный
`/RMC2/camera_bottom/aruco_id` (и старый `/RMC2/aruco_id` для Webots), а также
`/RMC2/lift_status`. По документации реального RMC2 статус лифта имеет QoS
`Transient Local` и значения `lowered`, `moving`, `raised`; команды — `0.0` вниз
и `0.05` м вверх. Мост настроен именно так.
Симулятор из открытого пакета не публикует напряжение аккумуляторов. Мост уже
подписан на `/RMC*/battery_state` (`sensor_msgs/BatteryState`) и
module4/server/commands.ts
+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:
module4/server/ros_bridge.py
+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"
module4/server/state.test.ts
+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')
module4/server/state.ts
+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,
module4/src/App.tsx
+10 -8
View File
@@ -24,6 +24,7 @@ const initialState: FmsState = {
type: 'state',
timestamp: 0,
mode: 'ros',
fieldSize: 5,
bridgeOnline: false,
bridgeError: null,
map: null,
@@ -89,10 +90,11 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
return result.slice(0, 5000)
}, [state.map])
const markers = Array.from({ length: 36 }, (_, id) => ({
const fieldSize = state.fieldSize || 5
const markers = Array.from({ length: fieldSize ** 2 }, (_, id) => ({
id,
x: -Math.floor(id / 6),
y: id % 6,
x: -Math.floor(id / fieldSize),
y: id % fieldSize,
}))
return (
@@ -115,10 +117,10 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
)
})}
{Array.from({ length: 6 }, (_, row) => Array.from({ length: 6 }, (_, col) => {
{Array.from({ length: fieldSize }, (_, row) => Array.from({ length: fieldSize }, (_, col) => {
const here = worldToSvg({ x: -row, y: col })
const right = col < 5 ? worldToSvg({ x: -row, y: col + 1 }) : null
const down = row < 5 ? worldToSvg({ x: -(row + 1), y: col }) : null
const right = col < fieldSize - 1 ? worldToSvg({ x: -row, y: col + 1 }) : null
const down = row < fieldSize - 1 ? worldToSvg({ x: -(row + 1), y: col }) : null
return (
<g key={`edges-${row}-${col}`} stroke="#1e3a5f" strokeWidth="2">
{right && <line x1={here.x} y1={here.y} x2={right.x} y2={right.y} />}
@@ -165,7 +167,7 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
})}
</svg>
<div className="absolute left-3 top-3 rounded-lg bg-slate-950/80 px-3 py-2 text-xs text-slate-300 backdrop-blur">
ArUco: шаг 1 м · X вниз · Y вправо
ArUco: поле {fieldSize}×{fieldSize} · шаг 1 м · X вниз · Y вправо
</div>
<div className="absolute bottom-3 right-3 flex gap-3 rounded-lg bg-slate-950/80 px-3 py-2 text-xs backdrop-blur">
<span className="text-cyan-300">● RMC1</span><span className="text-amber-300">● RMC2</span><span className="text-violet-300">-- маршрут</span>
@@ -385,7 +387,7 @@ export default function App() {
{selected === 'RMC1' ? (
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'gripper', robot: 'RMC1', state: 'open' })} className="rounded-lg bg-cyan-800 px-3 py-2 disabled:opacity-40">Открыть схват</button><button disabled={rover.emergency} onClick={() => send({ type: 'gripper', robot: 'RMC1', state: 'closed' })} className="rounded-lg bg-cyan-800 px-3 py-2 disabled:opacity-40">Закрыть схват</button></div>
) : (
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0.1 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Поднять лифт</button><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Опустить лифт</button></div>
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0.05 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Поднять лифт</button><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Опустить лифт</button></div>
)}
</Card>
</aside>
module4/src/types.ts
+2 -1
View File
@@ -31,6 +31,7 @@ export type FmsState = {
type: 'state'
timestamp: number
mode: 'ros' | 'mock'
fieldSize: number
bridgeOnline: boolean
bridgeError: string | null
map: MapState | null
@@ -44,5 +45,5 @@ export type Command =
| { type: 'set_pose'; robot: RobotId; pose: Pose }
| { type: 'set_goal'; robot: RobotId; pose: Pose }
| { type: 'cancel_goal'; robot: RobotId }
| { type: 'lift'; robot: 'RMC2'; height: 0 | 0.1 }
| { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 }
| { type: 'gripper'; robot: 'RMC1'; state: 'open' | 'closed' }