519 lines
20 KiB
Python
519 lines
20 KiB
Python
#!/usr/bin/env python3
|
|
import json
|
|
import math
|
|
import os
|
|
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
|
|
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():
|
|
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",
|
|
"fieldSize": max(2, min(10, int(os.environ.get("MVCH_FIELD_SIZE", "5")))),
|
|
"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)
|
|
# Первый топик физического 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)
|
|
lift_qos = QoSProfile(
|
|
reliability=ReliabilityPolicy.RELIABLE,
|
|
durability=(
|
|
DurabilityPolicy.TRANSIENT_LOCAL
|
|
if LIFT_UP_HEIGHT == 0.05
|
|
else DurabilityPolicy.VOLATILE
|
|
),
|
|
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,
|
|
"/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 = 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"
|
|
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()
|