feat(visualization): render vehicle steering feedback

resources/slam-ros-bridge/bridge.py

@@ -31,7 +31,8 @@ from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
 from rclpy.time import Time
-from sensor_msgs.msg import LaserScan, PointCloud2
+from sensor_msgs.msg import JointState, LaserScan, PointCloud2
+from std_msgs.msg import Float64
 from tf2_msgs.msg import TFMessage
 import uvicorn
 
@@ -129,6 +130,10 @@ class VelocityPayload(BaseModel):
     angular: float = 0.0
 
 
+class SteeringPayload(BaseModel):
+    angle: float
+
+
 class NavigationGoalPayload(BaseModel):
     x: float
     y: float
@@ -158,6 +163,9 @@ class SlamNode(Node):
         self.cmd_vel_topic = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel")
         self.map_topic = os.environ.get("MAP_TOPIC", "/map")
         self.plan_topic = os.environ.get("PLAN_TOPIC", "/plan")
+        self.joint_states_topic = os.environ.get("JOINT_STATES_TOPIC", "/joint_states")
+        self.steering_topic = os.environ.get("STEERING_TOPIC", "/mkmini/steering_command")
+        self.steering_feedback_topic = os.environ.get("STEERING_FEEDBACK_TOPIC", "/mkmini/steering_angle")
         self.navigate_to_pose_action = os.environ.get("NAVIGATE_TO_POSE_ACTION", "/navigate_to_pose")
         self.read_only = os.environ.get("READ_ONLY", "false").lower() in ("1", "true", "yes")
 
@@ -190,6 +198,9 @@ class SlamNode(Node):
         self._icp_odom: Optional[Odometry] = None
         self._map: Optional[OccupancyGrid] = None
         self._plan: Optional[NavPath] = None
+        self._joint_positions = {}
+        self._steering_feedback = None
+        self._simulator_steering_angle = None
         self._tf = {}
         self._odom_count = 0
         self._scan_count = 0
@@ -212,6 +223,7 @@ class SlamNode(Node):
         self._topic_status_last_refresh = 0.0
         self._navigation = {"status": "idle", "goal_id": None}
         self._active_goal_handle = None
+        self._requested_initial_pose: Optional[dict] = None
         self._lock = threading.Lock()
 
         self.create_subscription(Odometry, self.odom_topic, self._on_odom, reliable_qos)
@@ -228,9 +240,12 @@ class SlamNode(Node):
         self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, map_qos)
         self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, best_effort_qos)
         self.create_subscription(NavPath, self.plan_topic, self._on_plan, reliable_qos)
+        self.create_subscription(JointState, self.joint_states_topic, self._on_joint_states, reliable_qos)
+        self.create_subscription(Float64, self.steering_feedback_topic, self._on_steering_feedback, reliable_qos)
         self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data)
         self.create_subscription(TFMessage, "/tf_static", self._on_tf, tf_static_qos)
         self._cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+        self._steering_pub = self.create_publisher(Float64, self.steering_topic, 10)
         self._initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
         self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action)
 
@@ -269,6 +284,18 @@ class SlamNode(Node):
         message.pose.covariance[0] = 0.25
         message.pose.covariance[7] = 0.25
         message.pose.covariance[35] = 0.0685
+        with self._lock:
+            # Use this only until AMCL starts publishing map -> odom. It is a
+            # short-lived viewer fallback, not a replacement for live TF.
+            self._requested_initial_pose = {
+                "x": float(x),
+                "y": float(y),
+                "z": 0.0,
+                "yaw": float(yaw),
+                "frame_id": frame_id or "map",
+                "t": time.time(),
+                "expires_at": time.monotonic() + 2.0,
+            }
         self._initial_pose_pub.publish(message)
 
     def save_map(self, name: Optional[str] = None) -> dict:
@@ -425,6 +452,22 @@ class SlamNode(Node):
             self._plan_count += 1
             self._last_plan_wall_time = time.time()
 
+    def _clear_plan_locked(self):
+        """Discard a path that belongs to a completed or replaced navigation goal."""
+        self._plan = None
+        self._last_plan_wall_time = None
+
+    def _on_joint_states(self, msg: JointState):
+        with self._lock:
+            self._joint_positions = {
+                name: position
+                for name, position in zip(msg.name, msg.position)
+            }
+
+    def _on_steering_feedback(self, msg: Float64):
+        with self._lock:
+            self._steering_feedback = float(msg.data)
+
     def topic_status(self) -> dict:
         now = time.time()
         with self._lock:
@@ -563,6 +606,21 @@ class SlamNode(Node):
         msg.angular.z = float(angular)
         self._cmd_vel_pub.publish(msg)
 
+    def set_simulator_steering(self, angle: float) -> None:
+        if self.read_only:
+            raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.")
+        if not math.isfinite(angle):
+            raise RuntimeError("조향각이 올바르지 않습니다.")
+        steering_angle = max(-0.5, min(0.5, float(angle)))
+        command = Float64()
+        command.data = steering_angle
+        with self._lock:
+            self._simulator_steering_angle = steering_angle
+        self._steering_pub.publish(command)
+        drive_command = Twist()
+        drive_command.angular.z = steering_angle
+        self._cmd_vel_pub.publish(drive_command)
+
     def send_navigation_goal(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> dict:
         if self.read_only:
             raise RuntimeError("읽기 전용 3D SLAM 화면에서는 목표점을 설정할 수 없습니다.")
@@ -596,6 +654,7 @@ class SlamNode(Node):
 
         goal_id = bytes(goal_handle.goal_id.uuid).hex()
         with self._lock:
+            self._clear_plan_locked()
             self._navigation = {"status": "running", "goal_id": goal_id}
             self._active_goal_handle = goal_handle
         goal_handle.get_result_async().add_done_callback(
@@ -631,11 +690,13 @@ class SlamNode(Node):
                 return
             self._navigation = {**self._navigation, "status": result_status}
             self._active_goal_handle = None
+            self._clear_plan_locked()
 
     def cancel_navigation(self, wait: bool = False) -> bool:
         with self._lock:
             goal_handle = self._active_goal_handle
             self._navigation = {**self._navigation, "status": "canceled"}
+            self._clear_plan_locked()
         if goal_handle is None:
             return True
 
@@ -662,6 +723,10 @@ class SlamNode(Node):
             scan = self._scan
             grid = self._map
             plan = self._plan
+            amcl_pose = self._amcl_pose
+            requested_initial_pose = dict(self._requested_initial_pose) if self._requested_initial_pose else None
+            joint_positions = dict(self._joint_positions)
+            steering_feedback = self._steering_feedback
             tf_snapshot = dict(self._tf)
             navigation = dict(self._navigation)
         effective_map = self._effective_map(grid, scan, odom)
@@ -671,6 +736,7 @@ class SlamNode(Node):
         pose_map = None
         scan_pose = None
         scan_pose_map = None
+        vehicle = {}
         if odom is not None:
             pos = odom.pose.pose.position
             ori = odom.pose.pose.orientation
@@ -703,6 +769,10 @@ class SlamNode(Node):
                     "source_frame_id": scan.header.frame_id,
                     "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9,
                 }
+            # map -> base is the live localization transform and must win over
+            # /amcl_pose. AMCL may publish a pose only when it changes, while
+            # TF continues updating with odometry; preferring the message made
+            # the viewer appear to stop between AMCL updates.
             map_to_base = resolve_2d_transform(tf_snapshot, "map", odom.child_frame_id)
             if map_to_base is not None:
                 pose_map = {
@@ -712,10 +782,79 @@ class SlamNode(Node):
                     "source_frame_id": odom.header.frame_id,
                     "t": pose["t"],
                 }
+            elif amcl_pose is not None and amcl_pose.header.frame_id.lstrip("/") == "map":
+                amcl_position = amcl_pose.pose.pose.position
+                pose_map = {
+                    "x": amcl_position.x,
+                    "y": amcl_position.y,
+                    "z": amcl_position.z,
+                    "yaw": yaw_from_quaternion(amcl_pose.pose.pose.orientation),
+                    "frame_id": "map",
+                    "child_frame_id": odom.child_frame_id,
+                    "source_frame_id": odom.header.frame_id,
+                    "t": amcl_pose.header.stamp.sec + amcl_pose.header.stamp.nanosec * 1e-9,
+                }
+            elif (
+                requested_initial_pose
+                and requested_initial_pose["frame_id"].lstrip("/") == "map"
+                and time.monotonic() <= requested_initial_pose.get("expires_at", 0.0)
+            ):
+                pose_map = {
+                    **requested_initial_pose,
+                    "child_frame_id": odom.child_frame_id,
+                    "source_frame_id": odom.header.frame_id,
+                }
+
+            # The Gazebo MK-mini uses independent front steering joints. Keep
+            # both values so the viewer renders the Ackermann inner/outer turn.
+            steering_left_angle = next(
+                (
+                    position
+                    for name, position in joint_positions.items()
+                    if name == "front_left_visual_steering_joint" or name.endswith("/front_left_visual_steering_joint")
+                ),
+                None,
+            )
+            steering_right_angle = next(
+                (
+                    position
+                    for name, position in joint_positions.items()
+                    if name == "front_right_visual_steering_joint" or name.endswith("/front_right_visual_steering_joint")
+                ),
+                None,
+            )
+            steering_angle = None
+            if steering_left_angle is not None and steering_right_angle is not None:
+                vehicle["steering_left_angle"] = steering_left_angle
+                vehicle["steering_right_angle"] = steering_right_angle
+                steering_angle = (steering_left_angle + steering_right_angle) * 0.5
+            else:
+                steering_angle = next(
+                    (
+                        position
+                        for name, position in joint_positions.items()
+                        if name == "front_steering_joint" or name.endswith("/front_steering_joint")
+                    ),
+                    None,
+                )
+            if steering_angle is None and steering_feedback is not None:
+                steering_angle = steering_feedback
+            if steering_angle is None:
+                front_fork = resolve_2d_transform(
+                    tf_snapshot,
+                    odom.child_frame_id or "base_link",
+                    "front_fork",
+                )
+                steering_angle = front_fork["yaw"] if front_fork is not None else None
+            if steering_angle is None:
+                steering_angle = self._simulator_steering_angle
+            if steering_angle is not None:
+                vehicle["steering_angle"] = steering_angle
 
         if scan is not None and pose_map is not None:
-            # Resolve map -> sensor directly through the published TF graph so
+            # Prefer direct map -> sensor TF for the current scan. It carries
-            # the scan uses the same localization correction as the map.
+            # the current odometry timestamp and keeps the laser synchronized
+            # with the moving robot.
             map_to_scan = resolve_2d_transform(tf_snapshot, "map", scan.header.frame_id)
             if map_to_scan is not None:
                 scan_pose_map = {
@@ -726,6 +865,21 @@ class SlamNode(Node):
                     "source_frame_id": scan.header.frame_id,
                     "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9,
                 }
+            else:
+                base_to_scan = resolve_2d_transform(
+                    tf_snapshot,
+                    odom.child_frame_id if odom is not None else "base_link",
+                    scan.header.frame_id,
+                )
+                if base_to_scan is not None:
+                    scan_pose_map = {
+                        **compose_2d(pose_map, base_to_scan),
+                        "roll": 0.0,
+                        "pitch": 0.0,
+                        "frame_id": "map",
+                        "source_frame_id": scan.header.frame_id,
+                        "t": pose_map["t"],
+                    }
 
         pose_3d = None
         if icp_odom is not None:
@@ -805,6 +959,7 @@ class SlamNode(Node):
             "scan_pose": scan_pose,
             "scan_pose_map": scan_pose_map,
             "pose_3d": pose_3d,
+            "vehicle": vehicle,
             "scan": scan_payload,
             "point_cloud": point_cloud_payload,
             "map": map_payload,
@@ -1036,6 +1191,17 @@ def cmd_vel(payload: VelocityPayload):
     return {"ok": True}
 
 
+@app.post("/steering")
+def steering(payload: SteeringPayload):
+    if not slam_node:
+        raise HTTPException(status_code=503, detail="ROS node is not ready")
+    try:
+        slam_node.set_simulator_steering(payload.angle)
+    except RuntimeError as error:
+        raise HTTPException(status_code=409, detail=str(error)) from error
+    return {"ok": True}
+
+
 @app.post("/stop")
 def stop():
     if not slam_node: