init
This commit is contained in:
@@ -0,0 +1,42 @@
|
||||
import type { Command, Pose, RobotId } from '../src/types'
|
||||
|
||||
const ROBOTS: RobotId[] = ['RMC1', 'RMC2']
|
||||
|
||||
function isRobot(value: unknown): value is RobotId {
|
||||
return typeof value === 'string' && ROBOTS.includes(value as RobotId)
|
||||
}
|
||||
|
||||
function isFiniteNumber(value: unknown): value is number {
|
||||
return typeof value === 'number' && Number.isFinite(value)
|
||||
}
|
||||
|
||||
function isPose(value: unknown): value is Pose {
|
||||
if (!value || typeof value !== 'object') return false
|
||||
const pose = value as Record<string, unknown>
|
||||
return isFiniteNumber(pose.x) && isFiniteNumber(pose.y) && isFiniteNumber(pose.yaw)
|
||||
}
|
||||
|
||||
export function isCommand(value: unknown): value is Command {
|
||||
if (!value || typeof value !== 'object') return false
|
||||
const command = value as Record<string, unknown>
|
||||
if (!isRobot(command.robot) || typeof command.type !== 'string') return false
|
||||
|
||||
switch (command.type) {
|
||||
case 'manual':
|
||||
return isFiniteNumber(command.x) && isFiniteNumber(command.y) && isFiniteNumber(command.yaw)
|
||||
case 'stop':
|
||||
case 'cancel_goal':
|
||||
return true
|
||||
case 'emergency':
|
||||
return typeof command.active === 'boolean'
|
||||
case 'set_pose':
|
||||
case 'set_goal':
|
||||
return isPose(command.pose)
|
||||
case 'lift':
|
||||
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.1)
|
||||
case 'gripper':
|
||||
return command.robot === 'RMC1' && (command.state === 'open' || command.state === 'closed')
|
||||
default:
|
||||
return false
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
import { dirname, join } from 'node:path'
|
||||
import type { Server, ServerWebSocket } from 'bun'
|
||||
import type { Command, FmsState } from '../src/types'
|
||||
import { isCommand } from './commands'
|
||||
import { applyMockCommand, initialState, tickMock } from './state'
|
||||
|
||||
type WsData = { connectedAt: number }
|
||||
|
||||
const moduleDir = dirname(dirname(import.meta.path))
|
||||
const port = Number(process.env.PORT ?? (process.env.NODE_ENV === 'development' ? 3001 : 3000))
|
||||
const mock = process.env.MOCK_ROS === '1'
|
||||
let state = initialState(mock ? 'mock' : 'ros')
|
||||
let bridge: Bun.Subprocess<'pipe', 'pipe', 'inherit'> | null = null
|
||||
|
||||
function json(data: unknown, status = 200): Response {
|
||||
return Response.json(data, { status, headers: { 'cache-control': 'no-store' } })
|
||||
}
|
||||
|
||||
function sendToBridge(command: Command): boolean {
|
||||
if (mock) {
|
||||
applyMockCommand(state, command)
|
||||
return true
|
||||
}
|
||||
if (!bridge?.stdin) return false
|
||||
bridge.stdin.write(`${JSON.stringify(command)}\n`)
|
||||
bridge.stdin.flush()
|
||||
return true
|
||||
}
|
||||
|
||||
function broadcast(server: Server<WsData>): void {
|
||||
server.publish('telemetry', JSON.stringify(state))
|
||||
}
|
||||
|
||||
async function readBridge(server: Server<WsData>): Promise<void> {
|
||||
if (!bridge?.stdout) return
|
||||
const decoder = new TextDecoder()
|
||||
let pending = ''
|
||||
for await (const chunk of bridge.stdout) {
|
||||
pending += decoder.decode(chunk, { stream: true })
|
||||
const lines = pending.split('\n')
|
||||
pending = lines.pop() ?? ''
|
||||
for (const line of lines) {
|
||||
if (!line.trim()) continue
|
||||
try {
|
||||
const next = JSON.parse(line) as FmsState
|
||||
if (next.type === 'state') {
|
||||
state = next
|
||||
state.mode = 'ros'
|
||||
state.bridgeOnline = true
|
||||
state.bridgeError = null
|
||||
broadcast(server)
|
||||
}
|
||||
} catch {
|
||||
console.error(`[ROS bridge] invalid JSON: ${line.slice(0, 180)}`)
|
||||
}
|
||||
}
|
||||
}
|
||||
state.bridgeOnline = false
|
||||
state.bridgeError = `ROS bridge stopped (code ${await bridge.exited})`
|
||||
broadcast(server)
|
||||
}
|
||||
|
||||
function startBridge(server: Server<WsData>): void {
|
||||
if (mock) {
|
||||
setInterval(() => {
|
||||
tickMock(state)
|
||||
broadcast(server)
|
||||
}, 100)
|
||||
return
|
||||
}
|
||||
|
||||
bridge = Bun.spawn(['python3', join(moduleDir, 'server', 'ros_bridge.py')], {
|
||||
cwd: moduleDir,
|
||||
stdin: 'pipe',
|
||||
stdout: 'pipe',
|
||||
stderr: 'inherit',
|
||||
env: { ...process.env, PYTHONUNBUFFERED: '1' },
|
||||
})
|
||||
void readBridge(server)
|
||||
}
|
||||
|
||||
const server = Bun.serve<WsData>({
|
||||
port,
|
||||
hostname: '0.0.0.0',
|
||||
async fetch(request, server) {
|
||||
const url = new URL(request.url)
|
||||
if (url.pathname === '/ws') {
|
||||
return server.upgrade(request, { data: { connectedAt: Date.now() } })
|
||||
? undefined
|
||||
: new Response('WebSocket upgrade failed', { status: 400 })
|
||||
}
|
||||
if (url.pathname === '/api/health') {
|
||||
return json({ ok: true, mode: state.mode, bridgeOnline: state.bridgeOnline, timestamp: Date.now() })
|
||||
}
|
||||
if (url.pathname === '/api/state') return json(state)
|
||||
if (url.pathname === '/api/command' && request.method === 'POST') {
|
||||
let body: unknown
|
||||
try {
|
||||
body = await request.json()
|
||||
} catch {
|
||||
return json({ ok: false, error: 'invalid JSON' }, 400)
|
||||
}
|
||||
if (!isCommand(body)) return json({ ok: false, error: 'invalid command' }, 400)
|
||||
if (!sendToBridge(body)) return json({ ok: false, error: 'ROS bridge is offline' }, 503)
|
||||
broadcast(server)
|
||||
return json({ ok: true })
|
||||
}
|
||||
|
||||
const relative = url.pathname === '/' ? 'index.html' : url.pathname.slice(1)
|
||||
if (relative.includes('..')) return new Response('Bad path', { status: 400 })
|
||||
const file = Bun.file(join(moduleDir, 'dist', relative))
|
||||
if (await file.exists()) return new Response(file)
|
||||
const fallback = Bun.file(join(moduleDir, 'dist', 'index.html'))
|
||||
return (await fallback.exists()) ? new Response(fallback) : new Response('Run bun run build first', { status: 404 })
|
||||
},
|
||||
websocket: {
|
||||
open(ws) {
|
||||
ws.subscribe('telemetry')
|
||||
ws.send(JSON.stringify(state))
|
||||
},
|
||||
message(ws: ServerWebSocket<WsData>, message) {
|
||||
try {
|
||||
const command: unknown = JSON.parse(String(message))
|
||||
if (isCommand(command)) sendToBridge(command)
|
||||
} catch {
|
||||
ws.send(JSON.stringify({ type: 'error', message: 'invalid command' }))
|
||||
}
|
||||
},
|
||||
},
|
||||
})
|
||||
|
||||
startBridge(server)
|
||||
console.log(`MVCH FMS: http://0.0.0.0:${port} (${mock ? 'mock' : 'ROS'})`)
|
||||
@@ -0,0 +1,504 @@
|
||||
#!/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 queue
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from copy import deepcopy
|
||||
|
||||
import rclpy
|
||||
from builtin_interfaces.msg import Duration
|
||||
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
|
||||
from nav_msgs.msg import OccupancyGrid, Odometry, Path
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from sensor_msgs.msg import BatteryState, LaserScan
|
||||
from std_msgs.msg import Float64, String
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
try:
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
except ImportError:
|
||||
NavigateToPose = None
|
||||
|
||||
|
||||
ROBOTS = ("RMC1", "RMC2")
|
||||
MANUAL_TIMEOUT = 0.35
|
||||
|
||||
|
||||
def empty_robot():
|
||||
return {
|
||||
"online": False,
|
||||
"lastMessageMs": None,
|
||||
"pose": {"x": 0.0, "y": 0.0, "yaw": 0.0},
|
||||
"velocity": {"x": 0.0, "y": 0.0, "yaw": 0.0},
|
||||
"battery": None,
|
||||
"lidar": [],
|
||||
"trail": [],
|
||||
"plan": [],
|
||||
"goal": None,
|
||||
"emergency": False,
|
||||
"arucoId": None,
|
||||
"liftStatus": None,
|
||||
"gripper": "unknown",
|
||||
}
|
||||
|
||||
|
||||
def yaw_from_quaternion(q):
|
||||
siny = 2.0 * (q.w * q.z + q.x * q.y)
|
||||
cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
|
||||
return math.atan2(siny, cosy)
|
||||
|
||||
|
||||
def quaternion_from_yaw(yaw):
|
||||
return 0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)
|
||||
|
||||
|
||||
def wrap_angle(angle):
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
|
||||
def clamp(value, limit):
|
||||
return max(-limit, min(limit, value))
|
||||
|
||||
|
||||
class FmsBridge(Node):
|
||||
def __init__(self):
|
||||
super().__init__("mvch_fms_bridge")
|
||||
self.commands = queue.Queue()
|
||||
self.lock = threading.Lock()
|
||||
self.state = {
|
||||
"type": "state",
|
||||
"timestamp": int(time.time() * 1000),
|
||||
"mode": "ros",
|
||||
"bridgeOnline": True,
|
||||
"bridgeError": None,
|
||||
"map": None,
|
||||
"robots": {robot: empty_robot() for robot in ROBOTS},
|
||||
}
|
||||
self.last_seen = {robot: 0.0 for robot in ROBOTS}
|
||||
self.last_manual = {robot: 0.0 for robot in ROBOTS}
|
||||
self.last_sent_zero = {robot: True for robot in ROBOTS}
|
||||
self.pose_offsets = {robot: {"x": 0.0, "y": 0.0, "yaw": 0.0} for robot in ROBOTS}
|
||||
self.raw_pose = {robot: {"x": 0.0, "y": 0.0, "yaw": 0.0} for robot in ROBOTS}
|
||||
self.goal_handles = {robot: None for robot in ROBOTS}
|
||||
self.simple_goals = {robot: None for robot in ROBOTS}
|
||||
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
durability=DurabilityPolicy.VOLATILE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
map_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
|
||||
self.cmd_publishers = {}
|
||||
self.pose_publishers = {}
|
||||
self.goal_publishers = {}
|
||||
self.nav_clients = {}
|
||||
for robot in ROBOTS:
|
||||
self.cmd_publishers[robot] = self.create_publisher(Twist, f"/{robot}/cmd_vel", 10)
|
||||
self.pose_publishers[robot] = self.create_publisher(
|
||||
PoseWithCovarianceStamped, f"/{robot}/initialpose", 10
|
||||
)
|
||||
self.goal_publishers[robot] = self.create_publisher(PoseStamped, f"/{robot}/goal_pose", 10)
|
||||
if NavigateToPose is not None:
|
||||
self.nav_clients[robot] = ActionClient(
|
||||
self, NavigateToPose, f"/{robot}/navigate_to_pose"
|
||||
)
|
||||
self.create_subscription(
|
||||
Odometry,
|
||||
f"/{robot}/odometry",
|
||||
lambda msg, r=robot: self.on_odometry(r, msg),
|
||||
sensor_qos,
|
||||
)
|
||||
self.create_subscription(
|
||||
LaserScan,
|
||||
f"/{robot}/scan",
|
||||
lambda msg, r=robot: self.on_scan(r, msg),
|
||||
sensor_qos,
|
||||
)
|
||||
self.create_subscription(
|
||||
Path,
|
||||
f"/{robot}/plan",
|
||||
lambda msg, r=robot: self.on_path(r, msg),
|
||||
10,
|
||||
)
|
||||
self.create_subscription(
|
||||
BatteryState,
|
||||
f"/{robot}/battery_state",
|
||||
lambda msg, r=robot: self.on_battery(r, msg.voltage),
|
||||
sensor_qos,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float64,
|
||||
f"/{robot}/battery_voltage",
|
||||
lambda msg, r=robot: self.on_battery(r, msg.data),
|
||||
sensor_qos,
|
||||
)
|
||||
|
||||
self.create_subscription(OccupancyGrid, "/map", self.on_map, map_qos)
|
||||
self.create_subscription(OccupancyGrid, "/RMC1/map", self.on_map, map_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)
|
||||
self.lift_publisher = self.create_publisher(Float64, "/RMC2/lift", 10)
|
||||
self.gripper_publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
"/RMC1/arm95/gripper_trajectory_controller/joint_trajectory",
|
||||
10,
|
||||
)
|
||||
|
||||
self.create_timer(0.05, self.process_commands_and_watchdog)
|
||||
self.create_timer(0.2, self.emit_state)
|
||||
|
||||
def mark_seen(self, robot):
|
||||
self.last_seen[robot] = time.monotonic()
|
||||
|
||||
def on_odometry(self, robot, msg):
|
||||
raw = {
|
||||
"x": float(msg.pose.pose.position.x),
|
||||
"y": float(msg.pose.pose.position.y),
|
||||
"yaw": yaw_from_quaternion(msg.pose.pose.orientation),
|
||||
}
|
||||
with self.lock:
|
||||
self.raw_pose[robot] = raw
|
||||
offset = self.pose_offsets[robot]
|
||||
pose = {
|
||||
"x": raw["x"] + offset["x"],
|
||||
"y": raw["y"] + offset["y"],
|
||||
"yaw": raw["yaw"] + offset["yaw"],
|
||||
}
|
||||
rover = self.state["robots"][robot]
|
||||
rover["pose"] = pose
|
||||
rover["velocity"] = {
|
||||
"x": float(msg.twist.twist.linear.x),
|
||||
"y": float(msg.twist.twist.linear.y),
|
||||
"yaw": float(msg.twist.twist.angular.z),
|
||||
}
|
||||
trail = rover["trail"]
|
||||
if not trail or math.hypot(pose["x"] - trail[-1]["x"], pose["y"] - trail[-1]["y"]) > 0.04:
|
||||
trail.append({"x": pose["x"], "y": pose["y"]})
|
||||
del trail[:-250]
|
||||
self.mark_seen(robot)
|
||||
|
||||
def on_scan(self, robot, msg):
|
||||
with self.lock:
|
||||
pose = self.state["robots"][robot]["pose"]
|
||||
points = []
|
||||
stride = max(1, len(msg.ranges) // 180)
|
||||
for index in range(0, len(msg.ranges), stride):
|
||||
distance = float(msg.ranges[index])
|
||||
if not math.isfinite(distance) or distance < msg.range_min or distance > msg.range_max:
|
||||
continue
|
||||
angle = msg.angle_min + index * msg.angle_increment + pose["yaw"]
|
||||
points.append(
|
||||
{
|
||||
"x": pose["x"] + math.cos(angle) * distance,
|
||||
"y": pose["y"] + math.sin(angle) * distance,
|
||||
}
|
||||
)
|
||||
self.state["robots"][robot]["lidar"] = points[:200]
|
||||
self.mark_seen(robot)
|
||||
|
||||
def on_path(self, robot, msg):
|
||||
with self.lock:
|
||||
self.state["robots"][robot]["plan"] = [
|
||||
{"x": float(p.pose.position.x), "y": float(p.pose.position.y)}
|
||||
for p in msg.poses[:: max(1, len(msg.poses) // 200)]
|
||||
][:200]
|
||||
|
||||
def on_map(self, msg):
|
||||
width, height = int(msg.info.width), int(msg.info.height)
|
||||
if not width or not height:
|
||||
return
|
||||
step = max(1, math.ceil(max(width, height) / 120))
|
||||
sampled = []
|
||||
for y in range(0, height, step):
|
||||
for x in range(0, width, step):
|
||||
sampled.append(int(msg.data[y * width + x]))
|
||||
with self.lock:
|
||||
self.state["map"] = {
|
||||
"width": math.ceil(width / step),
|
||||
"height": math.ceil(height / step),
|
||||
"resolution": float(msg.info.resolution) * step,
|
||||
"origin": {
|
||||
"x": float(msg.info.origin.position.x),
|
||||
"y": float(msg.info.origin.position.y),
|
||||
},
|
||||
"data": sampled,
|
||||
}
|
||||
|
||||
def on_battery(self, robot, value):
|
||||
if math.isfinite(float(value)):
|
||||
with self.lock:
|
||||
self.state["robots"][robot]["battery"] = round(float(value), 2)
|
||||
|
||||
def on_aruco(self, msg):
|
||||
with self.lock:
|
||||
self.state["robots"]["RMC2"]["arucoId"] = str(msg.data)
|
||||
self.mark_seen("RMC2")
|
||||
|
||||
def on_lift_status(self, msg):
|
||||
with self.lock:
|
||||
self.state["robots"]["RMC2"]["liftStatus"] = str(msg.data)
|
||||
|
||||
def publish_twist(self, robot, x=0.0, y=0.0, yaw=0.0):
|
||||
msg = Twist()
|
||||
msg.linear.x = max(-0.7, min(0.7, float(x)))
|
||||
msg.linear.y = max(-0.7, min(0.7, float(y))) if robot == "RMC1" else 0.0
|
||||
msg.angular.z = max(-1.2, min(1.2, float(yaw)))
|
||||
self.cmd_publishers[robot].publish(msg)
|
||||
|
||||
def process_commands_and_watchdog(self):
|
||||
while True:
|
||||
try:
|
||||
command = self.commands.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
try:
|
||||
self.handle_command(command)
|
||||
except Exception as error:
|
||||
print(f"command error: {error}", file=sys.stderr, flush=True)
|
||||
|
||||
now = time.monotonic()
|
||||
with self.lock:
|
||||
emergency = {r: self.state["robots"][r]["emergency"] for r in ROBOTS}
|
||||
simple_goals = deepcopy(self.simple_goals)
|
||||
for robot in ROBOTS:
|
||||
stale = now - self.last_manual[robot] > MANUAL_TIMEOUT
|
||||
if emergency[robot]:
|
||||
self.publish_twist(robot)
|
||||
self.last_sent_zero[robot] = True
|
||||
elif simple_goals[robot] is not None:
|
||||
self.drive_simple_goal(robot, simple_goals[robot])
|
||||
elif stale and not self.last_sent_zero[robot]:
|
||||
self.publish_twist(robot)
|
||||
self.last_sent_zero[robot] = True
|
||||
|
||||
def handle_command(self, command):
|
||||
kind = command.get("type")
|
||||
robot = command.get("robot")
|
||||
if robot not in ROBOTS:
|
||||
return
|
||||
with self.lock:
|
||||
emergency = self.state["robots"][robot]["emergency"]
|
||||
|
||||
if kind == "manual" and not emergency:
|
||||
self.cancel_navigation(robot)
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = None
|
||||
self.state["robots"][robot]["goal"] = None
|
||||
self.state["robots"][robot]["plan"] = []
|
||||
self.publish_twist(robot, command.get("x", 0), command.get("y", 0), command.get("yaw", 0))
|
||||
self.last_manual[robot] = time.monotonic()
|
||||
self.last_sent_zero[robot] = False
|
||||
elif kind == "stop":
|
||||
self.cancel_navigation(robot)
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = None
|
||||
self.publish_twist(robot)
|
||||
self.last_sent_zero[robot] = True
|
||||
elif kind == "emergency":
|
||||
active = bool(command.get("active"))
|
||||
with self.lock:
|
||||
self.state["robots"][robot]["emergency"] = active
|
||||
self.publish_twist(robot)
|
||||
self.last_sent_zero[robot] = True
|
||||
if active:
|
||||
self.cancel_navigation(robot)
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = None
|
||||
elif kind == "set_pose":
|
||||
self.set_pose(robot, command.get("pose", {}))
|
||||
elif kind == "set_goal" and not emergency:
|
||||
self.set_goal(robot, command.get("pose", {}))
|
||||
elif kind == "cancel_goal":
|
||||
self.cancel_navigation(robot)
|
||||
self.publish_twist(robot)
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = None
|
||||
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
|
||||
self.lift_publisher.publish(Float64(data=height))
|
||||
with self.lock:
|
||||
self.state["robots"][robot]["liftStatus"] = "command: up" if height else "command: down"
|
||||
elif kind == "gripper" and robot == "RMC1" and not emergency:
|
||||
self.set_gripper(command.get("state") == "open")
|
||||
|
||||
def set_pose(self, robot, pose):
|
||||
x, y, yaw = float(pose.get("x", 0)), float(pose.get("y", 0)), float(pose.get("yaw", 0))
|
||||
with self.lock:
|
||||
raw = self.raw_pose[robot]
|
||||
self.pose_offsets[robot] = {"x": x - raw["x"], "y": y - raw["y"], "yaw": yaw - raw["yaw"]}
|
||||
self.state["robots"][robot]["pose"] = {"x": x, "y": y, "yaw": yaw}
|
||||
self.state["robots"][robot]["trail"] = [{"x": x, "y": y}]
|
||||
msg = PoseWithCovarianceStamped()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = "map"
|
||||
msg.pose.pose.position.x = x
|
||||
msg.pose.pose.position.y = y
|
||||
qx, qy, qz, qw = quaternion_from_yaw(yaw)
|
||||
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y = qx, qy
|
||||
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w = qz, qw
|
||||
msg.pose.covariance[0] = msg.pose.covariance[7] = 0.25
|
||||
msg.pose.covariance[35] = 0.07
|
||||
self.pose_publishers[robot].publish(msg)
|
||||
|
||||
def make_goal(self, pose):
|
||||
goal = PoseStamped()
|
||||
goal.header.stamp = self.get_clock().now().to_msg()
|
||||
goal.header.frame_id = "map"
|
||||
goal.pose.position.x = float(pose.get("x", 0))
|
||||
goal.pose.position.y = float(pose.get("y", 0))
|
||||
qx, qy, qz, qw = quaternion_from_yaw(float(pose.get("yaw", 0)))
|
||||
goal.pose.orientation.x, goal.pose.orientation.y = qx, qy
|
||||
goal.pose.orientation.z, goal.pose.orientation.w = qz, qw
|
||||
return goal
|
||||
|
||||
def set_goal(self, robot, pose):
|
||||
goal = self.make_goal(pose)
|
||||
self.goal_publishers[robot].publish(goal)
|
||||
target = {"x": goal.pose.position.x, "y": goal.pose.position.y, "yaw": float(pose.get("yaw", 0))}
|
||||
with self.lock:
|
||||
rover = self.state["robots"][robot]
|
||||
rover["goal"] = target
|
||||
if not rover["plan"]:
|
||||
rover["plan"] = [
|
||||
{"x": rover["pose"]["x"], "y": rover["pose"]["y"]},
|
||||
{"x": target["x"], "y": target["y"]},
|
||||
]
|
||||
client = self.nav_clients.get(robot)
|
||||
if client is not None and client.server_is_ready():
|
||||
request = NavigateToPose.Goal()
|
||||
request.pose = goal
|
||||
future = client.send_goal_async(request)
|
||||
future.add_done_callback(lambda f, r=robot: self.on_goal_response(r, f))
|
||||
else:
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = target
|
||||
print(f"{robot}: Nav2 is unavailable; using odometry goal controller", file=sys.stderr, flush=True)
|
||||
|
||||
def drive_simple_goal(self, robot, target):
|
||||
with self.lock:
|
||||
pose = deepcopy(self.state["robots"][robot]["pose"])
|
||||
dx = target["x"] - pose["x"]
|
||||
dy = target["y"] - pose["y"]
|
||||
distance = math.hypot(dx, dy)
|
||||
|
||||
if distance < 0.12:
|
||||
yaw_error = wrap_angle(target["yaw"] - pose["yaw"])
|
||||
if abs(yaw_error) < 0.12:
|
||||
self.publish_twist(robot)
|
||||
with self.lock:
|
||||
self.simple_goals[robot] = None
|
||||
self.last_sent_zero[robot] = True
|
||||
else:
|
||||
self.publish_twist(robot, yaw=clamp(yaw_error * 1.5, 0.65))
|
||||
self.last_sent_zero[robot] = False
|
||||
return
|
||||
|
||||
if robot == "RMC1":
|
||||
cosine, sine = math.cos(pose["yaw"]), math.sin(pose["yaw"])
|
||||
body_x = cosine * dx + sine * dy
|
||||
body_y = -sine * dx + cosine * dy
|
||||
self.publish_twist(
|
||||
robot,
|
||||
x=clamp(body_x * 0.7, 0.38),
|
||||
y=clamp(body_y * 0.7, 0.38),
|
||||
yaw=clamp(wrap_angle(target["yaw"] - pose["yaw"]), 0.45),
|
||||
)
|
||||
else:
|
||||
heading_error = wrap_angle(math.atan2(dy, dx) - pose["yaw"])
|
||||
if abs(heading_error) > 0.28:
|
||||
self.publish_twist(robot, yaw=clamp(heading_error * 1.4, 0.7))
|
||||
else:
|
||||
self.publish_twist(
|
||||
robot,
|
||||
x=min(0.32, max(0.10, distance * 0.65)),
|
||||
yaw=clamp(heading_error * 1.2, 0.45),
|
||||
)
|
||||
self.last_sent_zero[robot] = False
|
||||
|
||||
def on_goal_response(self, robot, future):
|
||||
handle = future.result()
|
||||
if handle.accepted:
|
||||
self.goal_handles[robot] = handle
|
||||
else:
|
||||
print(f"{robot}: Nav2 rejected goal", file=sys.stderr, flush=True)
|
||||
|
||||
def cancel_navigation(self, robot):
|
||||
handle = self.goal_handles.get(robot)
|
||||
if handle is not None:
|
||||
handle.cancel_goal_async()
|
||||
self.goal_handles[robot] = None
|
||||
|
||||
def set_gripper(self, opened):
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = ["left_joint", "right_joint"]
|
||||
point = JointTrajectoryPoint()
|
||||
value = 0.04 if opened else 0.0213
|
||||
point.positions = [value, value]
|
||||
point.time_from_start = Duration(sec=1)
|
||||
msg.points = [point]
|
||||
self.gripper_publisher.publish(msg)
|
||||
with self.lock:
|
||||
self.state["robots"]["RMC1"]["gripper"] = "open" if opened else "closed"
|
||||
|
||||
def emit_state(self):
|
||||
now = time.monotonic()
|
||||
with self.lock:
|
||||
for robot in ROBOTS:
|
||||
age = now - self.last_seen[robot]
|
||||
self.state["robots"][robot]["online"] = age < 2.0
|
||||
self.state["robots"][robot]["lastMessageMs"] = round(age * 1000) if self.last_seen[robot] else None
|
||||
self.state["timestamp"] = int(time.time() * 1000)
|
||||
payload = deepcopy(self.state)
|
||||
print(json.dumps(payload, ensure_ascii=False, separators=(",", ":")), flush=True)
|
||||
|
||||
|
||||
def read_commands(node):
|
||||
for line in sys.stdin:
|
||||
try:
|
||||
command = json.loads(line)
|
||||
if isinstance(command, dict):
|
||||
node.commands.put(command)
|
||||
except json.JSONDecodeError as error:
|
||||
print(f"invalid command JSON: {error}", file=sys.stderr, flush=True)
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = FmsBridge()
|
||||
threading.Thread(target=read_commands, args=(node,), daemon=True).start()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
for robot in ROBOTS:
|
||||
node.publish_twist(robot)
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,31 @@
|
||||
import { describe, expect, test } from 'bun:test'
|
||||
import { isCommand } from './commands'
|
||||
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: '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: 'gripper', robot: 'RMC2', state: 'open' })).toBe(false)
|
||||
expect(isCommand({ type: 'set_goal', robot: 'RMC1', pose: { x: Number.NaN, y: 0, yaw: 0 } })).toBe(false)
|
||||
})
|
||||
})
|
||||
|
||||
describe('mock state', () => {
|
||||
test('emergency stop blocks motion and tool state is retained', () => {
|
||||
const state = initialState('mock')
|
||||
applyMockCommand(state, { type: 'manual', robot: 'RMC2', x: 0.5, y: 0, yaw: 0 })
|
||||
tickMock(state, 1)
|
||||
expect(state.robots.RMC2.pose.x).toBeCloseTo(0.5)
|
||||
|
||||
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')
|
||||
})
|
||||
})
|
||||
@@ -0,0 +1,116 @@
|
||||
import type { Command, FmsState, Point, RobotId, RobotState } from '../src/types'
|
||||
|
||||
const robot = (x: number, y: number): RobotState => ({
|
||||
online: false,
|
||||
lastMessageMs: null,
|
||||
pose: { x, y, yaw: 0 },
|
||||
velocity: { x: 0, y: 0, yaw: 0 },
|
||||
battery: null,
|
||||
lidar: [],
|
||||
trail: [{ x, y }],
|
||||
plan: [],
|
||||
goal: null,
|
||||
emergency: false,
|
||||
arucoId: null,
|
||||
liftStatus: null,
|
||||
gripper: 'unknown',
|
||||
})
|
||||
|
||||
export function initialState(mode: 'ros' | 'mock'): FmsState {
|
||||
const mockMap = mode === 'mock' ? makeMockMap() : null
|
||||
return {
|
||||
type: 'state',
|
||||
timestamp: Date.now(),
|
||||
mode,
|
||||
bridgeOnline: mode === 'mock',
|
||||
bridgeError: null,
|
||||
map: mockMap,
|
||||
robots: { RMC1: robot(-2, 3), RMC2: robot(0, 0) },
|
||||
}
|
||||
}
|
||||
|
||||
function makeMockMap() {
|
||||
const width = 60
|
||||
const height = 60
|
||||
const data = Array<number>(width * height).fill(0)
|
||||
for (let row = 0; row < height; row += 1) {
|
||||
for (let col = 0; col < width; col += 1) {
|
||||
const border = row < 2 || col < 2 || row > height - 3 || col > width - 3
|
||||
const shelf = col >= 7 && col <= 10 && row >= 8 && row <= 46
|
||||
if (border || shelf) data[row * width + col] = 100
|
||||
}
|
||||
}
|
||||
return { width, height, resolution: 0.1, origin: { x: -5.5, y: -0.5 }, data }
|
||||
}
|
||||
|
||||
const clamp = (value: number, min: number, max: number) => Math.max(min, Math.min(max, value))
|
||||
|
||||
export function applyMockCommand(state: FmsState, command: Command): void {
|
||||
const rover = state.robots[command.robot]
|
||||
if (!rover) return
|
||||
|
||||
if (command.type === 'emergency') {
|
||||
rover.emergency = command.active
|
||||
rover.velocity = { x: 0, y: 0, yaw: 0 }
|
||||
} else if (command.type === 'stop' || command.type === 'cancel_goal') {
|
||||
rover.velocity = { x: 0, y: 0, yaw: 0 }
|
||||
if (command.type === 'cancel_goal') {
|
||||
rover.goal = null
|
||||
rover.plan = []
|
||||
}
|
||||
} else if (command.type === 'manual' && !rover.emergency) {
|
||||
rover.velocity = {
|
||||
x: clamp(command.x, -0.7, 0.7),
|
||||
y: command.robot === 'RMC1' ? clamp(command.y, -0.7, 0.7) : 0,
|
||||
yaw: clamp(command.yaw, -1.2, 1.2),
|
||||
}
|
||||
} else if (command.type === 'set_pose') {
|
||||
rover.pose = command.pose
|
||||
rover.trail = [{ x: command.pose.x, y: command.pose.y }]
|
||||
} else if (command.type === 'set_goal') {
|
||||
rover.goal = command.pose
|
||||
rover.plan = [
|
||||
{ x: rover.pose.x, y: rover.pose.y },
|
||||
{ x: command.pose.x, y: command.pose.y },
|
||||
]
|
||||
} else if (command.type === 'lift') {
|
||||
rover.liftStatus = command.height > 0 ? 'up' : 'down'
|
||||
} else if (command.type === 'gripper') {
|
||||
rover.gripper = command.state
|
||||
}
|
||||
state.timestamp = Date.now()
|
||||
}
|
||||
|
||||
export function tickMock(state: FmsState, dt = 0.1): void {
|
||||
const now = Date.now()
|
||||
for (const id of ['RMC1', 'RMC2'] as RobotId[]) {
|
||||
const rover = state.robots[id]
|
||||
rover.online = true
|
||||
rover.lastMessageMs = 0
|
||||
rover.battery = id === 'RMC1' ? 24.4 : 23.9
|
||||
rover.pose.yaw += rover.velocity.yaw * dt
|
||||
const c = Math.cos(rover.pose.yaw)
|
||||
const s = Math.sin(rover.pose.yaw)
|
||||
rover.pose.x += (rover.velocity.x * c - rover.velocity.y * s) * dt
|
||||
rover.pose.y += (rover.velocity.x * s + rover.velocity.y * c) * dt
|
||||
if (rover.trail.length === 0 || Math.hypot(rover.pose.x - rover.trail.at(-1)!.x, rover.pose.y - rover.trail.at(-1)!.y) > 0.03) {
|
||||
rover.trail.push({ x: rover.pose.x, y: rover.pose.y })
|
||||
if (rover.trail.length > 180) rover.trail.shift()
|
||||
}
|
||||
const points: Point[] = []
|
||||
for (let angle = 0; angle < Math.PI * 2; angle += Math.PI / 28) {
|
||||
const distance = 1.0 + 0.35 * Math.sin(angle * 3 + now / 1200)
|
||||
points.push({
|
||||
x: rover.pose.x + Math.cos(angle + rover.pose.yaw) * distance,
|
||||
y: rover.pose.y + Math.sin(angle + rover.pose.yaw) * distance,
|
||||
})
|
||||
}
|
||||
rover.lidar = points
|
||||
if (id === 'RMC2') {
|
||||
const row = Math.max(0, Math.min(5, Math.round(-rover.pose.x)))
|
||||
const col = Math.max(0, Math.min(5, Math.round(rover.pose.y)))
|
||||
rover.arucoId = String(row * 6 + col)
|
||||
}
|
||||
}
|
||||
state.timestamp = now
|
||||
}
|
||||
Reference in New Issue
Block a user