Switch to Gunsan 2.5D map, add terrain-aware speed control and collision monitor, port motor control to C++

- Port serial_bridge_node's low-level Modbus RTU transport to C++ (fori_serial_bridge_cpp) to remove Python control-loop delay, keeping the existing control algorithm/parameters unchanged
- Switch Nav2 and mock/patrol map to the Gunsan 2.5D site (pcd_gunsan_output), with terrain_speed_node scaling drive speed up on uphill grades and down on downhill grades for both Nav2 and patrol
- Unify Nav2 cruise speed and motor safety-clamp speed at 0.3 m/s
- Add nav2_collision_monitor as a final safety gate against sudden dynamic obstacles, routed so it can't be bypassed by velocity_smoother's internal remap
- Fix web dashboard map rendering (stray map_server process, oversized GetMap payload, rosbridge fragment size vs. roslib.js incompatibility) and PWA reachability via mDNS
- Fix TF tree: wheel-odometry broadcasts odom->base_link, and camera_init is statically bridged to both odom and map so AMCL's own map->odom broadcast no longer conflicts with the SLAM anchor frame

src/FAST_LIO

@@ -1 +1 @@
-/home/yoo/FAST_LIO
+/home/yoo/duru_lio_ws

src/fori_collision_monitor_params.yaml

@@ -0,0 +1,59 @@
+# Nav2 Collision Monitor: 마지막 안전 게이트.
+#
+# terrain_speed_node가 낸 /cmd_vel_terrain(Nav2 순찰/자율주행 + parking_controller의
+# 도킹/순찰 명령 모두 여기로 합쳐짐)을 받아서, /scan 라이다 데이터를 기준으로 갑자기
+# 튀어나온 사람/장애물이 있으면 감속하거나(SlowZone) 완전 정지시킨(StopZone) 뒤 최종
+# /cmd_vel로 내보낸다. FootprintApproach는 현재 속도 명령대로 주행했을 때
+# time_before_collision초 안에 로봇 발자국(costmap footprint)이 장애물과 닿는지
+# 시뮬레이션해서 미리 감속하는 예측형 정지 로직이다.
+#
+# 로봇 사양(fori_nav2_params.yaml의 robot_radius: 0.38m, 순항속도 0.3m/s)에 맞춰
+# 반경/시간을 잡았다. 실제 부지에서 저속 시운전하며 StopZone/SlowZone 반경과
+# slowdown_ratio를 조정할 것을 권장.
+collision_monitor:
+  ros__parameters:
+    use_sim_time: False
+    base_frame_id: "base_link"
+    odom_frame_id: "odom"
+    cmd_vel_in_topic: "cmd_vel_terrain"
+    cmd_vel_out_topic: "cmd_vel"
+    transform_tolerance: 0.5
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    polygons: ["StopZone", "SlowZone", "FootprintApproach"]
+    # 로봇 중심에서 0.45m(로봇 반경 0.38m + 여유 0.07m) 이내에 장애물이 잡히면 완전 정지.
+    StopZone:
+      type: "circle"
+      radius: 0.45
+      action_type: "stop"
+      max_points: 3
+      visualize: True
+      polygon_pub_topic: "collision_monitor/stop_zone"
+      enabled: True
+    # 0.90m 이내로 접근하면 지령 속도를 40%로 줄여 미리 서행.
+    SlowZone:
+      type: "circle"
+      radius: 0.90
+      action_type: "slowdown"
+      slowdown_ratio: 0.4
+      max_points: 3
+      visualize: True
+      polygon_pub_topic: "collision_monitor/slow_zone"
+      enabled: True
+    # 현재 속도 명령으로 1.5초 앞을 시뮬레이션해서 로봇 발자국이 장애물과 겹치면
+    # 미리 감속 (local_costmap이 robot_radius 기준으로 자동 발행하는 발자국 사용).
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.5
+      simulation_time_step: 0.1
+      max_points: 3
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "/scan"
+      enabled: True

src/fori_nav2.launch.py

@@ -1,20 +1,31 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
+from launch.actions import GroupAction, IncludeLaunchDescription, SetEnvironmentVariable
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, SetRemap
 
 def generate_launch_description():
     # --- [경로 설정 - 사용자 환경에 맞춰 수정] ---
-    # 1. 2D 지도 파일 경로 (데스크탑에 있는 파일 기준)
+    # 1. 2D 지도 파일 경로 (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물)
-    map_yaml_file = '/home/yoo/fori_map.yaml'
+    map_yaml_file = '/home/yoo/pcd_gunsan_output/map.yaml'
     
     # 2. Nav2 파라미터 파일 경로
-    nav2_params_file = '/home/yoo/fori_ws/src/fori_nav2_params.yaml'
+    nav2_params_file = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2_params.yaml')
+    if not os.path.exists(nav2_params_file):
+        nav2_params_file = os.path.expanduser('~/fori_ws/src/fori_nav2_params.yaml')
+
+    # 2b. Collision Monitor 파라미터 파일 경로 (급출현 장애물 감속/정지 안전계층)
+    collision_monitor_params_file = os.path.expanduser(
+        '~/fori_ws/fori_ws/src/fori_collision_monitor_params.yaml')
+    if not os.path.exists(collision_monitor_params_file):
+        collision_monitor_params_file = os.path.expanduser(
+            '~/fori_ws/src/fori_collision_monitor_params.yaml')
     
     # 3. URDF 파일 경로 (분석된 full workspace 내 경로)
+    urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
+    if not os.path.exists(urdf_file_path):
         urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
 
     # 패키지 경로 획득
@@ -34,6 +45,46 @@ def generate_launch_description():
         output='screen'
     )
 
+    # 2단계: Static TF for camera_init -> odom (FAST-LIO2/FAST_LIO uses camera_init, Nav2 uses odom)
+    static_tf_camera_init_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_camera_init_to_odom',
+        arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'odom'],
+        output='screen'
+    )
+
+    # 2b단계: Static TF for camera_init -> map. AMCL이 표준대로 map->odom을 발행하면
+    # odom의 부모를 camera_init(위 정적 브릿지)과 map(AMCL)이 동시에 주장하게 돼 tf2
+    # 트리가 깨진다(실측으로 base_link<->odom "unconnected trees" 확인함). 그래서
+    # AMCL은 tf_broadcast:false로 TF 발행을 끄고(fori_nav2_params.yaml), 대신 map도
+    # odom과 마찬가지로 camera_init의 자식으로 정적 연결한다 - map과 odom은 형제라 충돌이
+    # 없다. AMCL은 여전히 /amcl_pose(스캔 매칭 기반 정밀 위치)는 정상 발행하므로
+    # terrain_speed_node/parking_controller_node의 위치 소스로는 계속 쓸 수 있다.
+    static_tf_camera_init_to_map_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_camera_init_to_map',
+        arguments=['0', '0', '0', '0', '0', '0', 'camera_init', 'map'],
+        output='screen'
+    )
+
+    # 3단계-B: FAST_LIO (카메라 불필요, LiDAR+IMU만 사용하는 LIO). FAST-LIVO2와 달리
+    # 카메라 연결 없이도 /cloud_registered + camera_init->aft_mapped TF를 발행해서
+    # AMCL이 map->odom을 낼 수 있게 해준다(child_frame_id를 URDF 루트 이름인
+    # "aft_mapped"에 맞춰 FAST_LIO/src/laserMapping.cpp를 수정해서 씀 - 원본 FAST_LIO는
+    # "body"를 써서 그대로 두면 TF 트리가 끊긴다). 라이다 드라이버
+    # (livox_ros_driver2 mid360s_fastlivo_launch.py)는 별도 터미널에서 먼저 띄워야 한다.
+    fast_lio_dir = get_package_share_directory('fast_lio')
+    fast_lio_config = os.path.join(fast_lio_dir, 'config', 'mid360s.yaml')
+    fast_lio_node = Node(
+        package='fast_lio',
+        executable='fastlio_mapping',
+        name='fastlio_mapping',
+        parameters=[fast_lio_config, {'use_sim_time': False}],
+        output='screen'
+    )
+
     # 3단계-A: 3D PointCloud -> 2D Scan 변환
     pc_to_ls_node = Node(
         package='pointcloud_to_laserscan',
@@ -62,8 +113,65 @@ def generate_launch_description():
         }.items()
     )
 
+    # Nav2 humble bringup은 controller_server가 낸 cmd_vel을 velocity_smoother가 받아
+    # 감속 램프를 적용한 뒤 'cmd_vel_smoothed'라는 내부 이름으로 최종 'cmd_vel'에 발행한다
+    # (navigation_launch.py에 하드코딩된 리맵: controller_server의 ('cmd_vel','cmd_vel_nav'),
+    # velocity_smoother의 ('cmd_vel','cmd_vel_nav')+('cmd_vel_smoothed','cmd_vel')). 즉
+    # 실제로 모터까지 가는 "진짜 최종" 순찰/자율주행 명령은 velocity_smoother의 출력이다.
+    # 바깥 이름 'cmd_vel'을 통째로 가로채면(처음 시도했던 방식) velocity_smoother 내부의
+    # 'cmd_vel'->'cmd_vel_nav' 입력 리맵까지 덮어써서 오히려 스무딩 단계를 건너뛰게 되고,
+    # 정작 진짜 최종 출력('cmd_vel_smoothed'->'cmd_vel')은 그대로 남아 terrain_speed_node/
+    # collision_monitor를 완전히 우회해버린다(실측으로 확인함). 그래서 velocity_smoother의
+    # 내부 입력 리맵은 건드리지 않고, 그 최종 출력 이름('cmd_vel_smoothed')만 정확히
+    # /cmd_vel_raw로 리다이렉트한다.
+    #
+    # [알려진 한계] behavior_server(spin/backup/drive_on_heading 복구 동작)는
+    # navigation_launch.py에서 cmd_vel 리맵이 없어 velocity_smoother를 거치지 않고 bare
+    # 'cmd_vel'에 직접 낸다 - 리맵 스코프가 GroupAction 단위라 controller_server의
+    # cmd_vel->cmd_vel_nav 페어링을 건드리지 않으면서 behavior_server만 선택적으로
+    # 가로챌 방법이 없다(nav2_bringup 내부 launch 파일을 포크하지 않는 한). 즉 복구 동작
+    # 중에는 terrain_speed_node의 경사 보정과 collision_monitor의 급출현 장애물 감속/정지가
+    # 적용되지 않는다. 다만 (1) 복구는 costmap이 이미 경로 막힘을 감지한 뒤에만 트리거되고
+    # (2) 각 복구 동작 자체가 저속·단거리로 제한돼 있고 (3) serial_bridge_node의
+    # target_linear_speed(0.3m/s) 하드 클램프는 경로와 무관하게 항상 적용되므로, 남은
+    # 위험은 제한적이다.
+    nav2_launch_group = GroupAction([
+        SetRemap('cmd_vel_smoothed', 'cmd_vel_raw'),
+        nav2_launch,
+    ])
+
+    # 5단계: Collision Monitor (최종 안전 게이트) - /cmd_vel_terrain(=terrain_speed_node
+    # 출력, Nav2 순찰/자율주행 + parking_controller 도킹/순찰 명령이 모두 여기로 합쳐짐)을
+    # 받아 /scan 기준으로 급출현 장애물을 감속/정지시킨 뒤 최종 /cmd_vel로 발행한다.
+    # Nav2 자체 lifecycle_manager가 관리하는 노드 목록에는 없으므로 별도 lifecycle_manager로
+    # 직접 configure/activate 시킨다.
+    collision_monitor_node = Node(
+        package='nav2_collision_monitor',
+        executable='collision_monitor',
+        name='collision_monitor',
+        parameters=[collision_monitor_params_file],
+        output='screen'
+    )
+
+    collision_monitor_lifecycle_manager = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_collision_monitor',
+        parameters=[{
+            'use_sim_time': False,
+            'autostart': True,
+            'node_names': ['collision_monitor']
+        }],
+        output='screen'
+    )
+
     return LaunchDescription([
         rsp_node,
+        static_tf_camera_init_node,
+        static_tf_camera_init_to_map_node,
+        fast_lio_node,
         pc_to_ls_node,
-        nav2_launch
+        nav2_launch_group,
+        collision_monitor_node,
+        collision_monitor_lifecycle_manager,
     ])

src/fori_nav2_params.yaml

@@ -1,6 +1,6 @@
 map_server:
   ros__parameters:
-    yaml_filename: "/home/yoo/fori_map.yaml"
+    yaml_filename: "/home/yoo/pcd_gunsan_output/map.yaml"
     use_sim_time: False
 amcl:
   ros__parameters:
@@ -12,16 +12,23 @@ amcl:
     alpha4: 0.2
     base_frame_id: "base_link"
     global_frame_id: "map"
-    odom_frame_id: "camera_init" # FAST-LIO2의 오도메트리 프레임
+    odom_frame_id: "odom"
     scan_topic: "scan"
     map_topic: "map"
     set_initial_pose: true
+    # fast_lio(LIDAR SLAM)가 camera_init->aft_mapped->base_link TF를 이미 발행하고,
+    # camera_init은 fori_nav2.launch.py의 static_tf_camera_init_node가 odom과 동일시해서
+    # 브릿지한다. AMCL이 표준대로 map->odom을 또 발행하면 odom의 부모를 두 군데(camera_init
+    # 정적 브릿지 vs AMCL의 map->odom)에서 동시에 주장하게 돼 tf2가 base_link<->odom
+    # 연결을 못 찾는 "unconnected trees" 오류로 이어진다. tf_broadcast를 꺼서 AMCL은
+    # /amcl_pose(위치 추정값)만 내고 TF는 발행하지 않게 한다.
+    tf_broadcast: false
 
 behavior_server:
   ros__parameters:
     local_frame: base_link
     global_frame: map
-    odom_frame: camera_init  # <--- 이 부분이 odom 에러를 해결합니다.
+    odom_frame: odom
     use_sim_time: False
     device_id: "robot"
     simulate_ahead_time: 2.0
@@ -31,7 +38,12 @@ bt_navigator:
     use_sim_time: False
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /aft_mapped_to_init # FAST-LIO2의 위치 토픽
+    odom_topic: /odom
+    default_nav_to_pose_bt_xml: ""        # Use Nav2 built-in default BehaviorTree
+    default_nav_through_poses_bt_xml: ""  # Use Nav2 built-in default BehaviorTree
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
 
 controller_server:
   ros__parameters:
@@ -50,7 +62,7 @@ controller_server:
       yaw_goal_tolerance: 0.35
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      desired_linear_vel: 0.25
+      desired_linear_vel: 0.3
       lookahead_dist: 0.6
       min_lookahead_dist: 0.3
       max_lookahead_dist: 0.9
@@ -65,7 +77,7 @@ local_costmap:
     ros__parameters:
       update_frequency: 5.0
       publish_frequency: 2.0
-      global_frame: camera_init # 지역 지도는 오도메트리 기준
+      global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
       transform_tolerance: 1.0

src/fori_serial_bridge/fori_serial_bridge/odom_relay_node.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+"""
+Simple relay node: /odom_wheels -> /odom
+Provides standard /odom topic for Nav2 from wheel odometry source.
+No external topic_tools package dependency required.
+"""
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from rclpy.qos import qos_profile_sensor_data
+
+
+class OdomRelayNode(Node):
+    def __init__(self):
+        super().__init__('odom_relay')
+        self.pub = self.create_publisher(Odometry, '/odom', 10)
+        self.sub = self.create_subscription(
+            Odometry, '/odom_wheels', self.callback, qos_profile_sensor_data
+        )
+        self.get_logger().info('OdomRelay: /odom_wheels -> /odom')
+
+    def callback(self, msg):
+        # Ensure frame_id is 'odom' for Nav2 compatibility
+        msg.header.frame_id = 'odom'
+        msg.child_frame_id = 'base_link'
+        self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomRelayNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py

@@ -39,14 +39,12 @@ class ParkingControllerNode(Node):
         self.kp_yaw = self.get_parameter('kp_yaw').value
 
         # Declare patrol waypoints as parameter (flat list of x, y, yaw)
-        # Default patrol coordinates following the 100% obstacle-free map circuit
+        # No default coordinates: these were tuned for the old indoor map and are
-        default_patrol = [
+        # meaningless (possibly unsafe) on the new Gunsan site map. Patrol falls back
-            1.10, -1.35, 2.35,
+        # to "go home and scan" until the user supplies real waypoints for the new
-            -2.20, 2.00, 3.14,
+        # map via this parameter (see send_patrol_goal/advance_patrol below, which
-            -3.00, 0.50, -1.57,
+        # already append the home position regardless of this list's length).
-            -1.80, 0.80, -0.78,
+        default_patrol = []
-            -0.50, 0.00, -0.78
-        ]
         self.declare_parameter('patrol_waypoints', default_patrol)
         self.patrol_waypoints_raw = self.get_parameter('patrol_waypoints').value
         self.patrol_waypoints = []
@@ -58,6 +56,13 @@ class ParkingControllerNode(Node):
                     'yaw': self.patrol_waypoints_raw[i+2]
                 })
 
+        if not self.patrol_waypoints:
+            self.get_logger().warn(
+                '[PATROL] patrol_waypoints 파라미터가 비어 있습니다 (신규 군산 지도용 좌표 미설정). '
+                '순찰 시작 시 홈 위치에서 360도 스캔만 수행합니다. rviz2로 새 지도를 확인한 뒤 '
+                'patrol_waypoints 파라미터로 실제 순찰 좌표를 설정하세요.'
+            )
+
         self.patrol_targets = []
         self.patrol_index = 0
         self.patrol_start_x = 0.0
@@ -137,6 +142,18 @@ class ParkingControllerNode(Node):
         self.create_subscription(PoseStamped, '/goal_pose', self.mock_nav2_goal_callback, 10)
         self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.initial_pose_callback, 10)
 
+        # AMCL pose subscriber (Real AGV mode: map-frame accurate position)
+        # Used to update sim_x/sim_y/sim_yaw with real localization data
+        from rclpy.qos import QoSReliabilityPolicy
+        amcl_qos = QoSProfile(depth=5,
+                              reliability=QoSReliabilityPolicy.BEST_EFFORT,
+                              durability=DurabilityPolicy.VOLATILE)
+        self.last_amcl_time = None
+        self.create_subscription(
+            PoseWithCovarianceStamped, '/amcl_pose',
+            self.amcl_pose_callback, amcl_qos
+        )
+        
         # ROS2 OccupancyGrid map server uses TRANSIENT_LOCAL durability.
         map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
         self.create_subscription(OccupancyGrid, '/map', self.map_callback, map_qos)
@@ -173,6 +190,21 @@ class ParkingControllerNode(Node):
             self.send_nav2_goal()
 
     def odom_callback(self, msg):
+        # Only use wheel odom if AMCL hasn't updated in the last 3 seconds
+        if self.last_amcl_time is not None:
+            elapsed = (self.get_clock().now() - self.last_amcl_time).nanoseconds / 1e9
+            if elapsed < 3.0:
+                return  # AMCL data is fresher, skip wheel odom update
+        self.sim_x = msg.pose.pose.position.x
+        self.sim_y = msg.pose.pose.position.y
+        q = msg.pose.pose.orientation
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
+        self.sim_yaw = math.atan2(siny_cosp, cosy_cosp)
+
+    def amcl_pose_callback(self, msg):
+        """Primary pose update for Real AGV mode (map frame, AMCL localization)."""
+        self.last_amcl_time = self.get_clock().now()
         self.sim_x = msg.pose.pose.position.x
         self.sim_y = msg.pose.pose.position.y
         q = msg.pose.pose.orientation

src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py

@@ -221,59 +221,88 @@ class ForiSerialBridge(Node):
         msg.capacity = 60.0        # 60Ah LiFePO4
         msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIFE # LiFePO4
 
-        voltage = 25.1  # Default nominal voltage for mock simulation mode
+        # === LiFePO4 8S Battery Voltage Bounds ===
+        # Full charge: 8 × 3.65V = 29.2V
+        # Cutoff:      8 × 2.80V = 22.4V
+        v_min = 22.4
+        v_max = 29.2
+
+        # Initialize last-valid-voltage storage
+        if not hasattr(self, 'last_valid_voltage'):
+            self.last_valid_voltage = None  # None = no valid reading yet
+
+        voltage = None  # Will be set from Modbus or fallback
+
         if not self.is_mock_mode and self.control_method == 'direct_pc':
             try:
-                # Read both 0x20A0 (External Voltage) and 0x20A1 (Bus Voltage)
+                # Read registers 0x20A0 (External Voltage) and 0x20A1 (Bus Voltage)
                 v_resp = self.modbus.read_registers(1, 0x20A0, 2)
                 if v_resp and len(v_resp) > 0:
                     raw_a0 = abs(v_resp[0])
                     raw_a1 = abs(v_resp[1]) if len(v_resp) > 1 else raw_a0
 
-                    # Uncalibrated raw voltage from driver sensor
+                    # ZLAC8015D reports voltage in units of 0.1V (e.g. 283 = 28.3V)
+                    # or sometimes 0.01V (e.g. 2830 = 28.3V). Detect scale by range.
                     raw_v = 0.0
-                    if 200 <= raw_a0 <= 320:
+                    if 200 <= raw_a0 <= 320:       # 20.0V ~ 32.0V in 0.1V steps
                         raw_v = raw_a0 * 0.1
-                    elif 2000 <= raw_a0 <= 3200:
+                    elif 2000 <= raw_a0 <= 3200:   # 20.0V ~ 32.0V in 0.01V steps
                         raw_v = raw_a0 * 0.01
                     elif 200 <= raw_a1 <= 320:
                         raw_v = raw_a1 * 0.1
                     elif 2000 <= raw_a1 <= 3200:
                         raw_v = raw_a1 * 0.01
                     else:
+                        # Fallback guess
                         raw_v = raw_a1 * 0.01 if raw_a1 > 1000 else (raw_a1 * 0.1 if raw_a1 > 100 else float(raw_a1))
 
-                    # Offset-Linear Calibration for 25.1V Multimeter Baseline
+                    # Sanity check: plausible battery voltage range (20V ~ 32V)
-                    # Midpoint of driver's raw reading band for 25.1V is 28.2V
+                    if 20.0 <= raw_v <= 32.0:
-                    raw_baseline = 28.2
+                        voltage = raw_v
-                    voltage = 25.1 + (raw_v - raw_baseline) * 0.88536
+                        self.last_valid_voltage = voltage
-                        
+                        self.get_logger().info(
-                    self.get_logger().info(f'[BATTERY TELEMETRY] Driver Raw: {raw_v:.2f}V -> Calibrated Real Battery: {voltage:.2f}V', throttle_duration_sec=3.0)
+                            f'[BATTERY] Driver raw: {raw_v:.2f}V  |  '
+                            f'{max(0.0, min(100.0, (raw_v - v_min) / (v_max - v_min) * 100)):.1f}%',
+                            throttle_duration_sec=3.0
+                        )
+                    else:
+                        self.get_logger().warn(
+                            f'[BATTERY] Raw voltage out of plausible range: raw_a0={raw_a0}, raw_a1={raw_a1}, computed={raw_v:.2f}V',
+                            throttle_duration_sec=5.0
+                        )
+                else:
+                    self.get_logger().warn('[BATTERY] Modbus read returned empty response.', throttle_duration_sec=5.0)
             except Exception as e:
-                pass
+                self.get_logger().warn(f'[BATTERY] Modbus read failed: {e}', throttle_duration_sec=5.0)
 
-        # 2. Sliding Window Median + Heavy EMA Filter (eliminates IR drop jumps & Modbus spikes)
+        # Fallback priority: last valid reading > mock nominal
+        if voltage is None:
+            if self.last_valid_voltage is not None:
+                voltage = self.last_valid_voltage
+                self.get_logger().warn('[BATTERY] Using last valid voltage reading as fallback.', throttle_duration_sec=10.0)
+            else:
+                # Absolute fallback: use midpoint of nominal LiFePO4 8S range
+                voltage = (v_min + v_max) / 2.0  # ~25.8V nominal
+                self.get_logger().warn(f'[BATTERY] No valid reading yet - using nominal fallback: {voltage:.1f}V', throttle_duration_sec=10.0)
+
+        # Sliding Window Median + Heavy EMA Filter (eliminates Modbus spikes)
         if not hasattr(self, 'voltage_history'):
             import collections
             self.voltage_history = collections.deque(maxlen=10)
 
         self.voltage_history.append(voltage)
 
-        # Median filtering to reject any sudden Modbus voltage spike
         sorted_v = sorted(self.voltage_history)
         median_v = sorted_v[len(sorted_v) // 2]
 
         if not hasattr(self, 'filtered_battery_voltage') or self.filtered_battery_voltage is None:
             self.filtered_battery_voltage = median_v
         else:
-            # Heavy EMA smoothing (Alpha = 0.05 for 10-second smooth transition)
+            # EMA smoothing (Alpha=0.05 = ~10s transition)
             self.filtered_battery_voltage = self.filtered_battery_voltage * 0.95 + median_v * 0.05
 
         voltage = self.filtered_battery_voltage
 
-        # 3. 24V LiFePO4 8S battery voltage bounds: Full = 28.0V, Cutoff = 21.6V
-        v_min = 21.6
-        v_max = 28.0
         percentage = (voltage - v_min) / (v_max - v_min)
         percentage = max(0.0, min(1.0, percentage))
 
@@ -282,6 +311,7 @@ class ForiSerialBridge(Node):
         msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
         self.battery_pub.publish(msg)
 
+
     def init_direct_drivers(self):
         """
         Initializes ZLAC8015D parameters (Operating mode to Velocity Control).

src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py

@@ -0,0 +1,228 @@
+#!/usr/bin/env python3
+"""
+Terrain-aware cmd_vel speed shaper.
+
+Sits between raw velocity command producers (Nav2's controller_server, remapped in
+fori_nav2.launch.py, and parking_controller_node's own patrol/docking/mock-drive
+commands, remapped in fori_full.launch.py) and the real /cmd_vel that
+serial_bridge_node drives the motors from. Looks up the 2.5D elevation map
+(pcd_gridmap_converter output) at the robot's current position and a short
+lookahead point in the direction of travel, computes the local grade, and scales
+the commanded forward speed: faster uphill (more torque margin needed), slower
+downhill (safer descent). serial_bridge_node's own target_linear_speed clamp is
+still the final hard safety ceiling regardless of this node's output.
+"""
+import math
+
+import numpy as np
+import rclpy
+import yaml
+from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, QoSReliabilityPolicy, qos_profile_sensor_data
+
+
+def _read_pgm_token(f):
+    token = b''
+    while True:
+        c = f.read(1)
+        if not c:
+            raise EOFError('Unexpected EOF while reading PGM header')
+        if c in b' \t\n\r':
+            if token:
+                return token
+            continue
+        if c == b'#':
+            while c not in (b'\n', b''):
+                c = f.read(1)
+            continue
+        token += c
+
+
+class ElevationMap:
+    """Loads a pcd_gridmap_converter elevation_map.pgm + elevation_map.yaml pair.
+
+    Pixel 0 means unknown. Pixel 1..255 linearly encodes height between
+    min_elevation and max_elevation (see pcd_to_gridmap_node.cpp's
+    publishAndSaveMaps()). Image rows are stored top-to-bottom for the highest-y
+    grid row first, mirroring how nav2_map_server decodes map.pgm.
+    """
+
+    def __init__(self, pgm_path, yaml_path, logger=None):
+        self.logger = logger
+        self.valid = False
+        try:
+            with open(yaml_path, 'r') as f:
+                meta = yaml.safe_load(f)
+            self.resolution = float(meta['resolution'])
+            origin = meta['origin']
+            self.origin_x = float(origin[0])
+            self.origin_y = float(origin[1])
+            self.width = int(meta['width'])
+            self.height = int(meta['height'])
+            self.min_elevation = float(meta['min_elevation'])
+            self.max_elevation = float(meta['max_elevation'])
+            self.unknown_value = int(meta.get('unknown_value', 0))
+
+            with open(pgm_path, 'rb') as f:
+                magic = _read_pgm_token(f)
+                if magic != b'P5':
+                    raise ValueError(f'Unsupported PGM magic: {magic!r}')
+                w = int(_read_pgm_token(f))
+                h = int(_read_pgm_token(f))
+                _read_pgm_token(f)  # maxval, assumed 255
+                data = f.read(w * h)
+
+            if len(data) != w * h:
+                raise ValueError('Truncated PGM pixel data')
+            if (w, h) != (self.width, self.height):
+                raise ValueError(f'PGM size {w}x{h} does not match yaml size {self.width}x{self.height}')
+
+            self.pixels = np.frombuffer(data, dtype=np.uint8).reshape((h, w))
+            self.valid = True
+        except Exception as e:  # noqa: BLE001 - any load failure -> safe passthrough mode
+            if self.logger:
+                self.logger.error(f'[ELEVATION] 고도 지도 로드 실패 ({pgm_path}): {e}')
+
+    def lookup(self, x, y):
+        if not self.valid:
+            return None
+        col = int(math.floor((x - self.origin_x) / self.resolution))
+        grid_y = int(math.floor((y - self.origin_y) / self.resolution))
+        if col < 0 or col >= self.width or grid_y < 0 or grid_y >= self.height:
+            return None
+        image_row = (self.height - 1) - grid_y
+        pixel = int(self.pixels[image_row, col])
+        if pixel == self.unknown_value:
+            return None
+        return self.min_elevation + (pixel - 1) / 254.0 * (self.max_elevation - self.min_elevation)
+
+
+class TerrainSpeedNode(Node):
+    def __init__(self):
+        super().__init__('terrain_speed_node')
+
+        self.declare_parameter('elevation_pgm', '/home/yoo/pcd_gunsan_output/elevation_map.pgm')
+        self.declare_parameter('elevation_yaml', '/home/yoo/pcd_gunsan_output/elevation_map.yaml')
+        # 기본값은 /cmd_vel (목 모드 등 collision_monitor가 없는 구성에서 안전한 기본).
+        # 실기체 모드에서는 fori_full.launch.py가 이 값을 /cmd_vel_terrain으로 넘겨서
+        # collision_monitor(급출현 장애물 감속/정지)가 마지막 안전 게이트로 끼어들게 한다.
+        self.declare_parameter('output_topic', '/cmd_vel')
+        self.declare_parameter('lookahead_dist', 0.5)
+        self.declare_parameter('uphill_gain', 3.0)
+        self.declare_parameter('downhill_gain', 4.0)
+        self.declare_parameter('max_boost', 1.3)
+        self.declare_parameter('min_scale', 0.5)
+        self.declare_parameter('flat_deadband', 0.02)
+        self.declare_parameter('smoothing_alpha', 0.25)
+
+        self.lookahead_dist = self.get_parameter('lookahead_dist').value
+        self.uphill_gain = self.get_parameter('uphill_gain').value
+        self.downhill_gain = self.get_parameter('downhill_gain').value
+        self.max_boost = self.get_parameter('max_boost').value
+        self.min_scale = self.get_parameter('min_scale').value
+        self.flat_deadband = self.get_parameter('flat_deadband').value
+        self.smoothing_alpha = self.get_parameter('smoothing_alpha').value
+
+        pgm_path = self.get_parameter('elevation_pgm').value
+        yaml_path = self.get_parameter('elevation_yaml').value
+        self.elevation = ElevationMap(pgm_path, yaml_path, logger=self.get_logger())
+        if self.elevation.valid:
+            self.get_logger().info(
+                f'[TERRAIN] 고도 지도 로드 완료: {pgm_path} '
+                f'({self.elevation.width}x{self.elevation.height}px, '
+                f'{self.elevation.min_elevation:.2f}m ~ {self.elevation.max_elevation:.2f}m)'
+            )
+        else:
+            self.get_logger().warn(
+                '[TERRAIN] 고도 지도를 사용할 수 없어 경사 보정 없이 /cmd_vel_raw를 그대로 통과시킵니다.'
+            )
+
+        self.pose_x = 0.0
+        self.pose_y = 0.0
+        self.pose_yaw = 0.0
+        self.last_amcl_time = None
+        self.scale = 1.0
+
+        # AMCL(맵 기준 정밀 위치)을 우선 사용하고, 3초 이상 갱신이 없으면 휠 오도메트리로
+        # 대체한다 - parking_controller_node.py의 amcl_pose_callback/odom_callback과 동일한
+        # freshness 기준.
+        amcl_qos = QoSProfile(depth=5,
+                               reliability=QoSReliabilityPolicy.BEST_EFFORT,
+                               durability=DurabilityPolicy.VOLATILE)
+        self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.amcl_pose_callback, amcl_qos)
+        self.create_subscription(Odometry, '/odom_wheels', self.odom_callback, qos_profile_sensor_data)
+        self.create_subscription(Twist, '/cmd_vel_raw', self.cmd_vel_callback, 10)
+        output_topic = self.get_parameter('output_topic').value
+        self.cmd_vel_pub = self.create_publisher(Twist, output_topic, 10)
+        self.get_logger().info(f'[TERRAIN] 출력 토픽: {output_topic}')
+
+    def _update_pose(self, x, y, q):
+        self.pose_x = x
+        self.pose_y = y
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
+        self.pose_yaw = math.atan2(siny_cosp, cosy_cosp)
+
+    def amcl_pose_callback(self, msg):
+        self.last_amcl_time = self.get_clock().now()
+        self._update_pose(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.orientation)
+
+    def odom_callback(self, msg):
+        if self.last_amcl_time is not None:
+            elapsed = (self.get_clock().now() - self.last_amcl_time).nanoseconds / 1e9
+            if elapsed < 3.0:
+                return
+        self._update_pose(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.orientation)
+
+    def cmd_vel_callback(self, msg):
+        out = Twist()
+        out.angular.z = msg.angular.z
+
+        # 제자리 회전이나 지도가 없는 경우엔 경사 보정 없이 그대로 통과 (안전 기본값)
+        if not self.elevation.valid or abs(msg.linear.x) < 0.02:
+            out.linear.x = msg.linear.x
+            self.scale = 1.0
+            self.cmd_vel_pub.publish(out)
+            return
+
+        direction = 1.0 if msg.linear.x >= 0.0 else -1.0
+        to_x = self.pose_x + direction * math.cos(self.pose_yaw) * self.lookahead_dist
+        to_y = self.pose_y + direction * math.sin(self.pose_yaw) * self.lookahead_dist
+
+        z_from = self.elevation.lookup(self.pose_x, self.pose_y)
+        z_to = self.elevation.lookup(to_x, to_y)
+
+        if z_from is None or z_to is None:
+            raw_scale = 1.0
+        else:
+            grade = (z_to - z_from) / self.lookahead_dist
+            if abs(grade) < self.flat_deadband:
+                raw_scale = 1.0
+            elif grade > 0.0:
+                raw_scale = 1.0 + self.uphill_gain * grade   # 오르막: 속도 부스트
+            else:
+                raw_scale = 1.0 + self.downhill_gain * grade  # 내리막: 속도 감쇠 (grade<0)
+            raw_scale = max(self.min_scale, min(self.max_boost, raw_scale))
+
+        # 셀 경계를 지날 때 속도가 튀지 않도록 EMA로 스무딩
+        self.scale = self.scale * (1.0 - self.smoothing_alpha) + raw_scale * self.smoothing_alpha
+        out.linear.x = msg.linear.x * self.scale
+        self.cmd_vel_pub.publish(out)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TerrainSpeedNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py

@@ -25,6 +25,8 @@ class UIServerNode(Node):
 
         if not os.path.exists(self.ui_dir):
             # Fallback to source directory for development / symlink install
+            self.ui_dir = os.path.expanduser('~/fori_ws/fori_ws/src/fori_serial_bridge/ui')
+            if not os.path.exists(self.ui_dir):
                 self.ui_dir = os.path.expanduser('~/fori_ws/src/fori_serial_bridge/ui')
 
         self.get_logger().info(f'Serving UI files from directory: {self.ui_dir}')
@@ -34,22 +36,35 @@ class UIServerNode(Node):
         self.server_thread.start()
 
     def start_server(self):
-        # Change working directory of the handler to UI directory
+        # Use a custom handler that serves from the UI directory without changing
-        os.chdir(self.ui_dir)
+        # the global working directory (which would break other ROS node operations)
-        handler = http.server.SimpleHTTPRequestHandler
+        ui_dir = self.ui_dir
+
+        class UIHandler(http.server.SimpleHTTPRequestHandler):
+            def __init__(self, *args, **kwargs):
+                super().__init__(*args, directory=ui_dir, **kwargs)
+
+            def log_message(self, format, *args):
+                pass  # Suppress HTTP access logs in ROS terminal output
 
         try:
-            self.server = ThreadingHTTPServer(("", self.port), handler)
+            self.server = ThreadingHTTPServer(("", self.port), UIHandler)
             self.get_logger().info('==========================================')
             self.get_logger().info(f'     Web UI Dashboard server launched!    ')
             self.get_logger().info(f'     Connect via: http://localhost:{self.port} ')
             self.get_logger().info('==========================================')
             
-            # Automatically open default web browser
+            # Automatically open default web browser after 1 second delay
+            def auto_open_browser():
+                url = f"http://localhost:{self.port}"
                 try:
-                webbrowser.open(f"http://localhost:{self.port}")
+                    opened = webbrowser.open(url)
-            except Exception as browser_err:
+                    if not opened:
-                self.get_logger().warn(f'Failed to auto-open web browser: {browser_err}')
+                        os.system(f"xdg-open {url} >/dev/null 2>&1 &")
+                except Exception:
+                    os.system(f"xdg-open {url} >/dev/null 2>&1 &")
+
+            threading.Timer(1.0, auto_open_browser).start()
 
             self.server.serve_forever()
         except Exception as e:

src/fori_serial_bridge/launch/fori_full.launch.py

@@ -14,8 +14,11 @@ def generate_launch_description():
     serial_bridge_dir = get_package_share_directory('fori_serial_bridge')
     hik_camera_dir = get_package_share_directory('hik_camera_ros2_driver')
     
-    # Map configuration file path
+    # Map configuration file path (군산 부지 PCD -> 2.5D 변환 결과, pcd_gridmap_converter 산출물)
-    map_yaml_file = '/home/yoo/fori_map.yaml'
+    map_output_dir = '/home/yoo/pcd_gunsan_output'
+    map_yaml_file = os.path.join(map_output_dir, 'map.yaml')
+    elevation_pgm_file = os.path.join(map_output_dir, 'elevation_map.pgm')
+    elevation_yaml_file = os.path.join(map_output_dir, 'elevation_map.yaml')
     
     # 2. Include Hikrobot Camera launch description (pass camera config to isolate it from Nav2 parameters)
     camera_launch = IncludeLaunchDescription(
@@ -36,18 +39,26 @@ def generate_launch_description():
     )
     
     # 4. Launch Motor Bridge Node (ZLAC8015D direct PC RS485 Mode by default)
+    # C++ port (fori_serial_bridge_cpp) replaces the Python node here: same
+    # topics/params, but a CRC-validated low-latency serial transport removes
+    # the Python control-loop jitter/delay.
     motor_bridge_node = Node(
-        package='fori_serial_bridge',
+        package='fori_serial_bridge_cpp',
         executable='serial_bridge_node',
         name='serial_bridge_node',
         parameters=[{
             'control_method': 'direct_pc',
             'port': '/dev/ttyUSB0',
             'baud': 115200,
-            'target_linear_speed': 0.5,
+            'target_linear_speed': 0.3,
             'accel_limit': 0.5,
             'wheel_base': 0.374,
-            'wheel_radius': 0.127
+            'wheel_radius': 0.127,
+            # 실기체 모드는 fori_nav2.launch.py가 fast_lio(LIDAR SLAM)를 같이 띄워서
+            # camera_init->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠
+            # 오도메트리로 odom->base_link를 또 쏘면 base_link 부모가 충돌한다. 모의
+            # 모드(SLAM 없음)에서만 휠 오도메트리 TF를 켠다.
+            'publish_odom_tf': mock_mode_param
         }],
         output='screen'
     )
@@ -69,18 +80,47 @@ def generate_launch_description():
             'mock_mode': mock_mode_param
         }],
         remappings=[
-            ('/cmd_vel', '/cmd_vel') # Keep topic clean
+            # 최종 속도 명령이 아니라 원시(raw) 명령으로 발행한다. terrain_speed_node가
+            # 이 값을 경사도에 맞게 보정한 뒤 실제 /cmd_vel로 발행한다.
+            ('/cmd_vel', '/cmd_vel_raw')
         ],
         output='screen'
     )
 
+    # 5b. Terrain-aware speed shaper: /cmd_vel_raw(parking_controller의 모의주행/도킹/순찰
+    # 명령 + Nav2 controller_server가 fori_nav2.launch.py에서 리맵되어 들어오는 실주행
+    # 명령)를 받아 진행방향 지면 경사(오르막/내리막)에 맞춰 선속도를 보정한 뒤 발행한다.
+    # 이 노드가 없으면 /cmd_vel_raw -> /cmd_vel 연결이 끊겨 로봇이 전혀 움직이지 않으므로
+    # 항상 실행되어야 한다.
+    # 실기체 모드에서는 fori_nav2.launch.py에 있는 collision_monitor(급출현 장애물
+    # 감속/정지 안전계층)가 마지막 게이트로 끼어들 수 있게 /cmd_vel_terrain으로 낸다.
+    # 목 모드는 collision_monitor가 없으므로 기본값인 /cmd_vel로 그대로 발행.
+    terrain_output_topic = '/cmd_vel_terrain' if not mock_mode_param else '/cmd_vel'
+    terrain_speed_node = Node(
+        package='fori_serial_bridge',
+        executable='terrain_speed_node',
+        name='terrain_speed_node',
+        parameters=[{
+            'elevation_pgm': elevation_pgm_file,
+            'elevation_yaml': elevation_yaml_file,
+            'output_topic': terrain_output_topic,
+        }],
+        output='screen'
+    )
+
     # 6. Launch ROSbridge WebSocket Server (run node directly to bypass XML syntax conflict)
+    # max_message_size 기본값(1MB)보다 큰 메시지(/map GetMap 응답 등, 약 2.5MB)는 rosbridge가
+    # 'fragment' 프로토콜 메시지로 쪼개서 보내는데, 대시보드가 쓰는 roslib.js(CDN,
+    # index.html)는 fragment 재조립을 구현하지 않아 이 경우 메시지가 그냥 유실된다(맵만
+    # 계속 빈 화면으로 보이는 원인이었음). 맵을 쪼개지지 않는 단일 메시지로 보내도록 상한을
+    # 넉넉히(20MB) 올려서 아예 fragmentation 경로를 타지 않게 한다.
     rosbridge_node = Node(
         package='rosbridge_server',
         executable='rosbridge_websocket',
         name='rosbridge_websocket',
         parameters=[{
-            'port': 9090
+            'port': 9090,
+            'max_message_size': 20000000
         }],
         output='screen'
     )
@@ -117,6 +157,7 @@ def generate_launch_description():
     ld.add_action(aruco_detector_node)
     ld.add_action(motor_bridge_node)
     ld.add_action(parking_controller_node)
+    ld.add_action(terrain_speed_node)
     ld.add_action(rosbridge_node)
     ld.add_action(ui_server_node)
     ld.add_action(web_video_server_node)
@@ -158,6 +199,8 @@ def generate_launch_description():
         )
         
         # 4. Robot State Publisher (URDF) to define joints/frames
+        urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
+        if not os.path.exists(urdf_file_path):
             urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
         with open(urdf_file_path, 'r') as infp:
             robot_desc = infp.read()
@@ -176,9 +219,23 @@ def generate_launch_description():
         ld.add_action(rsp_node)
     else:
         # In real mode, include the full Nav2/AMCL/LIDAR localization launch
+        nav2_launch_path = os.path.expanduser('~/fori_ws/fori_ws/src/fori_nav2.launch.py')
+        if not os.path.exists(nav2_launch_path):
+            nav2_launch_path = os.path.expanduser('~/fori_ws/src/fori_nav2.launch.py')
+        if os.path.exists(nav2_launch_path):
             nav2_launch = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource('/home/yoo/fori_ws/src/fori_nav2.launch.py')
+                PythonLaunchDescriptionSource(nav2_launch_path)
             )
             ld.add_action(nav2_launch)
 
+        # Relay /odom_wheels -> /odom so Nav2 receives standard odometry
+        # (Uses internal odom_relay_node - no topic_tools dependency needed)
+        odom_relay_node = Node(
+            package='fori_serial_bridge',
+            executable='odom_relay_node',
+            name='odom_relay',
+            output='screen'
+        )
+        ld.add_action(odom_relay_node)
+
     return ld

src/fori_serial_bridge/setup.py

@@ -11,7 +11,7 @@ setup(
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', ['launch/fori_full.launch.py']),
-        ('share/' + package_name + '/ui', ['ui/index.html', 'ui/style.css', 'ui/app.js']),
+        ('share/' + package_name + '/ui', ['ui/index.html', 'ui/style.css', 'ui/app.js', 'ui/manifest.json', 'ui/sw.js']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,7 +24,9 @@ setup(
         'console_scripts': [
             'serial_bridge_node = fori_serial_bridge.serial_bridge_node:main',
             'parking_controller_node = fori_serial_bridge.parking_controller_node:main',
-            'ui_server_node = fori_serial_bridge.ui_server_node:main'
+            'ui_server_node = fori_serial_bridge.ui_server_node:main',
+            'odom_relay_node = fori_serial_bridge.odom_relay_node:main',
+            'terrain_speed_node = fori_serial_bridge.terrain_speed_node:main',
         ],
     },
 )

src/fori_serial_bridge/ui/app.js

@@ -18,6 +18,8 @@ ros.on('connection', () => {
     statusIndicator.className = 'pulse-indicator green';
     statusText.innerText = 'Connected';
     console.log('Connected to rosbridge WebSocket server.');
+    // Request map immediately via service (handles transient_local QoS on reconnect)
+    setTimeout(() => requestMapFromService(), 1500);
 });
 
 ros.on('error', (error) => {
@@ -83,20 +85,78 @@ const arucoPoseSub = new ROSLIB.Topic({
     throttle_rate: 50
 });
 
-// Map subscriber (throttled to 1Hz since maps update infrequently)
+// Map subscriber
 const mapSub = new ROSLIB.Topic({
     ros: ros,
     name: '/map',
     messageType: 'nav_msgs/msg/OccupancyGrid',
-    throttle_rate: 1000
+    throttle_rate: 2000
 });
 
-// Odometry subscriber (throttled to 10Hz for smoother rendering without CPU spikes)
+// Map service client - to request map from map_server after connection
+const mapServiceClient = new ROSLIB.Service({
+    ros: ros,
+    name: '/map_server/map',
+    serviceType: 'nav_msgs/srv/GetMap'
+});
+
+// Also try the GetMap service name used in Nav2 Humble
+const getMapServiceClient = new ROSLIB.Service({
+    ros: ros,
+    name: '/map_server/get_map',
+    serviceType: 'nav_msgs/srv/GetMap'
+});
+
+function requestMapFromService() {
+    // Try /map_server/get_map first (Nav2 Humble standard)
+    getMapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result) => {
+        if (result && result.map) {
+            console.log('Map received via /map_server/get_map service.');
+            processMapMessage(result.map);
+        }
+    }, (err) => {
+        // Fallback: try /map_server/map
+        mapServiceClient.callService(new ROSLIB.ServiceRequest({}), (result2) => {
+            if (result2 && result2.map) {
+                console.log('Map received via /map_server/map service.');
+                processMapMessage(result2.map);
+            }
+        }, (err2) => {
+            console.warn('Both map services unavailable, waiting for /map topic publish:', err2);
+        });
+    });
+}
+
+// Primary AMCL pose subscriber (Nav2 Map frame pose)
+const amclPoseSub = new ROSLIB.Topic({
+    ros: ros,
+    name: '/amcl_pose',
+    messageType: 'geometry_msgs/msg/PoseWithCovarianceStamped',
+    throttle_rate: 50
+});
+
+// FAST-LIO2 LiDAR Odometry subscriber
+const fastLioOdomSub = new ROSLIB.Topic({
+    ros: ros,
+    name: '/aft_mapped_to_init',
+    messageType: 'nav_msgs/msg/Odometry',
+    throttle_rate: 50
+});
+
+// Standard Odometry subscriber
 const odomSub = new ROSLIB.Topic({
+    ros: ros,
+    name: '/odom',
+    messageType: 'nav_msgs/msg/Odometry',
+    throttle_rate: 50
+});
+
+// Wheel Odometry subscriber
+const wheelOdomSub = new ROSLIB.Topic({
     ros: ros,
     name: '/odom_wheels',
     messageType: 'nav_msgs/msg/Odometry',
-    throttle_rate: 100
+    throttle_rate: 50
 });
 
 // Joint State subscriber (throttled to 5Hz to update RPM gauges efficiently)
@@ -116,39 +176,31 @@ const batterySub = new ROSLIB.Topic({
 });
 
 batterySub.subscribe(function(msg) {
-    const voltage = msg.voltage ? msg.voltage.toFixed(1) : '26.4';
+    // percentage is 0.0-1.0 range from sensor_msgs/BatteryState
-    const pct = Math.round((msg.percentage !== undefined ? msg.percentage : 0.85) * 100);
+    let pct;
+    if (msg.percentage !== undefined && msg.percentage !== null && !isNaN(msg.percentage)) {
+        // Normalize: if value > 1.0, it's already in percent form
+        pct = msg.percentage > 1.0 ? Math.round(msg.percentage) : Math.round(msg.percentage * 100);
+    } else if (msg.voltage && msg.voltage > 0) {
+        // Fallback: estimate from voltage (24V LiFePO4 8S: 21.6V=0%, 28.0V=100%)
+        const minV = 21.6, maxV = 28.0;
+        pct = Math.round(Math.max(0, Math.min(100, (msg.voltage - minV) / (maxV - minV) * 100)));
+    } else {
+        return; // No valid data
+    }
 
     const badge = document.getElementById('battery-badge');
     const bar = document.getElementById('battery-bar');
+    const valDisplay = document.getElementById('battery-value-display');
+    const ringFill = document.getElementById('battery-ring-fill');
 
-    if (badge) {
+    if (badge) badge.innerText = `${pct}%`;
-        badge.innerText = `${pct}% (${voltage}V)`;
+    if (valDisplay) valDisplay.innerText = `${pct}%`;
-        if (pct >= 50) {
+    if (ringFill) {
-            badge.style.background = 'rgba(34, 197, 94, 0.2)';
+        const strokeDash = 188 - Math.round((pct / 100) * 188);
-            badge.style.color = '#4ade80';
+        ringFill.setAttribute('stroke-dashoffset', strokeDash);
-            badge.style.borderColor = 'rgba(74, 222, 128, 0.4)';
-        } else if (pct >= 20) {
-            badge.style.background = 'rgba(234, 179, 8, 0.2)';
-            badge.style.color = '#facc15';
-            badge.style.borderColor = 'rgba(250, 204, 21, 0.4)';
-        } else {
-            badge.style.background = 'rgba(239, 68, 68, 0.2)';
-            badge.style.color = '#f87171';
-            badge.style.borderColor = 'rgba(248, 113, 113, 0.4)';
-        }
-    }
-    
-    if (bar) {
-        bar.style.width = `${pct}%`;
-        if (pct >= 50) {
-            bar.style.background = 'linear-gradient(90deg, #22c55e, #4ade80)';
-        } else if (pct >= 20) {
-            bar.style.background = 'linear-gradient(90deg, #eab308, #facc15)';
-        } else {
-            bar.style.background = 'linear-gradient(90deg, #ef4444, #f87171)';
-        }
     }
+    if (bar) bar.style.width = `${pct}%`;
 });
 
 
@@ -177,7 +229,16 @@ let panStart = { x: 0, y: 0 };
 const canvas = document.getElementById('map-canvas');
 const ctx = canvas.getContext('2d');
 
+// requestAnimationFrame throttle to avoid redundant redraws
+let drawMapPending = false;
 function drawMap() {
+    if (drawMapPending) return; // Already queued
+    drawMapPending = true;
+    requestAnimationFrame(_doDrawMap);
+}
+
+function _doDrawMap() {
+    drawMapPending = false;
     if (!mapData || !mapInfo) return;
 
     const w = mapInfo.width;
@@ -260,16 +321,19 @@ function drawRobot(rx, ry, ryaw, color) {
     ctx.rotate(-ryaw);
 
     // Draw triangle
-    ctx.fillStyle = color;
+    ctx.fillStyle = '#22A774';
-    ctx.shadowBlur = 10;
+    ctx.strokeStyle = '#047857';
-    ctx.shadowColor = color;
+    ctx.lineWidth = 2;
+    ctx.shadowBlur = 6;
+    ctx.shadowColor = 'rgba(34, 167, 116, 0.4)';
     ctx.beginPath();
-    ctx.moveTo(10, 0);
+    ctx.moveTo(12, 0);
-    ctx.lineTo(-8, -6);
+    ctx.lineTo(-9, -7);
-    ctx.lineTo(-4, 0);
+    ctx.lineTo(-5, 0);
-    ctx.lineTo(-8, 6);
+    ctx.lineTo(-9, 7);
     ctx.closePath();
     ctx.fill();
+    ctx.stroke();
     ctx.restore();
 }
 
@@ -300,8 +364,8 @@ function drawTarget(rx, ry, ryaw, color) {
 }
 
 // --- [Subscribe Listeners] ---
-// Map listener
+// processMapMessage: shared handler for both /map topic and /map_server/map service
-mapSub.subscribe((message) => {
+function processMapMessage(message) {
     mapData = message.data;
     mapInfo = message.info;
 
@@ -320,11 +384,14 @@ mapSub.subscribe((message) => {
         const val = mapData[i];
         let r, g, b, a;
         if (val === 0) {
-            r = 15; g = 18; b = 32; a = 255;
+            // Free space: Clean crisp white
+            r = 255; g = 255; b = 255; a = 255;
         } else if (val === 100) {
-            r = 157; g = 78; b = 221; a = 255;
+            // Occupied wall: Dark slate
+            r = 30; g = 41; b = 59; a = 255;
         } else {
-            r = 6; g = 6; b = 10; a = 255;
+            // Unknown area: Soft light gray
+            r = 226; g = 232; b = 240; a = 255;
         }
 
         const col = i % w;
@@ -339,33 +406,82 @@ mapSub.subscribe((message) => {
     offscreenCtx.putImageData(imgData, 0, 0);
 
     drawMap();
+}
+
+// Map listener - handles live updates
+mapSub.subscribe((message) => {
+    processMapMessage(message);
 });
 
-// Odom listener
+// --- [Pose Tracking] ---
-odomSub.subscribe((message) => {
+// Strategy: Use AMCL pose (map frame) as primary. Fall back to wheel odom ONLY if AMCL
-    const pose = message.pose.pose;
+// has not been received for 3+ seconds. NEVER mix frame origins.
-    robotPose.x = pose.position.x;
+let amclActive = false;
-    robotPose.y = pose.position.y;
+let lastAmclTime = 0;
 
-    // Quaternion to Euler yaw
+function applySmoothedPose(newX, newY, newYaw) {
-    const q = pose.orientation;
+    // Strong smoothing to prevent UI jitter (alpha=0.4 = 40% new, 60% old)
+    const alpha = 0.4;
+    robotPose.x = alpha * newX + (1 - alpha) * robotPose.x;
+    robotPose.y = alpha * newY + (1 - alpha) * robotPose.y;
+
+    // Yaw wrap-around aware interpolation
+    let diffYaw = newYaw - robotPose.yaw;
+    while (diffYaw > Math.PI) diffYaw -= 2 * Math.PI;
+    while (diffYaw < -Math.PI) diffYaw += 2 * Math.PI;
+    robotPose.yaw += alpha * diffYaw;
+
+    const poseVal = document.getElementById('val-pose');
+    if (poseVal) {
+        let yawDeg = Math.round(robotPose.yaw * 180 / Math.PI);
+        if (yawDeg < 0) yawDeg += 360;
+        poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m &nbsp;|&nbsp; Y: ${robotPose.y.toFixed(2)}m &nbsp;|&nbsp; Yaw: ${yawDeg}°`;
+    }
+    drawMap();
+}
+
+function quatToYaw(q) {
     const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
     const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
-    robotPose.yaw = Math.atan2(siny_cosp, cosy_cosp);
+    return Math.atan2(siny_cosp, cosy_cosp);
+}
+
+// 1. AMCL Pose Listener - PRIMARY source for Real AGV mode (map frame, most accurate)
+amclPoseSub.subscribe((message) => {
+    amclActive = true;
+    lastAmclTime = Date.now();
+    const pose = message.pose.pose;
+    applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));
+});
+
+// 2. Wheel Odometry - FALLBACK only when AMCL is not active (simulation/odom frame)
+wheelOdomSub.subscribe((message) => {
+    // Only use wheel odom if AMCL hasn't been received in last 3 seconds
+    if (Date.now() - lastAmclTime < 3000) return;
+    const pose = message.pose.pose;
+    applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));
 
-    // Update speeds
     const twist = message.twist.twist;
     document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`;
     document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`;
+});
 
-    // Update real-time pose metrics
+// 3. Standard /odom - Only update velocity display, never pose (to avoid frame mixing)
-    const poseVal = document.getElementById('val-pose');
+odomSub.subscribe((message) => {
-    if (poseVal) {
+    const twist = message.twist.twist;
-        const yawDeg = Math.round(robotPose.yaw * 180 / Math.PI);
+    document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`;
-        poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m &nbsp;|&nbsp; Y: ${robotPose.y.toFixed(2)}m &nbsp;|&nbsp; Yaw: ${yawDeg}°`;
+    document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`;
+});
+
+// 4. FAST-LIO2 - Only velocity display, AMCL gives more accurate map-frame pose
+fastLioOdomSub.subscribe((message) => {
+    if (!amclActive) {
+        // If no AMCL available (e.g. before localization converges), use FAST-LIO pose
+        if (Date.now() - lastAmclTime > 3000) {
+            const pose = message.pose.pose;
+            applySmoothedPose(pose.position.x, pose.position.y, quatToYaw(pose.orientation));
+        }
     }
-
-    drawMap();
 });
 
 // Camera stream setup with automatic fallback
@@ -473,6 +589,14 @@ arucoPoseSub.subscribe((message) => {
     }, 1000);
 });
 
+// Helper to update top mode summary card
+function updateTopModeText(modeStr) {
+    const topEl = document.getElementById('val-state-top');
+    if (topEl) {
+        topEl.innerText = modeStr;
+    }
+}
+
 // Robot state listener
 robotModeStatusSub.subscribe((message) => {
     document.getElementById('val-state').innerText = message.data;
@@ -490,24 +614,28 @@ robotModeStatusSub.subscribe((message) => {
 
     if (status.includes('mode: nav2')) {
         currentMode = 'nav2';
+        updateTopModeText('Nav2');
         document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
         document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
         document.getElementById('btn-mode-parking').className = 'btn btn-primary';
         document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
     } else if (status.includes('mode: patrol')) {
         currentMode = 'patrol';
+        updateTopModeText('Patrol');
         document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
         document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
         document.getElementById('btn-mode-parking').className = 'btn btn-primary';
         document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
     } else if (status.includes('mode: parking')) {
         currentMode = 'parking';
+        updateTopModeText('Parking');
         document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
         document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
         document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
         document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
     } else if (status.includes('mode: stop') || status.includes('state: estop')) {
         currentMode = 'stop';
+        updateTopModeText('STOP');
         document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
         document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
         document.getElementById('btn-mode-parking').className = 'btn btn-primary';
@@ -549,6 +677,7 @@ document.getElementById('btn-mode-nav2').addEventListener('click', () => {
     currentMode = 'nav2';
     nav2GoalActive = false;
     parkingWaypoint = null; // Clear waypoint on UI
+    updateTopModeText('Nav2');
     document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
     document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
     document.getElementById('btn-mode-parking').className = 'btn btn-primary';
@@ -562,6 +691,7 @@ document.getElementById('btn-mode-patrol').addEventListener('click', () => {
     currentMode = 'patrol';
     parkingWaypoint = null;
     nav2GoalActive = false;
+    updateTopModeText('Patrol');
     document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
     document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
     document.getElementById('btn-mode-parking').className = 'btn btn-primary';
@@ -574,6 +704,7 @@ document.getElementById('btn-mode-patrol').addEventListener('click', () => {
 document.getElementById('btn-mode-parking').addEventListener('click', () => {
     currentMode = 'parking';
     parkingWaypoint = null; // Clear waypoint on UI until set by user
+    updateTopModeText('Parking');
     document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
     document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
     document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
@@ -597,6 +728,7 @@ document.getElementById('btn-emergency-stop').addEventListener('click', () => {
     currentMode = 'stop';
     nav2GoalActive = false;
     parkingWaypoint = null; // Clear waypoint on UI
+    updateTopModeText('STOP');
     document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
     document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
     document.getElementById('btn-mode-parking').className = 'btn btn-primary';
@@ -787,6 +919,93 @@ canvas.addEventListener('wheel', (event) => {
     drawMap();
 }, { passive: false });
 
+// Mobile Touch Interactions (touchstart, touchmove, touchend)
+let touchStartCoords = { x: 0, y: 0 };
+let isTouchDragging = false;
+
+canvas.addEventListener('touchstart', (e) => {
+    if (e.touches.length === 1) {
+        e.preventDefault();
+        const touch = e.touches[0];
+        const rect = canvas.getBoundingClientRect();
+        const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
+        const dx_padding = (rect.width - canvas.width * scale) / 2;
+        const dy_padding = (rect.height - canvas.height * scale) / 2;
+
+        const clickU = (touch.clientX - rect.left - dx_padding) / scale;
+        const clickV = (touch.clientY - rect.top - dy_padding) / scale;
+
+        touchStartCoords.x = touch.clientX;
+        touchStartCoords.y = touch.clientY;
+        dragStartCoords.x = clickU;
+        dragStartCoords.y = clickV;
+
+        const coords = defCanvasToRos(clickU, clickV);
+        dragStartRos.x = coords.rx;
+        dragStartRos.y = coords.ry;
+
+        document.getElementById('wp-x').value = coords.rx.toFixed(2);
+        document.getElementById('wp-y').value = coords.ry.toFixed(2);
+
+        isTouchDragging = true;
+
+        if (poseEstimateMode) {
+            robotPose.x = coords.rx;
+            robotPose.y = coords.ry;
+        } else if (currentMode === 'nav2') {
+            nav2Goal.x = coords.rx;
+            nav2Goal.y = coords.ry;
+            nav2GoalActive = true;
+        } else {
+            parkingWaypoint = { x: coords.rx, y: coords.ry, yaw: 0.0 };
+        }
+        drawMap();
+    }
+}, { passive: false });
+
+canvas.addEventListener('touchmove', (e) => {
+    if (!isTouchDragging || e.touches.length !== 1) return;
+    e.preventDefault();
+    const touch = e.touches[0];
+    const rect = canvas.getBoundingClientRect();
+    const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
+
+    const dx = touch.clientX - touchStartCoords.x;
+    const dy = touch.clientY - touchStartCoords.y;
+
+    if (Math.abs(dx) > 5 || Math.abs(dy) > 5) {
+        currentDragYaw = Math.atan2(-dy, dx);
+        let yawDeg = Math.round(currentDragYaw * 180.0 / Math.PI);
+        if (yawDeg < 0) yawDeg += 360;
+        document.getElementById('wp-yaw').value = yawDeg;
+
+        if (poseEstimateMode) {
+            robotPose.yaw = currentDragYaw;
+        } else if (currentMode === 'nav2') {
+            nav2Goal.yaw = currentDragYaw;
+        } else {
+            if (parkingWaypoint) parkingWaypoint.yaw = currentDragYaw;
+        }
+        drawMap();
+    }
+}, { passive: false });
+
+canvas.addEventListener('touchend', (e) => {
+    if (isTouchDragging) {
+        isTouchDragging = false;
+        if (poseEstimateMode) {
+            publishInitialPose(dragStartRos.x, dragStartRos.y, currentDragYaw);
+            poseEstimateMode = false;
+            document.getElementById('btn-pose-estimate').className = 'btn btn-secondary';
+            document.getElementById('btn-pose-estimate').innerText = '📍 로봇 위치 초기화 (Pose Estimate)';
+        } else if (currentMode === 'nav2') {
+            publishNav2Goal();
+        } else {
+            publishWaypoint();
+        }
+    }
+});
+
 // Double-click to reset zoom & pan translation
 canvas.addEventListener('dblclick', () => {
     zoom = 1.0;
@@ -844,6 +1063,12 @@ function publishNav2Goal() {
 }
 
 function publishInitialPose(x, y, yaw) {
+    // Covariance is 6x6 row-major, indices [0][0]=x, [1][1]=y, [5][5]=yaw
+    const cov = new Array(36).fill(0.0);
+    cov[0]  = 0.25;                    // xx uncertainty (0.5m std dev)
+    cov[7]  = 0.25;                    // yy uncertainty (0.5m std dev)
+    cov[35] = 0.06853891945200942;     // yaw*yaw uncertainty (~15deg std dev)
+
     const msg = new ROSLIB.Message({
         header: {
             frame_id: 'map',
@@ -859,18 +1084,30 @@ function publishInitialPose(x, y, yaw) {
                     w: Math.cos(yaw * 0.5)
                 }
             },
-            covariance: [
+            covariance: cov
-                0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
-                0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
-                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-                0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0,
-                0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-                0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
-            ]
         }
     });
 
     initialPosePub.publish(msg);
     console.log(`Published Initial Pose Reset: x=${x.toFixed(2)}, y=${y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
-    drawMap();
+}
+
+// --- [Mobile UI Functions] ---
+
+// Toggle Mobile Drawer Sidebar
+function toggleMobileSidebar() {
+    const sidebar = document.getElementById('appSidebar');
+    const overlay = document.getElementById('sidebarOverlay');
+    if (sidebar && overlay) {
+        sidebar.classList.toggle('mobile-open');
+        overlay.classList.toggle('active');
+    }
+}
+
+if ('serviceWorker' in navigator) {
+    window.addEventListener('load', () => {
+        navigator.serviceWorker.register('./sw.js')
+            .then(reg => console.log('PWA ServiceWorker registered:', reg.scope))
+            .catch(err => console.warn('PWA ServiceWorker registration failed:', err));
+    });
 }

src/fori_serial_bridge/ui/index.html

@@ -2,41 +2,167 @@
 <html lang="ko">
 <head>
   <meta charset="UTF-8">
-    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+  <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0, user-scalable=no">
-    <title>FORI AGV Control Dashboard</title>
+  <title>AZMO - FORI AGV 자율주행 관제 대시보드</title>
-    <!-- Outfit Google Font -->
+  
-    <link href="https://fonts.googleapis.com/css2?family=Outfit:wght@300;400;600;800&display=swap" rel="stylesheet">
+  <!-- PWA Meta Tags & Web App Manifest -->
-    <link rel="stylesheet" href="style.css">
+  <link rel="manifest" href="manifest.json">
+  <meta name="theme-color" content="#22A774">
+  <meta name="mobile-web-app-capable" content="yes">
+  <meta name="apple-mobile-web-app-capable" content="yes">
+  <meta name="apple-mobile-web-app-status-bar-style" content="black-translucent">
+  <meta name="apple-mobile-web-app-title" content="FORI AGV">
+  <link rel="apple-touch-icon" href="data:image/svg+xml,<svg xmlns='http://www.w3.org/2000/svg' viewBox='0 0 100 100'><rect width='100' height='100' rx='22' fill='%2322A774'/><text x='50' y='68' font-size='55' text-anchor='middle' fill='white'>⚡</text></svg>">
+
+  <!-- Outfit & Pretendard Google Font -->
+  <link href="https://fonts.googleapis.com/css2?family=Outfit:wght@400;500;600;700;800&display=swap" rel="stylesheet">
+  <link rel="stylesheet" href="style.css?v=8">
   <!-- ROSlibJS -->
   <script src="https://cdn.jsdelivr.net/npm/roslib@1/build/roslib.min.js"></script>
 </head>
 <body>
+
+  <!-- Mobile Drawer Overlay -->
+  <div class="sidebar-overlay" id="sidebarOverlay" onclick="toggleMobileSidebar()"></div>
+
   <div class="app-container">
-        <!-- Header -->
+    
-        <header class="dashboard-header">
+    <!-- Emerald Green Sidebar (Matching image_1) -->
-            <div class="logo-area">
+    <aside class="sidebar" id="appSidebar">
-                <h1>FORI <span>AGV</span></h1>
+      <div class="sidebar-header">
-                <span class="system-status">BLDC & ArUco Parking System</span>
+        <div class="logo-icon">⚡</div>
+        <div class="logo-text">FORI <span>AGV</span></div>
+        <button class="mobile-close-btn" onclick="toggleMobileSidebar()">&times;</button>
       </div>
+
+      <nav class="sidebar-nav">
+        <a href="#" class="nav-item active" onclick="toggleMobileSidebar()">
+          <span>🗺️</span>
+          <span>2D 관제 대시보드</span>
+        </a>
+      </nav>
+
+      <div class="sidebar-footer">
+        © 2026 GARDENTECH Co., Ltd.
+      </div>
+    </aside>
+
+    <!-- Main Content Wrapper -->
+    <div class="main-wrapper">
+      
+      <!-- Top Navbar Header -->
+      <header class="top-header">
+        <div class="header-left">
+          <!-- Mobile Hamburger Menu Button -->
+          <button class="mobile-hamburger-btn" onclick="toggleMobileSidebar()">
+            <span>≡</span>
+          </button>
+          
+          <span class="mobile-header-title">FORI AGV</span>
+
+          <div class="page-breadcrumb">
+            <span>AZMO 관제 Center</span>
+            <span style="color: var(--text-subtle);">&gt;</span>
+            <span style="font-weight: 700; color: var(--text-main);">FORI AGV 자율주행 대시보드</span>
+          </div>
+        </div>
+
+        <div class="header-right">
+          <!-- Connection Status -->
           <div class="connection-status" id="conn-status">
-                <span id="env-badge" class="env-badge sim">Simulation Mode</span>
+            <span id="env-badge" class="env-badge sim">Simulation</span>
             <span class="pulse-indicator red" id="status-indicator"></span>
             <span id="status-text">Disconnected</span>
           </div>
+        </div>
       </header>
 
-        <!-- Main Dashboard Grid -->
+      <!-- Dashboard Content Container -->
+      <main class="dashboard-content">
+        
+        <div class="section-title-bar">
+          <div class="section-title">
+            AGV 실시간 관제 및 주행 제어
+          </div>
+        </div>
+
+        <!-- Metric Gauge Summary Cards ("Total Condition" derived from image_1) -->
+        <section class="metrics-grid">
+          
+          <!-- Metric 1: Battery Ring Gauge -->
+          <div class="metric-card">
+            <div class="metric-info">
+              <span class="metric-label">FORI 배터리 잔량</span>
+              <span class="metric-value" id="battery-value-display">85%</span>
+              <span class="metric-subtext">24V 60Ah LiFePO4</span>
+            </div>
+            <div class="ring-gauge-container">
+              <svg class="ring-gauge-svg" viewBox="0 0 72 72">
+                <circle class="ring-bg" cx="36" cy="36" r="30"></circle>
+                <circle class="ring-fill green" id="battery-ring-fill" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="28"></circle>
+              </svg>
+              <div class="ring-center-text" id="battery-badge">85%</div>
+            </div>
+          </div>
+
+          <!-- Metric 2: Connection Status -->
+          <div class="metric-card">
+            <div class="metric-info">
+              <span class="metric-label">통신 & ROS2 연결</span>
+              <span class="metric-value" style="color: var(--primary-green-dark);">정상</span>
+              <span class="metric-subtext">rosbridge ws://9090</span>
+            </div>
+            <div class="ring-gauge-container">
+              <svg class="ring-gauge-svg" viewBox="0 0 72 72">
+                <circle class="ring-bg" cx="36" cy="36" r="30"></circle>
+                <circle class="ring-fill blue" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="0"></circle>
+              </svg>
+              <div class="ring-center-text" style="color: var(--accent-blue);">OK</div>
+            </div>
+          </div>
+
+          <!-- Metric 3: Robot Operating State -->
+          <div class="metric-card">
+            <div class="metric-info">
+              <span class="metric-label">현재 주행 모드</span>
+              <span class="metric-value" id="val-state-top" style="color: var(--primary-green-dark);">Nav2</span>
+              <span class="metric-subtext">BLDC &amp; ArUco Parking System</span>
+            </div>
+            <div class="ring-gauge-container">
+              <svg class="ring-gauge-svg" viewBox="0 0 72 72">
+                <circle class="ring-bg" cx="36" cy="36" r="30"></circle>
+                <circle class="ring-fill green" cx="36" cy="36" r="30" stroke-dasharray="188" stroke-dashoffset="0"></circle>
+              </svg>
+              <div class="ring-center-text" style="color: var(--primary-green);">RUN</div>
+            </div>
+          </div>
+
+        </section>
+
+        <!-- Main Dashboard 2-Column Grid -->
         <main class="dashboard-grid">
           
-            <!-- Left Side: Map & Configuration -->
+          <!-- Left Column: Map & Configuration Panel -->
           <section class="grid-card map-card">
             <div class="card-header">
-                    <h2>🗺️ 실시간 2D 맵 시각화</h2>
+              <h2>🗺️ 실시간 2D 맵 시각화 및 Waypoint 설정</h2>
-                    <span class="help-text">지도를 클릭하여 주차 진입점(Waypoint)을 등록하세요.</span>
+              <span class="help-text">지도를 클릭하여 주차 진입점(Waypoint)을 등록하거나 드래그하여 목표 방향을 지정하세요.</span>
             </div>
+
+            <!-- Canvas Container -->
             <div class="canvas-container">
               <canvas id="map-canvas"></canvas>
             </div>
+
+            <!-- Pose Panel -->
+            <div class="pose-panel">
+              <h3>📍 로봇 실시간 현재 위치 (Pose)</h3>
+              <div class="pose-value-display" id="val-pose">
+                X: 0.00m &nbsp;|&nbsp; Y: 0.00m &nbsp;|&nbsp; Yaw: 0°
+              </div>
+            </div>
+
+            <!-- Waypoint Config Panel -->
             <div class="config-panel">
               <h3>📍 주차 진입점 (Waypoint) 설정</h3>
               <div class="coord-inputs">
@@ -53,29 +179,17 @@
                   <input type="number" id="wp-yaw" step="1" value="0">
                 </div>
               </div>
-                    <button class="btn btn-secondary" id="btn-set-wp">진입점 좌표 전송</button>
+              <button class="btn btn-secondary" id="btn-set-wp" style="width: 100%;">진입점 좌표 전송</button>
-                </div>
-                <div class="pose-panel">
-                    <h3>📍 로봇 실시간 현재 위치 (Pose)</h3>
-                    <div class="pose-value-display" id="val-pose">
-                        X: 0.00m &nbsp;|&nbsp; Y: 0.00m &nbsp;|&nbsp; Yaw: 0°
-                    </div>
-                </div>
-                <div class="battery-panel" style="margin-top: 15px; background: rgba(255,255,255,0.03); border: 1px solid rgba(255,255,255,0.08); padding: 15px; border-radius: 12px;">
-                    <div style="display: flex; justify-content: space-between; align-items: center;">
-                        <h3 style="margin: 0; font-size: 1rem; color: var(--text-heading, #fff);">🔋 FORI 배터리 잔량 (24V 60Ah LiFePO4)</h3>
-                        <span id="battery-badge" class="badge badge-normal" style="background: rgba(34, 197, 94, 0.2); color: #4ade80; border: 1px solid rgba(74, 222, 128, 0.4); padding: 4px 10px; border-radius: 20px; font-size: 0.85rem; font-weight: 600;">--% (--.-V)</span>
-                    </div>
-                    <div style="width: 100%; background: rgba(255,255,255,0.08); height: 12px; border-radius: 6px; margin-top: 10px; overflow: hidden; border: 1px solid rgba(255,255,255,0.1);">
-                        <div id="battery-bar" style="width: 0%; height: 100%; background: linear-gradient(90deg, #22c55e, #4ade80); transition: width 0.5s ease, background 0.5s ease; border-radius: 6px;"></div>
-                    </div>
             </div>
+
+            <!-- Hidden battery bar element for app.js compatibility -->
+            <div id="battery-bar" style="display: none;"></div>
           </section>
 
-            <!-- Right Side: Camera View & Telemetry -->
+          <!-- Right Column: Camera View & Telemetry Control -->
           <div class="right-column">
             
-                <!-- Camera & ArUco view -->
+            <!-- Camera & ArUco View -->
             <section class="grid-card camera-card">
               <div class="card-header">
                 <h2>📷 실시간 아루코 인식 카메라</h2>
@@ -104,7 +218,7 @@
                       <span class="detail-value" id="aruco-y">-</span>
                     </div>
                     <div class="detail-row">
-                                    <span class="detail-label">Yaw (좌우 회전)</span>
+                      <span class="detail-label">Yaw (회전)</span>
                       <span class="detail-value" id="aruco-yaw">-</span>
                     </div>
                     <div class="detail-row">
@@ -116,24 +230,27 @@
               </div>
             </section>
 
-                <!-- Mode Controls & Telemetry -->
+            <!-- Mode Controls & Telemetry Panel -->
             <section class="grid-card controls-card">
               <div class="card-header">
                 <h2>⚙️ 제어 모드 및 텔레메트리</h2>
               </div>
               
-                    <!-- Mode Switch Buttons -->
+              <!-- Mode Selectors -->
               <div class="mode-selector">
                 <button class="btn btn-primary active" id="btn-mode-nav2">자율 주행 (Nav2)</button>
                 <button class="btn btn-primary" id="btn-mode-patrol">자동 순찰 (Patrol)</button>
                 <button class="btn btn-primary" id="btn-mode-parking">자동 주차 (Parking)</button>
               </div>
-                    <button class="btn btn-secondary" id="btn-pose-estimate" style="margin-bottom: 12px; font-size: 13px; font-weight: 700; width: 100%;">
+
+              <div style="display: flex; gap: 10px; flex-direction: column;">
+                <button class="btn btn-secondary" id="btn-pose-estimate">
                   📍 로봇 위치 초기화 (Pose Estimate)
                 </button>
-                    <button class="btn btn-danger" id="btn-emergency-stop" style="margin-bottom: 20px; font-size: 14px; font-weight: 800; width: 100%;">
+                <button class="btn btn-danger" id="btn-emergency-stop">
                   🛑 긴급 정지 (Emergency Stop)
                 </button>
+              </div>
 
               <!-- Telemetry Metrics Grid -->
               <div class="telemetry-grid">
@@ -142,11 +259,11 @@
                   <span class="metric-value" id="val-state">IDLE</span>
                 </div>
                 <div class="metric-box">
-                            <span class="metric-label">로봇 선속도 (Linear)</span>
+                  <span class="metric-label">선속도 (Linear)</span>
                   <span class="metric-value" id="val-linear">0.00 m/s</span>
                 </div>
                 <div class="metric-box">
-                            <span class="metric-label">로봇 각속도 (Angular)</span>
+                  <span class="metric-label">각속도 (Angular)</span>
                   <span class="metric-value" id="val-angular">0.00 rad/s</span>
                 </div>
               </div>
@@ -186,9 +303,16 @@
                 </div>
               </div>
             </section>
+
           </div>
+
         </main>
+
+      </main>
+
     </div>
-    <script src="app.js"></script>
+  </div>
+
+  <script src="app.js?v=8"></script>
 </body>
 </html>

src/fori_serial_bridge/ui/manifest.json

@@ -0,0 +1,18 @@
+{
+  "name": "FORI AGV 자율주행 관제",
+  "short_name": "FORI AGV",
+  "description": "AZMO - FORI AGV 모바일 자율주행 관제 대시보드",
+  "start_url": "./index.html",
+  "display": "standalone",
+  "orientation": "portrait",
+  "background_color": "#F4F6F5",
+  "theme_color": "#22A774",
+  "icons": [
+    {
+      "src": "data:image/svg+xml,<svg xmlns='http://www.w3.org/2000/svg' viewBox='0 0 100 100'><rect width='100' height='100' rx='22' fill='%2322A774'/><text x='50' y='68' font-size='55' text-anchor='middle' fill='white'>⚡</text></svg>",
+      "sizes": "192x192 512x512",
+      "type": "image/svg+xml",
+      "purpose": "any maskable"
+    }
+  ]
+}

src/fori_serial_bridge/ui/sw.js

@@ -0,0 +1,36 @@
+// FORI AGV PWA Service Worker
+const CACHE_NAME = 'fori-agv-pwa-v1';
+const ASSETS = [
+  './',
+  './index.html',
+  './style.css',
+  './app.js',
+  './manifest.json'
+];
+
+self.addEventListener('install', (event) => {
+  event.waitUntil(
+    caches.open(CACHE_NAME).then((cache) => cache.addAll(ASSETS))
+  );
+  self.skipWaiting();
+});
+
+self.addEventListener('activate', (event) => {
+  event.waitUntil(
+    caches.keys().then((keys) => {
+      return Promise.all(
+        keys.map((key) => {
+          if (key !== CACHE_NAME) return caches.delete(key);
+        })
+      );
+    })
+  );
+  self.clients.claim();
+});
+
+self.addEventListener('fetch', (event) => {
+  // Network first strategy with fallback to cache for real-time WebSocket dashboard
+  event.respondWith(
+    fetch(event.request).catch(() => caches.match(event.request))
+  );
+});

src/fori_serial_bridge_cpp/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(fori_serial_bridge_cpp)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+add_compile_options(-O2 -Wall -Wextra)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(serial_bridge_node
+  src/modbus_serial.cpp
+  src/serial_bridge_node.cpp
+)
+target_include_directories(serial_bridge_node PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+)
+ament_target_dependencies(serial_bridge_node
+  rclcpp
+  geometry_msgs
+  sensor_msgs
+  nav_msgs
+  std_msgs
+  tf2_ros
+)
+
+install(TARGETS
+  serial_bridge_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/fori_serial_bridge_cpp/include/fori_serial_bridge_cpp/modbus_serial.hpp

@@ -0,0 +1,43 @@
+#pragma once
+// Low-level RS485 Modbus RTU transport for the ZLAC8015D BLDC driver.
+//
+// Ported from the field-verified xbox_motor_control.cpp (fori_zltech_motor_test):
+// unlike the previous Python pyserial implementation (single fixed-timeout read,
+// no CRC check on write acks), every response here is accumulated in a short
+// polling loop and CRC16-validated before being trusted. That is what actually
+// removes the serial-jitter delay in the control loop.
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace fori_serial_bridge_cpp {
+
+uint16_t calcCRC16(const uint8_t *data, size_t len);
+
+class SerialPort {
+public:
+  SerialPort() = default;
+  ~SerialPort();
+
+  SerialPort(const SerialPort &) = delete;
+  SerialPort &operator=(const SerialPort &) = delete;
+
+  bool openPort(const std::string &port_name, int baudrate = 115200);
+  void closePort();
+  bool isOpen() const { return fd_ >= 0; }
+  int fd() const { return fd_; }
+
+  // Modbus function 0x06: write single holding register.
+  bool writeReg(uint8_t slave, uint16_t reg, uint16_t val);
+  // Modbus function 0x10: write multiple holding registers.
+  bool writeRegs(uint8_t slave, uint16_t reg, const std::vector<uint16_t> &vals);
+  // Modbus function 0x03: read holding registers (returned as raw uint16, caller
+  // reinterprets signed fields as needed, matching ZLAC8015D register semantics).
+  bool readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t> &out_vals);
+
+private:
+  int fd_ = -1;
+};
+
+} // namespace fori_serial_bridge_cpp

src/fori_serial_bridge_cpp/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fori_serial_bridge_cpp</name>
+  <version>0.0.1</version>
+  <description>Low-latency C++ port of the ZLAC8015D RS485/Arduino motor serial bridge (replaces the Python serial_bridge_node hot loop)</description>
+  <maintainer email="user@todo.todo">yoo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/fori_serial_bridge_cpp/src/modbus_serial.cpp

@@ -0,0 +1,230 @@
+#include "fori_serial_bridge_cpp/modbus_serial.hpp"
+
+#include <chrono>
+#include <cstring>
+#include <fcntl.h>
+#include <termios.h>
+#include <thread>
+#include <unistd.h>
+
+namespace fori_serial_bridge_cpp {
+
+uint16_t calcCRC16(const uint8_t *data, size_t len) {
+  uint16_t crc = 0xFFFF;
+  for (size_t i = 0; i < len; ++i) {
+    crc ^= data[i];
+    for (int j = 0; j < 8; ++j) {
+      if (crc & 0x0001) {
+        crc >>= 1;
+        crc ^= 0xA001;
+      } else {
+        crc >>= 1;
+      }
+    }
+  }
+  return crc;
+}
+
+SerialPort::~SerialPort() { closePort(); }
+
+bool SerialPort::openPort(const std::string &port_name, int baudrate) {
+  fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd_ < 0) {
+    return false;
+  }
+
+  fcntl(fd_, F_SETFL, 0);
+
+  struct termios options;
+  tcgetattr(fd_, &options);
+
+  speed_t speed = B115200;
+  switch (baudrate) {
+  case 9600:
+    speed = B9600;
+    break;
+  case 19200:
+    speed = B19200;
+    break;
+  case 38400:
+    speed = B38400;
+    break;
+  case 57600:
+    speed = B57600;
+    break;
+  default:
+    speed = B115200;
+    break;
+  }
+
+  cfsetispeed(&options, speed);
+  cfsetospeed(&options, speed);
+
+  options.c_cflag &= ~PARENB; // No parity
+  options.c_cflag &= ~CSTOPB; // 1 stop bit
+  options.c_cflag &= ~CSIZE;
+  options.c_cflag |= CS8;      // 8 data bits
+  options.c_cflag &= ~CRTSCTS; // No hardware flow control
+  options.c_cflag |= CREAD | CLOCAL;
+
+  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+  options.c_iflag &= ~(IXON | IXOFF | IXANY | IGNBRK | BRKINT | PARMRK |
+                        ISTRIP | INLCR | IGNCR | ICRNL);
+  options.c_oflag &= ~OPOST;
+
+  options.c_cc[VMIN] = 0;
+  options.c_cc[VTIME] = 1; // 0.1s timeout
+
+  tcsetattr(fd_, TCSANOW, &options);
+  tcflush(fd_, TCIOFLUSH);
+  return true;
+}
+
+void SerialPort::closePort() {
+  if (fd_ >= 0) {
+    close(fd_);
+    fd_ = -1;
+  }
+}
+
+bool SerialPort::writeReg(uint8_t slave, uint16_t reg, uint16_t val) {
+  uint8_t pkt[8];
+  pkt[0] = slave;
+  pkt[1] = 0x06;
+  pkt[2] = (reg >> 8) & 0xFF;
+  pkt[3] = reg & 0xFF;
+  pkt[4] = (val >> 8) & 0xFF;
+  pkt[5] = val & 0xFF;
+
+  uint16_t crc = calcCRC16(pkt, 6);
+  pkt[6] = crc & 0xFF;
+  pkt[7] = (crc >> 8) & 0xFF;
+
+  tcflush(fd_, TCIOFLUSH);
+  ssize_t written = write(fd_, pkt, 8);
+  if (written != 8)
+    return false;
+
+  if (slave == 0)
+    return true; // Broadcast: driver sends no response.
+
+  uint8_t rx_buf[8];
+  ssize_t rx_bytes = 0;
+  auto start = std::chrono::steady_clock::now();
+  while (rx_bytes < 8) {
+    ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
+    if (res > 0)
+      rx_bytes += res;
+    auto now = std::chrono::steady_clock::now();
+    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50)
+      break;
+    std::this_thread::sleep_for(std::chrono::microseconds(500));
+  }
+  if (rx_bytes != 8)
+    return false;
+
+  uint16_t resp_crc = calcCRC16(rx_buf, 6);
+  uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(rx_buf[7]) << 8);
+  return resp_crc == recv_crc && rx_buf[0] == slave;
+}
+
+bool SerialPort::writeRegs(uint8_t slave, uint16_t reg, const std::vector<uint16_t> &vals) {
+  size_t count = vals.size();
+  size_t pkt_len = 7 + count * 2 + 2;
+  std::vector<uint8_t> pkt(pkt_len);
+
+  pkt[0] = slave;
+  pkt[1] = 0x10;
+  pkt[2] = (reg >> 8) & 0xFF;
+  pkt[3] = reg & 0xFF;
+  pkt[4] = (count >> 8) & 0xFF;
+  pkt[5] = count & 0xFF;
+  pkt[6] = static_cast<uint8_t>(count * 2);
+
+  for (size_t i = 0; i < count; ++i) {
+    pkt[7 + i * 2] = (vals[i] >> 8) & 0xFF;
+    pkt[8 + i * 2] = vals[i] & 0xFF;
+  }
+
+  uint16_t crc = calcCRC16(pkt.data(), pkt_len - 2);
+  pkt[pkt_len - 2] = crc & 0xFF;
+  pkt[pkt_len - 1] = (crc >> 8) & 0xFF;
+
+  tcflush(fd_, TCIOFLUSH);
+  ssize_t written = write(fd_, pkt.data(), pkt_len);
+  if (written != static_cast<ssize_t>(pkt_len))
+    return false;
+
+  if (slave == 0)
+    return true;
+
+  uint8_t rx_buf[8];
+  ssize_t rx_bytes = 0;
+  auto start = std::chrono::steady_clock::now();
+  while (rx_bytes < 8) {
+    ssize_t res = read(fd_, rx_buf + rx_bytes, 8 - rx_bytes);
+    if (res > 0)
+      rx_bytes += res;
+    auto now = std::chrono::steady_clock::now();
+    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 50)
+      break;
+    std::this_thread::sleep_for(std::chrono::microseconds(500));
+  }
+  if (rx_bytes != 8)
+    return false;
+
+  uint16_t resp_crc = calcCRC16(rx_buf, 6);
+  uint16_t recv_crc = static_cast<uint16_t>(rx_buf[6]) | (static_cast<uint16_t>(rx_buf[7]) << 8);
+  return resp_crc == recv_crc && rx_buf[0] == slave;
+}
+
+bool SerialPort::readRegs(uint8_t slave, uint16_t reg, uint16_t count, std::vector<uint16_t> &out_vals) {
+  uint8_t pkt[8];
+  pkt[0] = slave;
+  pkt[1] = 0x03;
+  pkt[2] = (reg >> 8) & 0xFF;
+  pkt[3] = reg & 0xFF;
+  pkt[4] = (count >> 8) & 0xFF;
+  pkt[5] = count & 0xFF;
+
+  uint16_t crc = calcCRC16(pkt, 6);
+  pkt[6] = crc & 0xFF;
+  pkt[7] = (crc >> 8) & 0xFF;
+
+  tcflush(fd_, TCIOFLUSH);
+  if (write(fd_, pkt, 8) != 8)
+    return false;
+
+  size_t expected_bytes = 5 + count * 2;
+  std::vector<uint8_t> rx_buf(expected_bytes);
+  size_t rx_bytes = 0;
+
+  auto start = std::chrono::steady_clock::now();
+  while (rx_bytes < expected_bytes) {
+    ssize_t res = read(fd_, rx_buf.data() + rx_bytes, expected_bytes - rx_bytes);
+    if (res > 0)
+      rx_bytes += res;
+    auto now = std::chrono::steady_clock::now();
+    // 100Hz(10ms) 루프 안에서도 응답을 안정적으로 받도록 25ms까지 허용.
+    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() > 25)
+      break;
+    std::this_thread::sleep_for(std::chrono::microseconds(100));
+  }
+
+  if (rx_bytes == expected_bytes && rx_buf[0] == slave && rx_buf[1] == 0x03) {
+    uint16_t resp_crc = calcCRC16(rx_buf.data(), expected_bytes - 2);
+    uint16_t recv_crc = static_cast<uint16_t>(rx_buf[expected_bytes - 2]) |
+                         (static_cast<uint16_t>(rx_buf[expected_bytes - 1]) << 8);
+    if (resp_crc != recv_crc)
+      return false;
+
+    out_vals.resize(count);
+    for (uint16_t i = 0; i < count; ++i) {
+      out_vals[i] = (static_cast<uint16_t>(rx_buf[3 + i * 2]) << 8) | rx_buf[4 + i * 2];
+    }
+    return true;
+  }
+  return false;
+}
+
+} // namespace fori_serial_bridge_cpp

src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp

@@ -0,0 +1,567 @@
+// C++ port of fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py.
+//
+// This node owns the only hardware-realtime loop in the FORI stack (20Hz RS485
+// Modbus RTU to the ZLAC8015D BLDC drivers, or the Arduino serial fallback).
+// The control algorithm (accel ramp + IMU yaw-rate PI correction, differential
+// kinematics, odometry integration, battery filtering) is kept identical to the
+// Python original; only the low-level serial transport is replaced with the
+// CRC-validated, short-poll implementation verified on the real robot in
+// zlac8015d_motor_test/fori_zltech_motor_test/xbox_motor_control.cpp.
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <cstring>
+#include <deque>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+#include "fori_serial_bridge_cpp/modbus_serial.hpp"
+
+using namespace std::chrono_literals;
+
+namespace fori_serial_bridge_cpp {
+
+namespace {
+constexpr double kPi = 3.14159265358979323846;
+
+double quatToYaw(double w, double x, double y, double z) {
+  double siny_cosp = 2.0 * (w * z + x * y);
+  double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
+  return std::atan2(siny_cosp, cosy_cosp);
+}
+} // namespace
+
+class ForiSerialBridge : public rclcpp::Node {
+public:
+  ForiSerialBridge() : rclcpp::Node("fori_serial_bridge") {
+    declare_parameter("control_method", std::string("direct_pc"));
+    declare_parameter("port", std::string("/dev/ttyUSB0"));
+    declare_parameter("baud", 115200);
+    declare_parameter("target_linear_speed", 0.5);
+    declare_parameter("accel_limit", 0.5);
+    declare_parameter("wheel_base", 0.374);
+    declare_parameter("wheel_radius", 0.127);
+    // FAST_LIO/FAST-LIVO2가 함께 도는 실주행 모드에서는 그쪽이 이미
+    // camera_init->aft_mapped->base_link TF 체인을 소유하고 있어서, 여기서 동시에
+    // odom->base_link를 쏘면 base_link에 부모가 둘(aft_mapped, odom) 생겨 TF 트리가
+    // 충돌한다. LIDAR SLAM 없이 순수 휠 오도메트리로만 굴릴 때(모의주행/벤치 테스트)만
+    // true로 켠다.
+    declare_parameter("publish_odom_tf", true);
+
+    control_method_ = get_parameter("control_method").as_string();
+    port_ = get_parameter("port").as_string();
+    baud_ = get_parameter("baud").as_int();
+    limit_v_ = get_parameter("target_linear_speed").as_double();
+    accel_limit_ = get_parameter("accel_limit").as_double();
+    publish_odom_tf_ = get_parameter("publish_odom_tf").as_bool();
+    wheel_base_ = get_parameter("wheel_base").as_double();
+    wheel_radius_ = get_parameter("wheel_radius").as_double();
+
+    last_time_ = now();
+
+    RCLCPP_INFO(get_logger(), "==========================================");
+    RCLCPP_INFO(get_logger(), "     FORI BLDC ZLAC8015D CONTROL BRIDGE (C++)   ");
+    RCLCPP_INFO(get_logger(), "==========================================");
+    RCLCPP_INFO(get_logger(), "    Control Method: %s", control_method_.c_str());
+    RCLCPP_INFO(get_logger(), "    Serial Port: %s at %d baud", port_.c_str(), baud_);
+    RCLCPP_INFO(get_logger(), "==========================================");
+
+    if (control_method_ == "direct_pc") {
+      if (modbus_port_.openPort(port_, baud_)) {
+        RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 대기 중...");
+        initDirectDrivers();
+        RCLCPP_INFO(get_logger(), "BLDC 드라이버 초기화 완료!");
+      } else {
+        RCLCPP_WARN(get_logger(), "!!! 드라이버 연결 실패: %s !!!", port_.c_str());
+        RCLCPP_WARN(get_logger(), "실제 모터 드라이버 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.");
+        is_mock_mode_ = true;
+      }
+    } else if (control_method_ == "arduino") {
+      if (arduino_port_.openPort(port_, baud_)) {
+        RCLCPP_INFO(get_logger(), "아두이노 초기화 대기 중 (3초)...");
+        std::this_thread::sleep_for(3s);
+        RCLCPP_INFO(get_logger(), "아두이노 메가 통신 준비 완료!");
+      } else {
+        RCLCPP_WARN(get_logger(), "!!! 아두이노 시리얼 연결 실패: %s !!!", port_.c_str());
+        RCLCPP_WARN(get_logger(), "아두이노 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.");
+        is_mock_mode_ = true;
+      }
+    }
+
+    rclcpp::QoS qos_depth1(1);
+
+    cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>(
+        "/cmd_vel", qos_depth1,
+        [this](const geometry_msgs::msg::Twist::SharedPtr msg) { cmdVelCallback(msg); });
+    imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+        "/livox/imu", qos_depth1,
+        [this](const sensor_msgs::msg::Imu::SharedPtr msg) { imuCallback(msg); });
+    initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+        "/initialpose", 10,
+        [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+          initialPoseCallback(msg);
+        });
+    mode_status_sub_ = create_subscription<std_msgs::msg::String>(
+        "/robot_mode_status", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg) { modeStatusCallback(msg); });
+
+    odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("/odom_wheels", qos_depth1);
+    joint_pub_ = create_publisher<sensor_msgs::msg::JointState>("/joint_states", qos_depth1);
+    battery_pub_ = create_publisher<sensor_msgs::msg::BatteryState>("/battery_state", qos_depth1);
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+
+    control_timer_ = create_wall_timer(50ms, [this]() { controlLoop(); });
+    battery_timer_ = create_wall_timer(1000ms, [this]() { publishBatteryStatus(); });
+  }
+
+  // Safe stop invoked from the shutdown hook (Ctrl+C), mirrors the Python
+  // KeyboardInterrupt handler in main().
+  void safeStop() {
+    if (control_method_ == "direct_pc") {
+      disableDirectDrivers();
+    } else if (control_method_ == "arduino") {
+      uint8_t packet[7] = {0};
+      packet[0] = 0xFE;
+      packet[5] = 0;
+      uint8_t checksum = 0;
+      for (int i = 1; i < 6; ++i)
+        checksum += packet[i];
+      packet[6] = checksum;
+      if (arduino_port_.isOpen()) {
+        ssize_t written = write(arduino_port_.fd(), packet, sizeof(packet));
+        (void)written;
+      }
+    }
+    RCLCPP_INFO(get_logger(), "ROS2 BRIDGE STOPPED");
+  }
+
+private:
+  // -------------------------------------------------------------------
+  // Subscription callbacks
+  // -------------------------------------------------------------------
+  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    target_v_ = std::clamp(msg->linear.x, -limit_v_, limit_v_);
+    target_w_ = msg->angular.z;
+  }
+
+  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    current_imu_w_ = msg->angular_velocity.z;
+  }
+
+  void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    odom_x_ = msg->pose.pose.position.x;
+    odom_y_ = msg->pose.pose.position.y;
+    const auto &q = msg->pose.pose.orientation;
+    odom_th_ = quatToYaw(q.w, q.x, q.y, q.z);
+    RCLCPP_INFO(get_logger(), "[MOCK] Reset robot pose to: x=%.2f, y=%.2f, yaw=%.2f", odom_x_, odom_y_, odom_th_);
+  }
+
+  void modeStatusCallback(const std_msgs::msg::String::SharedPtr msg) {
+    is_parked_ = msg->data.find("State: PARKED") != std::string::npos;
+  }
+
+  // -------------------------------------------------------------------
+  // Direct-PC (RS485 Modbus) driver control, ported 1:1 from the Python
+  // ZLAC8015D register sequence but using the CRC-validated SerialPort.
+  // -------------------------------------------------------------------
+  void initDirectDrivers() {
+    modbus_port_.writeReg(1, 0x200D, 3); // Velocity mode
+    std::this_thread::sleep_for(10ms);
+    modbus_port_.writeReg(2, 0x200D, 3);
+    std::this_thread::sleep_for(10ms);
+    disableDirectDrivers();
+    drivers_initialized_ = true;
+  }
+
+  void enableDirectDrivers() {
+    if (drivers_enabled_)
+      return;
+    modbus_port_.writeReg(1, 0x201A, 0);
+    modbus_port_.writeReg(1, 0x201B, 0);
+    modbus_port_.writeReg(2, 0x201A, 0);
+    modbus_port_.writeReg(2, 0x201B, 0);
+    std::this_thread::sleep_for(10ms);
+    modbus_port_.writeReg(1, 0x200E, 8);
+    modbus_port_.writeReg(2, 0x200E, 8);
+    drivers_enabled_ = true;
+    RCLCPP_INFO(get_logger(), "BLDC Drivers and Brakes Released (ENABLED)");
+  }
+
+  void disableDirectDrivers() {
+    modbus_port_.writeRegs(1, 0x2088, {0, 0});
+    modbus_port_.writeRegs(2, 0x2088, {0, 0});
+    modbus_port_.writeReg(1, 0x201A, 1);
+    modbus_port_.writeReg(1, 0x201B, 1);
+    modbus_port_.writeReg(2, 0x201A, 1);
+    modbus_port_.writeReg(2, 0x201B, 1);
+    std::this_thread::sleep_for(10ms);
+    modbus_port_.writeReg(1, 0x200E, 7);
+    modbus_port_.writeReg(2, 0x200E, 7);
+    drivers_enabled_ = false;
+    RCLCPP_INFO(get_logger(), "BLDC Brakes Locked (DISABLED)");
+  }
+
+  // -------------------------------------------------------------------
+  // Battery telemetry (1Hz)
+  // -------------------------------------------------------------------
+  void publishBatteryStatus() {
+    sensor_msgs::msg::BatteryState msg;
+    msg.header.stamp = now();
+    msg.header.frame_id = "base_link";
+    msg.design_capacity = 60.0f;
+    msg.capacity = 60.0f;
+    msg.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_LIFE;
+
+    constexpr double v_min = 22.4;
+    constexpr double v_max = 29.2;
+
+    std::optional<double> voltage;
+
+    if (!is_mock_mode_ && control_method_ == "direct_pc") {
+      std::vector<uint16_t> v_resp;
+      if (modbus_port_.readRegs(1, 0x20A0, 2, v_resp) && !v_resp.empty()) {
+        double raw_a0 = std::abs(static_cast<double>(static_cast<int16_t>(v_resp[0])));
+        double raw_a1 = v_resp.size() > 1
+                             ? std::abs(static_cast<double>(static_cast<int16_t>(v_resp[1])))
+                             : raw_a0;
+
+        double raw_v = 0.0;
+        if (raw_a0 >= 200 && raw_a0 <= 320) {
+          raw_v = raw_a0 * 0.1;
+        } else if (raw_a0 >= 2000 && raw_a0 <= 3200) {
+          raw_v = raw_a0 * 0.01;
+        } else if (raw_a1 >= 200 && raw_a1 <= 320) {
+          raw_v = raw_a1 * 0.1;
+        } else if (raw_a1 >= 2000 && raw_a1 <= 3200) {
+          raw_v = raw_a1 * 0.01;
+        } else {
+          raw_v = raw_a1 > 1000 ? raw_a1 * 0.01 : (raw_a1 > 100 ? raw_a1 * 0.1 : raw_a1);
+        }
+
+        if (raw_v >= 20.0 && raw_v <= 32.0) {
+          voltage = raw_v;
+          last_valid_voltage_ = voltage;
+          RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000,
+                                "[BATTERY] Driver raw: %.2fV  |  %.1f%%", raw_v,
+                                std::clamp((raw_v - v_min) / (v_max - v_min) * 100.0, 0.0, 100.0));
+        } else {
+          RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
+                                "[BATTERY] Raw voltage out of plausible range: raw_a0=%.0f, raw_a1=%.0f, computed=%.2fV",
+                                raw_a0, raw_a1, raw_v);
+        }
+      } else {
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "[BATTERY] Modbus read returned empty response.");
+      }
+    }
+
+    if (!voltage.has_value()) {
+      if (last_valid_voltage_.has_value()) {
+        voltage = last_valid_voltage_;
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000,
+                              "[BATTERY] Using last valid voltage reading as fallback.");
+      } else {
+        voltage = (v_min + v_max) / 2.0;
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 10000,
+                              "[BATTERY] No valid reading yet - using nominal fallback: %.1fV", *voltage);
+      }
+    }
+
+    voltage_history_.push_back(*voltage);
+    if (voltage_history_.size() > 10)
+      voltage_history_.pop_front();
+
+    std::vector<double> sorted_v(voltage_history_.begin(), voltage_history_.end());
+    std::sort(sorted_v.begin(), sorted_v.end());
+    double median_v = sorted_v[sorted_v.size() / 2];
+
+    if (!filtered_battery_voltage_.has_value()) {
+      filtered_battery_voltage_ = median_v;
+    } else {
+      filtered_battery_voltage_ = *filtered_battery_voltage_ * 0.95 + median_v * 0.05;
+    }
+
+    double final_voltage = *filtered_battery_voltage_;
+    double percentage = std::clamp((final_voltage - v_min) / (v_max - v_min), 0.0, 1.0);
+
+    msg.voltage = static_cast<float>(final_voltage);
+    msg.percentage = static_cast<float>(percentage);
+    msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+    battery_pub_->publish(msg);
+  }
+
+  // -------------------------------------------------------------------
+  // 20Hz realtime control loop
+  // -------------------------------------------------------------------
+  void controlLoop() {
+    rclcpp::Time current_now = now();
+    double dt = (current_now - last_time_).seconds();
+    last_time_ = current_now;
+    if (dt <= 0.0)
+      return;
+
+    // 1. Acceleration ramp
+    double dv = target_v_ - current_v_;
+    double max_dv = accel_limit_ * dt;
+    current_v_ += std::clamp(dv, -max_dv, max_dv);
+
+    double dw = target_w_ - current_w_;
+    double max_dw = 2.0 * dt;
+    current_w_ += std::clamp(dw, -max_dw, max_dw);
+
+    // 2. Yaw rate PI control
+    double yaw_error = current_w_ - current_imu_w_;
+    bool is_idle = std::abs(target_v_) < 0.01 && std::abs(target_w_) < 0.01;
+    double correction = 0.0;
+    if (is_idle) {
+      yaw_error_integral_ = 0.0;
+      current_v_ = 0.0;
+      current_w_ = 0.0;
+    } else {
+      yaw_error_integral_ += yaw_error * dt;
+      yaw_error_integral_ = std::clamp(yaw_error_integral_, -max_integral_, max_integral_);
+      correction = kp_yaw_ * yaw_error + ki_yaw_ * yaw_error_integral_;
+    }
+
+    // 3. Inverse kinematics -> wheel RPM
+    double v_left = current_v_ - (current_w_ + correction) * wheel_base_ / 2.0;
+    double v_right = current_v_ + (current_w_ + correction) * wheel_base_ / 2.0;
+
+    int rpm_left = static_cast<int>(std::lround(v_left / (2.0 * kPi * wheel_radius_) * 60.0));
+    int rpm_right = static_cast<int>(std::lround(v_right / (2.0 * kPi * wheel_radius_) * 60.0));
+    rpm_left = std::clamp(rpm_left, -kMaxRpm, kMaxRpm);
+    rpm_right = std::clamp(rpm_right, -kMaxRpm, kMaxRpm);
+
+    // 4. Command the motors
+    int feedback_speeds[4] = {0, 0, 0, 0}; // FL, FR, RL, RR
+
+    if (is_mock_mode_) {
+      feedback_speeds[0] = rpm_left;
+      feedback_speeds[1] = rpm_right;
+      feedback_speeds[2] = rpm_left;
+      feedback_speeds[3] = rpm_right;
+    } else if (control_method_ == "direct_pc") {
+      if (is_idle && is_parked_) {
+        if (drivers_enabled_)
+          disableDirectDrivers();
+      } else {
+        if (!drivers_enabled_)
+          enableDirectDrivers();
+        modbus_port_.writeRegs(1, 0x2088,
+                                {static_cast<uint16_t>(static_cast<int16_t>(rpm_left)),
+                                 static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
+        modbus_port_.writeRegs(2, 0x2088,
+                                {static_cast<uint16_t>(static_cast<int16_t>(rpm_left)),
+                                 static_cast<uint16_t>(static_cast<int16_t>(-rpm_right))});
+      }
+
+      std::vector<uint16_t> front_resp, rear_resp;
+      if (modbus_port_.readRegs(1, 0x20AD, 2, front_resp)) {
+        feedback_speeds[0] = static_cast<int16_t>(front_resp[0]);
+        feedback_speeds[1] = -static_cast<int16_t>(front_resp[1]);
+      }
+      if (modbus_port_.readRegs(2, 0x20AD, 2, rear_resp)) {
+        feedback_speeds[2] = static_cast<int16_t>(rear_resp[0]);
+        feedback_speeds[3] = -static_cast<int16_t>(rear_resp[1]);
+      }
+    } else if (control_method_ == "arduino") {
+      runArduinoIo(rpm_left, rpm_right, is_idle && is_parked_, feedback_speeds);
+    }
+
+    // 5. Odometry & telemetry
+    processFeedback(feedback_speeds, dt);
+
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
+                          "[BLDC FEEDBACK] FL:%d FR:%d | RL:%d RR:%d RPM | Target_W:%.2f IMU_W:%.2f",
+                          feedback_speeds[0], feedback_speeds[1], feedback_speeds[2], feedback_speeds[3],
+                          current_w_, current_imu_w_);
+  }
+
+  void runArduinoIo(int rpm_left, int rpm_right, bool disable, int feedback_speeds[4]) {
+    if (!arduino_port_.isOpen())
+      return;
+
+    uint8_t packet[7];
+    packet[0] = 0xFE;
+    packet[1] = static_cast<uint8_t>((rpm_left >> 8) & 0xFF);
+    packet[2] = static_cast<uint8_t>(rpm_left & 0xFF);
+    packet[3] = static_cast<uint8_t>((rpm_right >> 8) & 0xFF);
+    packet[4] = static_cast<uint8_t>(rpm_right & 0xFF);
+    packet[5] = disable ? 0 : 1;
+    uint8_t checksum = 0;
+    for (int i = 1; i < 6; ++i)
+      checksum += packet[i];
+    packet[6] = checksum;
+
+    if (write(arduino_port_.fd(), packet, sizeof(packet)) != static_cast<ssize_t>(sizeof(packet))) {
+      RCLCPP_ERROR(get_logger(), "아두이노 송신 오류");
+    }
+
+    int bytes_avail = 0;
+    if (ioctl(arduino_port_.fd(), FIONREAD, &bytes_avail) == 0 && bytes_avail >= 10) {
+      uint8_t head = 0;
+      if (read(arduino_port_.fd(), &head, 1) == 1 && head == 0xFD) {
+        uint8_t data[9];
+        ssize_t got = read(arduino_port_.fd(), data, 9);
+        if (got == 9) {
+          uint8_t sum = 0;
+          for (int i = 0; i < 8; ++i)
+            sum += data[i];
+          if (sum == data[8]) {
+            feedback_speeds[0] = static_cast<int16_t>((data[0] << 8) | data[1]);
+            feedback_speeds[1] = static_cast<int16_t>((data[2] << 8) | data[3]);
+            feedback_speeds[2] = static_cast<int16_t>((data[4] << 8) | data[5]);
+            feedback_speeds[3] = static_cast<int16_t>((data[6] << 8) | data[7]);
+          }
+        }
+      }
+    }
+  }
+
+  void processFeedback(const int feedback_speeds[4], double dt) {
+    double actual_rpm_l = (feedback_speeds[0] + feedback_speeds[2]) / 2.0;
+    double actual_rpm_r = (feedback_speeds[1] + feedback_speeds[3]) / 2.0;
+
+    double v_l = actual_rpm_l * 2.0 * kPi * wheel_radius_ / 60.0;
+    double v_r = actual_rpm_r * 2.0 * kPi * wheel_radius_ / 60.0;
+
+    double linear_vel = (v_r + v_l) / 2.0;
+    double angular_vel = (v_r - v_l) / wheel_base_;
+
+    double delta_th = angular_vel * dt;
+    odom_th_ += delta_th;
+    odom_x_ += linear_vel * std::cos(odom_th_) * dt;
+    odom_y_ += linear_vel * std::sin(odom_th_) * dt;
+
+    double rads_l = v_l / wheel_radius_;
+    double rads_r = v_r / wheel_radius_;
+    left_wheel_joint_pos_ += rads_l * dt;
+    right_wheel_joint_pos_ += rads_r * dt;
+
+    auto stamp = now();
+
+    sensor_msgs::msg::JointState joint_state;
+    joint_state.header.stamp = stamp;
+    joint_state.name = {"front_left_wheel_joint", "front_right_wheel_joint", "rear_left_wheel_joint",
+                         "rear_right_wheel_joint"};
+    joint_state.position = {left_wheel_joint_pos_, right_wheel_joint_pos_, left_wheel_joint_pos_,
+                             right_wheel_joint_pos_};
+    joint_state.velocity = {rads_l, rads_r, rads_l, rads_r};
+    joint_pub_->publish(joint_state);
+
+    nav_msgs::msg::Odometry odom;
+    odom.header.stamp = stamp;
+    odom.header.frame_id = "odom";
+    odom.child_frame_id = "base_link";
+    odom.pose.pose.position.x = odom_x_;
+    odom.pose.pose.position.y = odom_y_;
+    odom.pose.pose.position.z = 0.0;
+
+    double cy = std::cos(odom_th_ * 0.5);
+    double sy = std::sin(odom_th_ * 0.5);
+    odom.pose.pose.orientation.w = cy;
+    odom.pose.pose.orientation.z = sy;
+
+    odom.twist.twist.linear.x = linear_vel;
+    odom.twist.twist.angular.z = angular_vel;
+
+    odom_pub_->publish(odom);
+
+    // Nav2's local_costmap needs a live odom->base_link TF (not just the /odom_wheels
+    // topic) to transform sensor data into the robot frame. Only broadcast it when no
+    // LIDAR SLAM (FAST_LIO/FAST-LIVO2) is providing base_link's TF via aft_mapped —
+    // see publish_odom_tf_ declaration above for why both can't be active at once.
+    if (publish_odom_tf_) {
+      geometry_msgs::msg::TransformStamped tf_msg;
+      tf_msg.header.stamp = stamp;
+      tf_msg.header.frame_id = "odom";
+      tf_msg.child_frame_id = "base_link";
+      tf_msg.transform.translation.x = odom.pose.pose.position.x;
+      tf_msg.transform.translation.y = odom.pose.pose.position.y;
+      tf_msg.transform.translation.z = 0.0;
+      tf_msg.transform.rotation = odom.pose.pose.orientation;
+      tf_broadcaster_->sendTransform(tf_msg);
+    }
+  }
+
+  static constexpr int kMaxRpm = 250;
+
+  // Parameters
+  std::string control_method_;
+  std::string port_;
+  int baud_ = 115200;
+  double limit_v_ = 0.5;
+  double accel_limit_ = 0.5;
+  double wheel_base_ = 0.374;
+  double wheel_radius_ = 0.127;
+  bool publish_odom_tf_ = true;
+
+  // Yaw PI gains
+  const double kp_yaw_ = 0.50;
+  const double ki_yaw_ = 0.20;
+  double yaw_error_integral_ = 0.0;
+  const double max_integral_ = 1.0;
+
+  // Robot state
+  double target_v_ = 0.0, target_w_ = 0.0;
+  double current_v_ = 0.0, current_w_ = 0.0;
+  double current_imu_w_ = 0.0;
+  rclcpp::Time last_time_;
+
+  double odom_x_ = 0.0, odom_y_ = 0.0, odom_th_ = 0.0;
+  double left_wheel_joint_pos_ = 0.0, right_wheel_joint_pos_ = 0.0;
+
+  bool drivers_initialized_ = false;
+  bool drivers_enabled_ = false;
+  bool is_parked_ = false;
+  bool is_mock_mode_ = false;
+
+  SerialPort modbus_port_;
+  SerialPort arduino_port_;
+
+  std::optional<double> last_valid_voltage_;
+  std::optional<double> filtered_battery_voltage_;
+  std::deque<double> voltage_history_;
+
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_status_sub_;
+
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  rclcpp::TimerBase::SharedPtr control_timer_;
+  rclcpp::TimerBase::SharedPtr battery_timer_;
+};
+
+} // namespace fori_serial_bridge_cpp
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<fori_serial_bridge_cpp::ForiSerialBridge>();
+  rclcpp::on_shutdown([node]() { node->safeStop(); });
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

start_fori.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+# FORI AGV 안전 시작 스크립트
+# 이전 프로세스 잔류 문제를 자동으로 처리합니다
+
+echo "=========================================="
+echo "     FORI AGV - 안전 시작 스크립트"
+echo "=========================================="
+
+# 1. 이전 실행 잔류 프로세스 모두 종료
+echo "[1/3] 이전 프로세스 정리 중..."
+pkill -9 -f "serial_bridge_node" 2>/dev/null
+pkill -9 -f "parking_controller_node" 2>/dev/null
+pkill -9 -f "ui_server_node" 2>/dev/null
+pkill -9 -f "rosbridge_websocket" 2>/dev/null
+pkill -9 -f "web_video_server" 2>/dev/null
+pkill -9 -f "aruco_detector_node.py" 2>/dev/null
+pkill -9 -f "fori_full.launch" 2>/dev/null
+pkill -9 -f "fori_nav2.launch" 2>/dev/null
+fuser -k -9 8080/tcp 9090/tcp 8085/tcp 2>/dev/null
+sleep 2
+
+echo "[2/3] ROS2 환경 로드..."
+source /opt/ros/humble/setup.bash
+source /home/yoo/fori_ws/fori_ws/install/setup.bash
+
+# Hikrobot 카메라 SDK(/opt/MVS/lib/64)가 LD_LIBRARY_PATH 맨 앞에 있어서 시스템
+# libusb보다 먼저 잡히는데, 그 안의 libusb가 구버전이라 PCL(fast_lio, pcd_gridmap_converter
+# 등)이 필요로 하는 libusb_set_option 심볼이 없어 "symbol lookup error"로 죽는다.
+# 시스템 라이브러리 경로를 앞에 둬서 심볼 충돌만 피하고, MVS 경로는 카메라 전용 라이브러리
+# 탐색을 위해 그대로 뒤에 남겨둔다.
+export LD_LIBRARY_PATH="/usr/lib/x86_64-linux-gnu:/lib/x86_64-linux-gnu:${LD_LIBRARY_PATH}"
+
+echo "[3/3] FORI AGV 시작!"
+echo "  ✅ 웹 UI: http://localhost:8080"
+echo "  ✅ 외부(모바일): http://$(hostname -I | awk '{print $1}'):8080"
+echo "=========================================="
+ros2 launch fori_serial_bridge fori_full.launch.py