fix ROS profiles and simulator reliability
This commit is contained in:
@@ -76,6 +76,13 @@ RMC2 до 0.870 м. С `--sim` остаётся прежний профиль We
|
||||
сетки, тормозной путь и аппаратную аварийную остановку. Документ не сообщает шаг
|
||||
между маркерами и точные имена TF-фреймов. Испытания в Webots не подтверждают
|
||||
готовность к реальному полю.
|
||||
Если значения отличаются, код менять не нужно:
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14 --rows 5 --columns 5 --spacing 1.0 \
|
||||
--base-frame RMC2/base_link --odom-frame RMC2/odom
|
||||
```
|
||||
|
||||
У штатного Webots-драйвера нет таймаута последней команды: при SIGKILL/обрыве питания
|
||||
процесса программная остановка не гарантируется; остановить симулятор вручную.
|
||||
|
||||
|
||||
+18
-9
@@ -1,10 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Модуль Б: один запуск, маршрут туда, пауза эксперта, маршрут обратно.
|
||||
|
||||
python3 module_b.py --target 14 --sim
|
||||
Команды в терминале: Enter / go, stop, resume, return, quit.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import heapq
|
||||
import json
|
||||
@@ -138,11 +132,16 @@ class Field:
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
target = parser.add_mutually_exclusive_group(required=True)
|
||||
target.add_argument("--target", type=int, help="ID целевой метки")
|
||||
target.add_argument("--xy", nargs=2, type=float, help="Координаты цели на карте, метры")
|
||||
parser.add_argument("--sim", action="store_true", help="Часы Webots /clock")
|
||||
parser.add_argument("--rows", type=int, help="Число строк ArUco-сетки")
|
||||
parser.add_argument("--columns", type=int, help="Число столбцов ArUco-сетки")
|
||||
parser.add_argument("--spacing", type=float, help="Шаг между ArUco, м")
|
||||
parser.add_argument("--base-frame", help="TF-фрейм корпуса RMC2")
|
||||
parser.add_argument("--odom-frame", help="TF-фрейм одометрии RMC2")
|
||||
parser.add_argument("--target-yaw", type=float, help="Ориентация на цели в системе поля, рад")
|
||||
parser.add_argument("--auto", action="store_true", help="Только тренировка: без команд эксперта")
|
||||
parser.add_argument("--return-delay", type=float, default=5.0, help="Пауза --auto, секунд симуляции")
|
||||
@@ -157,12 +156,21 @@ def arguments():
|
||||
parser.error("--target-yaw должен быть конечным числом")
|
||||
if args.xy and not all(math.isfinite(v) for v in args.xy):
|
||||
parser.error("--xy должен содержать конечные числа")
|
||||
if args.rows is not None and args.rows < 1:
|
||||
parser.error("--rows должен быть больше нуля")
|
||||
if args.columns is not None and args.columns < 1:
|
||||
parser.error("--columns должен быть больше нуля")
|
||||
if args.spacing is not None and (args.spacing <= 0 or not math.isfinite(args.spacing)):
|
||||
parser.error("--spacing должен быть конечным положительным числом")
|
||||
return args
|
||||
|
||||
|
||||
def main():
|
||||
args = arguments()
|
||||
rows, columns, spacing = SIM_FIELD if args.sim else REAL_FIELD
|
||||
default_rows, default_columns, default_spacing = SIM_FIELD if args.sim else REAL_FIELD
|
||||
rows = args.rows if args.rows is not None else default_rows
|
||||
columns = args.columns if args.columns is not None else default_columns
|
||||
spacing = args.spacing if args.spacing is not None else default_spacing
|
||||
half_size = SIM_HALF_SIZE if args.sim else REAL_HALF_SIZE
|
||||
field = Field(rows, columns, spacing, half_size+SAFETY_MARGIN)
|
||||
target = args.target if args.target is not None else field.nearest(*args.xy)
|
||||
@@ -218,7 +226,8 @@ def main():
|
||||
"safety_margin": SAFETY_MARGIN,
|
||||
}
|
||||
ns = ROBOT_NAMESPACE.strip("/")
|
||||
self.base, self.odom_frame = f"{ns}/base_link", f"{ns}/odom"
|
||||
self.base = args.base_frame or f"{ns}/base_link"
|
||||
self.odom_frame = args.odom_frame or f"{ns}/odom"
|
||||
self.buffer = Buffer()
|
||||
self.listener = TransformListener(self.buffer, self)
|
||||
if not args.sim:
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Сохраняет непохожие кадры с камеры захвата RMC1 для разметки."""
|
||||
|
||||
import argparse
|
||||
import time
|
||||
from datetime import datetime
|
||||
@@ -11,7 +9,7 @@ HERE = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--sim", action="store_true", help="Использовать камеру Webots")
|
||||
parser.add_argument(
|
||||
"--topic",
|
||||
|
||||
+1
-3
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Модуль В: распознать деталь, взять ARM95 и вернуть на полку."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
@@ -46,7 +44,7 @@ def target_name(value):
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--target", help="1/hammer, 2/wrench или 3/pliers")
|
||||
parser.add_argument("--weights", default="models/latest.pt")
|
||||
parser.add_argument("--sim", action="store_true", help="Камера/TF и часы Webots")
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Преобразует polygon-разметку Roboflow в обычные YOLO bounding boxes."""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
@@ -9,7 +7,7 @@ HERE = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--dataset", default="training", help="Каталог Roboflow export")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Рисует bbox, название класса и confidence модели на фотографии."""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
@@ -28,7 +26,7 @@ def select_device(requested, torch):
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("source", help="Фотография для проверки")
|
||||
parser.add_argument("--weights", default="models/latest.pt")
|
||||
parser.add_argument("--output", default="predictions", help="Каталог результата")
|
||||
|
||||
+1
-3
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Обучение YOLO11 на размеченном датасете инструментов."""
|
||||
|
||||
import argparse
|
||||
import shutil
|
||||
from pathlib import Path
|
||||
@@ -32,7 +30,7 @@ def select_device(requested, torch):
|
||||
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--data", default="training/data.yaml", help="data.yaml из Roboflow")
|
||||
parser.add_argument("--model", default="yolo11n.pt", help="Стартовые веса Ultralytics")
|
||||
parser.add_argument("--epochs", type=int, default=30)
|
||||
|
||||
+6
-1
@@ -105,7 +105,12 @@ WebSocket и компоновки интерфейса.
|
||||
`/RMC2/camera_bottom/aruco_id` (и старый `/RMC2/aruco_id` для Webots), а также
|
||||
`/RMC2/lift_status`. По документации реального RMC2 статус лифта имеет QoS
|
||||
`Transient Local` и значения `lowered`, `moving`, `raised`; команды — `0.0` вниз
|
||||
и `0.05` м вверх. Мост настроен именно так.
|
||||
и `0.05` м вверх. В Webots верхнее положение — `0.1` м, поэтому это значение
|
||||
используется по умолчанию. Для физического RMC2 запускайте мост так:
|
||||
|
||||
```bash
|
||||
MVCH_LIFT_UP_HEIGHT=0.05 bun run start
|
||||
```
|
||||
|
||||
Симулятор из открытого пакета не публикует напряжение аккумуляторов. Мост уже
|
||||
подписан на `/RMC*/battery_state` (`sensor_msgs/BatteryState`) и
|
||||
|
||||
@@ -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.05)
|
||||
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05 || command.height === 0.1)
|
||||
case 'gripper':
|
||||
return command.robot === 'RMC1' && (command.state === 'open' || command.state === 'closed')
|
||||
default:
|
||||
|
||||
@@ -1,11 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Small JSON <-> ROS 2 bridge for the module G web interface.
|
||||
|
||||
stdin: one JSON command per line
|
||||
stdout: one JSON state per line (only machine-readable data)
|
||||
stderr: diagnostics
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
@@ -35,6 +28,9 @@ except ImportError:
|
||||
|
||||
ROBOTS = ("RMC1", "RMC2")
|
||||
MANUAL_TIMEOUT = 0.35
|
||||
LIFT_UP_HEIGHT = float(os.environ.get("MVCH_LIFT_UP_HEIGHT", "0.1"))
|
||||
if LIFT_UP_HEIGHT not in (0.05, 0.1):
|
||||
raise SystemExit("MVCH_LIFT_UP_HEIGHT должен быть 0.05 или 0.1")
|
||||
|
||||
|
||||
def empty_robot():
|
||||
@@ -163,7 +159,11 @@ class FmsBridge(Node):
|
||||
self.create_subscription(String, "/RMC2/aruco_id", self.on_aruco, sensor_qos)
|
||||
lift_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
durability=(
|
||||
DurabilityPolicy.TRANSIENT_LOCAL
|
||||
if LIFT_UP_HEIGHT == 0.05
|
||||
else DurabilityPolicy.VOLATILE
|
||||
),
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
@@ -349,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.05 if float(command.get("height", 0)) >= 0.025 else 0.0
|
||||
height = LIFT_UP_HEIGHT if float(command.get("height", 0)) > 0 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,7 +5,7 @@ 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.05 })).toBe(true)
|
||||
expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.1 })).toBe(true)
|
||||
expect(isCommand({ type: 'gripper', robot: 'RMC1', state: 'closed' })).toBe(true)
|
||||
})
|
||||
|
||||
@@ -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.05 })
|
||||
applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.1 })
|
||||
applyMockCommand(state, { type: 'emergency', robot: 'RMC2', active: true })
|
||||
expect(state.robots.RMC2.velocity.x).toBe(0)
|
||||
expect(state.robots.RMC2.liftStatus).toBe('up')
|
||||
|
||||
+1
-1
@@ -387,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.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>
|
||||
<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>
|
||||
)}
|
||||
</Card>
|
||||
</aside>
|
||||
|
||||
@@ -45,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.05 }
|
||||
| { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 | 0.1 }
|
||||
| { type: 'gripper'; robot: 'RMC1'; state: 'open' | 'closed' }
|
||||
|
||||
+27
-11
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Модуль Д: один скрипт для складского кейса RMC1 + RMC2."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
@@ -188,7 +186,7 @@ class Anchor:
|
||||
|
||||
def arguments():
|
||||
global ROWS, COLS, MARKER_SPACING
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--target", help="1/hammer, 2/wrench, 3/pliers")
|
||||
parser.add_argument("--real", action="store_true", help="Физическое поле 5x5 и реальные ROS-топики")
|
||||
parser.add_argument("--spacing", type=float, default=1.0, help="Шаг ArUco-сетки, м")
|
||||
@@ -225,7 +223,7 @@ def arguments():
|
||||
parser.add_argument("--drop-arm-z", type=float, default=0.220)
|
||||
parser.add_argument("--drop-arm-yaw", type=float, default=math.pi / 2)
|
||||
parser.add_argument("--sensor-timeout", type=float, default=3.0)
|
||||
parser.add_argument("--waypoint-timeout", type=float, default=35.0)
|
||||
parser.add_argument("--waypoint-timeout", type=float, default=120.0)
|
||||
args = parser.parse_args()
|
||||
if args.spacing <= 0:
|
||||
parser.error("--spacing должен быть больше нуля")
|
||||
@@ -336,11 +334,12 @@ def build_ros_node(args, event_log):
|
||||
ReliabilityPolicy,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from rclpy.signals import SignalHandlerOptions
|
||||
from sensor_msgs.msg import LaserScan
|
||||
from std_msgs.msg import Bool, Float64, String
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
rclpy.init(signal_handler_options=SignalHandlerOptions.NO)
|
||||
|
||||
class WarehouseNode(Node):
|
||||
def __init__(self):
|
||||
@@ -376,7 +375,11 @@ def build_ros_node(args, event_log):
|
||||
lift_qos = QoSProfile(
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
durability=(
|
||||
DurabilityPolicy.TRANSIENT_LOCAL
|
||||
if args.real
|
||||
else DurabilityPolicy.VOLATILE
|
||||
),
|
||||
)
|
||||
self.create_subscription(
|
||||
String, "/RMC2/lift_status", self.on_lift, lift_qos
|
||||
@@ -465,7 +468,7 @@ def build_ros_node(args, event_log):
|
||||
time.sleep(0.04)
|
||||
|
||||
def set_lift(self, raised, allow_already=False):
|
||||
height = 0.05 if raised else 0.0
|
||||
height = (0.05 if args.real else 0.1) if raised else 0.0
|
||||
expected = "raised" if raised else "lowered"
|
||||
event_log.write("LIFT_COMMAND", state="up" if raised else "down", height=height)
|
||||
with self.lock:
|
||||
@@ -475,7 +478,7 @@ def build_ros_node(args, event_log):
|
||||
time.sleep(0.1)
|
||||
started = time.monotonic()
|
||||
success = False
|
||||
while time.monotonic() - started < 6.0:
|
||||
while time.monotonic() - started < 20.0:
|
||||
with self.lock:
|
||||
status = self.lift_status
|
||||
received_update = self.lift_updates > updates_before
|
||||
@@ -556,10 +559,19 @@ class Motion:
|
||||
carrying=carrying,
|
||||
allow_rack_entry=(rack_entry and index == len(route) - 1)
|
||||
or (rack_exit and index == 1),
|
||||
precise=rack_entry and index == len(route) - 2,
|
||||
)
|
||||
self.log.write("ROUTE_DONE", robot=robot, stage=name, marker=route[-1])
|
||||
|
||||
def drive_to(self, robot, marker, target, carrying=False, allow_rack_entry=False):
|
||||
def drive_to(
|
||||
self,
|
||||
robot,
|
||||
marker,
|
||||
target,
|
||||
carrying=False,
|
||||
allow_rack_entry=False,
|
||||
precise=False,
|
||||
):
|
||||
started = time.monotonic()
|
||||
obstacle_since = None
|
||||
while time.monotonic() - started < self.args.waypoint_timeout:
|
||||
@@ -595,8 +607,10 @@ class Motion:
|
||||
obstacle_since = None
|
||||
|
||||
yaw_error = wrap_angle(target_yaw - yaw)
|
||||
if distance < 0.09:
|
||||
if abs(yaw_error) < 0.10:
|
||||
position_tolerance = 0.03 if precise else 0.09
|
||||
yaw_tolerance = 0.04 if precise else 0.10
|
||||
if distance < position_tolerance:
|
||||
if abs(yaw_error) < yaw_tolerance:
|
||||
self.node.command(robot, immediate=True)
|
||||
self.log.write(
|
||||
"WAYPOINT", robot=robot, marker=marker, error=round(distance, 3)
|
||||
@@ -625,6 +639,8 @@ class Motion:
|
||||
)
|
||||
else:
|
||||
maximum = 0.18 if carrying else 0.27
|
||||
if allow_rack_entry:
|
||||
maximum = min(maximum, 0.10)
|
||||
self.node.command(
|
||||
robot,
|
||||
x=min(maximum, max(0.07, distance * 0.55)),
|
||||
|
||||
Reference in New Issue
Block a user