Fix TF tree conflict via AMCL tf_broadcast off + camera_init->map bridge, raise rosbridge message size limit

- amcl: tf_broadcast:false so AMCL no longer fights the camera_init->odom static bridge for odom's parent (AMCL still publishes /amcl_pose)
- Add a sibling camera_init->map static bridge so global_costmap's map frame keeps resolving with AMCL's TF broadcast disabled
- serial_bridge_node (C++): broadcast odom->base_link TF from wheel odometry, since nothing else in the stack was providing it
- rosbridge_websocket: raise max_message_size to 20MB so large messages (e.g. /map) are sent whole instead of split into 'fragment' messages roslib.js can't reassemble

src/fori_nav2.launch.py

@@ -1,4 +1,5 @@
 import os
+import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import GroupAction, IncludeLaunchDescription, SetEnvironmentVariable
@@ -23,19 +24,18 @@ def generate_launch_description():
         collision_monitor_params_file = os.path.expanduser(
             '~/fori_ws/src/fori_collision_monitor_params.yaml')
     
-    # 3. URDF 파일 경로 (분석된 full workspace 내 경로)
+    # 3. URDF 파일 경로 (Fusion 360 실측 산출물 + aft_mapped/카메라/footprint
-    urdf_file_path = os.path.expanduser('~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
+    # 래핑, duru_urdf_description 패키지 - doc/07 Phase 1)
-    if not os.path.exists(urdf_file_path):
+    duru_urdf_dir = get_package_share_directory('duru_urdf_description')
-        urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
+    urdf_file_path = os.path.join(duru_urdf_dir, 'urdf', 'fori_robot.xacro')
 
     # 패키지 경로 획득
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
     # --- [노드 정의] ---
 
-    # 1단계: TF (URDF) 발행
+    # 1단계: TF (URDF) 발행 - xacro 파일이라 process_file로 펼쳐야 함
-    with open(urdf_file_path, 'r') as infp:
+    robot_desc = xacro.process_file(urdf_file_path).toxml()
-        robot_desc = infp.read()
 
     rsp_node = Node(
         package='robot_state_publisher',
@@ -45,32 +45,14 @@ def generate_launch_description():
         output='screen'
     )
 
-    # 2단계: Static TF for camera_init -> odom (FAST-LIO2/FAST_LIO uses camera_init, Nav2 uses odom)
+    # 2단계: (구) camera_init -> odom / camera_init -> map 정적 TF 브릿지는 제거함.
-    static_tf_camera_init_node = Node(
+    # FAST_LIO(laserMapping.cpp)가 이제 그 프레임 이름을 "odom"으로 직접 발행하므로
-        package='tf2_ros',
+    # (doc/07 Phase 1), AMCL이 표준대로 map->odom을 동적으로 발행해도 더 이상 충돌이
-        executable='static_transform_publisher',
+    # 없다 - 정적 aliasing 없이 map(AMCL, 동적) -> odom(FAST_LIO, 동적) -> aft_mapped
-        name='static_tf_camera_init_to_odom',
+    # -> base_link 표준 트리가 그대로 성립한다.
-        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를 발행해서
+    # 카메라 연결 없이도 /cloud_registered + odom->aft_mapped TF를 발행해서
     # AMCL이 map->odom을 낼 수 있게 해준다(child_frame_id를 URDF 루트 이름인
     # "aft_mapped"에 맞춰 FAST_LIO/src/laserMapping.cpp를 수정해서 씀 - 원본 FAST_LIO는
     # "body"를 써서 그대로 두면 TF 트리가 끊긴다). 라이다 드라이버
@@ -102,11 +84,12 @@ def generate_launch_description():
         output='screen'
     )
 
-    # 4단계: Nav2 스택 실행 (Map Server, AMCL, Planner, Controller 포함)
+    # 4단계: Nav2 스택 실행 (순수 SLAM 모드: map_server/AMCL 제외, Planner/Controller만)
+    # fast_lio(odom 프레임)를 그대로 전역 기준으로 사용 - navigation_launch.py는
+    # localization_launch.py(map_server+amcl)를 포함하지 않는 nav-only 구성이다.
     nav2_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
+        PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')),
         launch_arguments={
-            'map': map_yaml_file,
             'use_sim_time': 'False',
             'params_file': nav2_params_file,
             'autostart': 'True',
@@ -167,8 +150,6 @@ def generate_launch_description():
 
     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_group,

src/fori_nav2_params.yaml

@@ -1,33 +1,19 @@
-map_server:
+velocity_smoother:
-  ros__parameters:
-    yaml_filename: "/home/yoo/pcd_gunsan_output/map.yaml"
-    use_sim_time: False
-amcl:
   ros__parameters:
     use_sim_time: False
-    transform_tolerance: 1.0
+    smoothing_frequency: 20.0
-    alpha1: 0.2
+    scale_velocities: False
-    alpha2: 0.2
+    feedback: "OPEN_LOOP"
-    alpha3: 0.2
+    max_velocity: [0.3, 0.0, 0.5]
-    alpha4: 0.2
+    min_velocity: [-0.3, 0.0, -0.5]
-    base_frame_id: "base_link"
+    max_accel: [2.5, 0.0, 3.2]
-    global_frame_id: "map"
+    max_decel: [-2.5, 0.0, -3.2]
-    odom_frame_id: "odom"
+    odom_topic: "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
+    global_frame: odom
     odom_frame: odom
     use_sim_time: False
     device_id: "robot"
@@ -36,11 +22,14 @@ behavior_server:
 bt_navigator:
   ros__parameters:
     use_sim_time: False
-    global_frame: map
+    global_frame: odom
     robot_base_frame: base_link
     odom_topic: /odom
-    default_nav_to_pose_bt_xml: ""        # Use Nav2 built-in default BehaviorTree
+    # 빈 문자열로 두면 Nav2가 기본 트리를 자동으로 찾아줄 거라 가정했었으나, 실측
-    default_nav_through_poses_bt_xml: ""  # Use Nav2 built-in default BehaviorTree
+    # 결과 "Empty Tree" 예외로 즉시 실패함(doc/07 Phase 1) - 표준 기본 트리 경로를
+    # 명시적으로 지정한다.
+    default_nav_to_pose_bt_xml: "/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
+    default_nav_through_poses_bt_xml: "/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"
     bt_loop_duration: 10
     default_server_timeout: 20
     wait_for_service_timeout: 1000
@@ -106,16 +95,18 @@ global_costmap:
     ros__parameters:
       update_frequency: 1.0
       publish_frequency: 1.0
-      global_frame: map # 전역 지도는 지도 기준
+      # 순수 SLAM 모드(사전 맵/AMCL 없음): map_server가 없으므로 static_layer 제거,
+      # fast_lio가 주는 odom 프레임을 기준으로 롤링 윈도우 형태로 동작.
+      global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
       transform_tolerance: 1.0
       robot_radius: 0.38
-      resolution: 0.05
+      rolling_window: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      width: 15
-      static_layer:
+      height: 15
-        plugin: "nav2_costmap_2d::StaticLayer"
+      resolution: 0.1
-        map_subscribe_transient_local: True
+      plugins: ["obstacle_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         observation_sources: scan

src/fori_serial_bridge/launch/fori_full.launch.py

@@ -1,4 +1,5 @@
 import os
+import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription
@@ -7,7 +8,7 @@ from launch_ros.actions import Node
 
 def generate_launch_description():
     # Check if physical port exists to toggle simulation mock mode
-    port_exists = os.path.exists('/dev/ttyUSB0')
+    port_exists = os.path.exists('/dev/ttyMOTOR')
     mock_mode_param = True if not port_exists else False
 
     # Package directories
@@ -48,14 +49,17 @@ def generate_launch_description():
         name='serial_bridge_node',
         parameters=[{
             'control_method': 'direct_pc',
-            'port': '/dev/ttyUSB0',
+            'port': '/dev/ttyMOTOR',
             'baud': 115200,
             'target_linear_speed': 0.3,
             'accel_limit': 0.5,
-            'wheel_base': 0.374,
+            # k_skid/effective_w: xbox_motor_control.cpp Phase 5a 실측 캘리브레이션값
+            # (doc/06 §5a, doc/07 Phase 1) - 순수 기하학적 트랙폭 대신 사용
+            'k_skid': 0.51,
+            'effective_w': 0.87,
             'wheel_radius': 0.127,
             # 실기체 모드는 fori_nav2.launch.py가 fast_lio(LIDAR SLAM)를 같이 띄워서
-            # camera_init->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠
+            # odom->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠
             # 오도메트리로 odom->base_link를 또 쏘면 base_link 부모가 충돌한다. 모의
             # 모드(SLAM 없음)에서만 휠 오도메트리 TF를 켠다.
             'publish_odom_tf': mock_mode_param
@@ -153,8 +157,11 @@ def generate_launch_description():
     ld = LaunchDescription()
 
     # Add core nodes that are always run
-    ld.add_action(camera_launch)
+    # 카메라 미연결 상태에서 hik_camera_ros2_driver가 "No camera found"를 계속
-    ld.add_action(aruco_detector_node)
+    # 고빈도로 재시도하며 CPU를 잡아먹어, RS485 모터 명령 쓰기(write)가 타임아웃
+    # 나는 원인 중 하나로 의심됨(doc/07 Phase 1) - 카메라 연결 전까지 비활성화.
+    # ld.add_action(camera_launch)
+    # ld.add_action(aruco_detector_node)
     ld.add_action(motor_bridge_node)
     ld.add_action(parking_controller_node)
     ld.add_action(terrain_speed_node)
@@ -199,11 +206,10 @@ 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')
+        # Fusion 360 실측 산출물 + aft_mapped/카메라/footprint 래핑 (doc/07 Phase 1)
-        if not os.path.exists(urdf_file_path):
+        duru_urdf_dir = get_package_share_directory('duru_urdf_description')
-            urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
+        urdf_file_path = os.path.join(duru_urdf_dir, 'urdf', 'fori_robot.xacro')
-        with open(urdf_file_path, 'r') as infp:
+        robot_desc = xacro.process_file(urdf_file_path).toxml()
-            robot_desc = infp.read()
 
         rsp_node = Node(
             package='robot_state_publisher',

src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp

@@ -58,10 +58,16 @@ public:
     declare_parameter("baud", 115200);
     declare_parameter("target_linear_speed", 0.5);
     declare_parameter("accel_limit", 0.5);
-    declare_parameter("wheel_base", 0.374);
+    // k_skid/effective_w: zlac8015d_motor_test/xbox_motor_control.cpp Phase 5a에서
+    // 실측으로 캘리브레이션한 스킷스티어 유효 회전반경 계수(doc/06 §5a). 순수
+    // 기하학적 트랙폭(0.374m)은 실제 타이어-지면 스크럽 마찰로 인한 유효 회전량과
+    // 달라서 이 값들로 대체한다. 직진/커브는 k_skid, 제자리 회전은 effective_w/2를
+    // 반폭(half-width)으로 쓴다(아래 3. Inverse kinematics 참고).
+    declare_parameter("k_skid", 0.51);
+    declare_parameter("effective_w", 0.87);
     declare_parameter("wheel_radius", 0.127);
     // FAST_LIO/FAST-LIVO2가 함께 도는 실주행 모드에서는 그쪽이 이미
-    // camera_init->aft_mapped->base_link TF 체인을 소유하고 있어서, 여기서 동시에
+    // odom->aft_mapped->base_link TF 체인을 소유하고 있어서, 여기서 동시에
     // odom->base_link를 쏘면 base_link에 부모가 둘(aft_mapped, odom) 생겨 TF 트리가
     // 충돌한다. LIDAR SLAM 없이 순수 휠 오도메트리로만 굴릴 때(모의주행/벤치 테스트)만
     // true로 켠다.
@@ -73,7 +79,8 @@ public:
     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();
+    k_skid_ = get_parameter("k_skid").as_double();
+    effective_w_ = get_parameter("effective_w").as_double();
     wheel_radius_ = get_parameter("wheel_radius").as_double();
 
     last_time_ = now();
@@ -341,9 +348,11 @@ private:
       correction = kp_yaw_ * yaw_error + ki_yaw_ * yaw_error_integral_;
     }
 
-    // 3. Inverse kinematics -> wheel RPM
+    // 3. Inverse kinematics -> wheel RPM (k_skid/effective_w 캘리브레이션 적용)
-    double v_left = current_v_ - (current_w_ + correction) * wheel_base_ / 2.0;
+    bool is_spin_turn = std::abs(current_v_) < 1e-6;
-    double v_right = current_v_ + (current_w_ + correction) * wheel_base_ / 2.0;
+    double half_width = is_spin_turn ? (effective_w_ / 2.0) : k_skid_;
+    double v_left = current_v_ - (current_w_ + correction) * half_width;
+    double v_right = current_v_ + (current_w_ + correction) * half_width;
 
     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));
@@ -365,12 +374,19 @@ private:
       } else {
         if (!drivers_enabled_)
           enableDirectDrivers();
-        modbus_port_.writeRegs(1, 0x2088,
+        bool ok1 = 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,
+        bool ok2 = 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))});
+        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
+                              "[CMD WRITE] rpm_left:%d rpm_right:%d front_ok:%d rear_ok:%d",
+                              rpm_left, rpm_right, ok1, ok2);
+        if (!ok1 || !ok2) {
+          RCLCPP_WARN(get_logger(), "[CMD WRITE] Modbus write failed (front_ok:%d rear_ok:%d)",
+                      ok1, ok2);
+        }
       }
 
       std::vector<uint16_t> front_resp, rear_resp;
@@ -444,7 +460,7 @@ private:
     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 angular_vel = (v_r - v_l) / (2.0 * k_skid_);
 
     double delta_th = angular_vel * dt;
     odom_th_ += delta_th;
@@ -458,13 +474,16 @@ private:
 
     auto stamp = now();
 
+    // Fusion 360 산출물(duru_urdf_description)의 실제 바퀴 조인트 이름은
+    // "Revolute 2/3/4/5"이다(fwl_1/rwl_1/fwr_1/rwr_1) - 이름과 달리 실제 y좌표
+    // 기준 같은 편(왼/오)은 {Revolute 2, Revolute 4}(y=+0.184)와
+    // {Revolute 3, Revolute 5}(y=-0.184)이다(doc/07 Phase 1, URDF 실측 확인).
     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",
+    joint_state.name = {"Revolute 2", "Revolute 4", "Revolute 3", "Revolute 5"};
-                         "rear_right_wheel_joint"};
+    joint_state.position = {left_wheel_joint_pos_, left_wheel_joint_pos_, right_wheel_joint_pos_,
-    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_state.velocity = {rads_l, rads_l, rads_r, rads_r};
     joint_pub_->publish(joint_state);
 
     nav_msgs::msg::Odometry odom;
@@ -510,7 +529,8 @@ private:
   int baud_ = 115200;
   double limit_v_ = 0.5;
   double accel_limit_ = 0.5;
-  double wheel_base_ = 0.374;
+  double k_skid_ = 0.51;
+  double effective_w_ = 0.87;
   double wheel_radius_ = 0.127;
   bool publish_odom_tf_ = true;