From 73cca7ee8f5ea80cac850f44cdde1bc24c9f4850 Mon Sep 17 00:00:00 2001 From: robin Date: Thu, 27 Aug 2026 11:43:54 +0900 Subject: [PATCH] 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 | 57 ++++++------------ src/fori_nav2_params.yaml | 57 ++++++++---------- .../parking_controller_node.cpython-310.pyc | Bin 24126 -> 24126 bytes .../ui_server_node.cpython-310.pyc | Bin 3715 -> 3715 bytes .../launch/fori_full.launch.py | 32 ++++++---- .../src/serial_bridge_node.cpp | 48 ++++++++++----- 6 files changed, 96 insertions(+), 98 deletions(-) diff --git a/src/fori_nav2.launch.py b/src/fori_nav2.launch.py index 7247755..0bcfad0 100644 --- a/src/fori_nav2.launch.py +++ b/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,20 +24,19 @@ 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', executable='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, diff --git a/src/fori_nav2_params.yaml b/src/fori_nav2_params.yaml index 9bb0bb2..4288f47 100644 --- a/src/fori_nav2_params.yaml +++ b/src/fori_nav2_params.yaml @@ -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 diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc index 059a49cff650ce2c5a60f029696ee1bc6f9b9742..d1ae689534b4c8652d735a2124e0e254de6f1b32 100644 GIT binary patch delta 22 ccmdnDhjHH?M(%uGUM>b8*wWUwk=r;908-QjssI20 delta 22 ccmdnDhjHH?M(%uGUM>b8kO;5c$ZZ@408JqU&Hw-a diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc index f9e1296016978ef639733365906094a145dbb6f4..c2585f9b825d456998c55caccea019d097a9ac63 100644 GIT binary patch delta 20 ZcmZpcZI)TeH*ze_y8|k1nB?( delta 20 acmZpcZIaft_mapped->base_link TF를 이미 발행하므로, 여기서 휠 + # odom->aft_mapped->base_link TF를 이미 발행하므로, 여기서 휠 # 오도메트리로 odom->base_link를 또 쏘면 base_link 부모가 충돌한다. 모의 # 모드(SLAM 없음)에서만 휠 오도메트리 TF를 켠다. 'publish_odom_tf': mock_mode_param }], output='screen' ) - + # 5. Launch Parking Controller Node parking_controller_node = Node( package='fori_serial_bridge', @@ -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,12 +206,11 @@ 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', executable='robot_state_publisher', diff --git a/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp b/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp index 8d53068..2c0d462 100644 --- a/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp +++ b/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 - 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(std::lround(v_left / (2.0 * kPi * wheel_radius_) * 60.0)); int rpm_right = static_cast(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(static_cast(rpm_left)), static_cast(static_cast(-rpm_right))}); - modbus_port_.writeRegs(2, 0x2088, + bool ok2 = modbus_port_.writeRegs(2, 0x2088, {static_cast(static_cast(rpm_left)), static_cast(static_cast(-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 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;