#!/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()