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
2026-08-27 11:43:54 +09:00
parent 99ae42f5b9
commit 73cca7ee8f
6 changed files with 96 additions and 98 deletions
@@ -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 내 경로)
-    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')
+    # 3. URDF 파일 경로 (Fusion 360 실측 산출물 + aft_mapped/카메라/footprint
+    # 래핑, duru_urdf_description 패키지 - doc/07 Phase 1)
+    duru_urdf_dir = get_package_share_directory('duru_urdf_description')
+    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) 발행
-    with open(urdf_file_path, 'r') as infp:
-        robot_desc = infp.read()
+    # 1단계: TF (URDF) 발행 - xacro 파일이라 process_file로 펼쳐야 함
+    robot_desc = xacro.process_file(urdf_file_path).toxml()

    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)
-    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'
-    )
+    # 2단계: (구) camera_init -> odom / camera_init -> map 정적 TF 브릿지는 제거함.
+    # FAST_LIO(laserMapping.cpp)가 이제 그 프레임 이름을 "odom"으로 직접 발행하므로
+    # (doc/07 Phase 1), AMCL이 표준대로 map->odom을 동적으로 발행해도 더 이상 충돌이
+    # 없다 - 정적 aliasing 없이 map(AMCL, 동적) -> odom(FAST_LIO, 동적) -> aft_mapped
+    # -> base_link 표준 트리가 그대로 성립한다.

    # 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,
@@ -1,33 +1,19 @@
-map_server:
-  ros__parameters:
-    yaml_filename: "/home/yoo/pcd_gunsan_output/map.yaml"
-    use_sim_time: False
-amcl:
+velocity_smoother:
  ros__parameters:
    use_sim_time: False
-    transform_tolerance: 1.0
-    alpha1: 0.2
-    alpha2: 0.2
-    alpha3: 0.2
-    alpha4: 0.2
-    base_frame_id: "base_link"
-    global_frame_id: "map"
-    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
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.3, 0.0, 0.5]
+    min_velocity: [-0.3, 0.0, -0.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"

 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
-    default_nav_through_poses_bt_xml: ""  # Use Nav2 built-in default BehaviorTree
+    # 빈 문자열로 두면 Nav2가 기본 트리를 자동으로 찾아줄 거라 가정했었으나, 실측
+    # 결과 "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
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
+      rolling_window: true
+      width: 15
+      height: 15
+      resolution: 0.1
+      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
@@ -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)
-    ld.add_action(aruco_detector_node)
+    # 카메라 미연결 상태에서 hik_camera_ros2_driver가 "No camera found"를 계속
+    # 고빈도로 재시도하며 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')
-        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()
+        # Fusion 360 실측 산출물 + aft_mapped/카메라/footprint 래핑 (doc/07 Phase 1)
+        duru_urdf_dir = get_package_share_directory('duru_urdf_description')
+        urdf_file_path = os.path.join(duru_urdf_dir, 'urdf', 'fori_robot.xacro')
+        robot_desc = xacro.process_file(urdf_file_path).toxml()

        rsp_node = Node(
            package='robot_state_publisher',
@@ -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
-    double v_left = current_v_ - (current_w_ + correction) * wheel_base_ / 2.0;
-    double v_right = current_v_ + (current_w_ + correction) * wheel_base_ / 2.0;
+    // 3. Inverse kinematics -> wheel RPM (k_skid/effective_w 캘리브레이션 적용)
+    bool is_spin_turn = std::abs(current_v_) < 1e-6;
+    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",
-                         "rear_right_wheel_joint"};
-    joint_state.position = {left_wheel_joint_pos_, right_wheel_joint_pos_, left_wheel_joint_pos_,
+    joint_state.name = {"Revolute 2", "Revolute 4", "Revolute 3", "Revolute 5"};
+    joint_state.position = {left_wheel_joint_pos_, left_wheel_joint_pos_, right_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;