Publish LIO anchor frame as odom, calibrate LiDAR mount pitch, tune point filtering

- Rename the published frame_id from camera_init to odom everywhere in laserMapping.cpp (child stays aft_mapped, matching the URDF root) so AMCL's map->odom broadcast no longer conflicts with a separate static camera_init->odom bridge in fori_ws's Nav2 launch
- Set extrinsic_R for the LiDAR's actual 20-degree nose-down mounting pitch (was identity)
- Increase point_filter_num and filter_size_surf/map for lighter point cloud load

config/mid360s.yaml

@@ -1,10 +1,10 @@
 /**:
     ros__parameters:
         feature_extract_enable: false
-        point_filter_num: 4
+        point_filter_num: 7
         max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
+        filter_size_surf: 0.8
+        filter_size_map: 0.8
         cube_side_length: 200.0
         runtime_pos_log_enable: false
         map_file_path: "./PCD/scans.pcd"
@@ -31,9 +31,13 @@
             det_range:     50.0
             extrinsic_est_en:  false
             extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
-            extrinsic_R: [ 1.0, 0.0, 0.0,
-                           0.0, 1.0, 0.0,
-                           0.0, 0.0, 1.0 ]
+            # 라이다가 전방 지면을 보도록 20도 nose-down(pitch)으로 장착됨.
+            # extrinsic_R은 "라이다 자세를 IMU/바디 프레임 기준으로" 표현(FAST-LIO
+            # README: extrinsic = LiDAR pose in IMU frame). Ry(+20deg): forward(+X)가
+            # -Z(아래) 쪽으로 기움 -> nose-down과 일치.
+            extrinsic_R: [ 0.9396926, 0.0, 0.3420201,
+                           0.0,       1.0, 0.0,
+                          -0.3420201, 0.0, 0.9396926 ]
 
         publish:
             path_en: true                # true: publish Path

src/laserMapping.cpp

@@ -485,7 +485,7 @@ void publish_frame_world(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::Share
         pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
         // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
         laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
-        laserCloudmsg.header.frame_id = "camera_init";
+        laserCloudmsg.header.frame_id = "odom";
         pubLaserCloudFull->publish(laserCloudmsg);
         publish_count -= PUBFRAME_PERIOD;
     }
@@ -554,7 +554,7 @@ void publish_effect_world(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::Shar
     sensor_msgs::msg::PointCloud2 laserCloudFullRes3;
     pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
     laserCloudFullRes3.header.stamp = get_ros_time(lidar_end_time);
-    laserCloudFullRes3.header.frame_id = "camera_init";
+    laserCloudFullRes3.header.frame_id = "odom";
     pubLaserCloudEffect->publish(laserCloudFullRes3);
 }
 
@@ -576,13 +576,13 @@ void publish_map(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub
     pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg);
     // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
     laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
-    laserCloudmsg.header.frame_id = "camera_init";
+    laserCloudmsg.header.frame_id = "odom";
     pubLaserCloudMap->publish(laserCloudmsg);
 
     // sensor_msgs::msg::PointCloud2 laserCloudMap;
     // pcl::toROSMsg(*featsFromMap, laserCloudMap);
     // laserCloudMap.header.stamp = get_ros_time(lidar_end_time);
-    // laserCloudMap.header.frame_id = "camera_init";
+    // laserCloudMap.header.frame_id = "odom";
     // pubLaserCloudMap->publish(laserCloudMap);
 }
 
@@ -607,10 +607,13 @@ void set_posestamp(T & out)
 
 void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped, std::unique_ptr<tf2_ros::TransformBroadcaster> & tf_br)
 {
-    odomAftMapped.header.frame_id = "camera_init";
-    // URDF(fori_robot.urdf) 루트 링크 이름이 "aft_mapped"라서(FAST-LIVO2 관례를 따름),
+    // 이 프레임(과거 이름 "camera_init")은 FAST-LIO의 고정 매핑 시작 원점으로,
+    // ROS 관례상 표준 오도메트리 프레임 "odom"과 역할이 같다. AMCL이 표준대로
+    // map->odom을 발행할 수 있게 이름을 "odom"으로 맞춘다(doc/07 Phase 1).
+    odomAftMapped.header.frame_id = "odom";
+    // URDF(fori_robot.xacro) 루트 링크 이름이 "aft_mapped"라서(FAST-LIVO2 관례를 따름),
     // TF 체인이 base_link까지 이어지려면 이 child_frame_id가 URDF 루트 이름과 일치해야
-    // 한다. 기본 FAST_LIO 상류 코드의 "body"를 그대로 쓰면 camera_init->body 트리와
+    // 한다. 기본 FAST_LIO 상류 코드의 "body"를 그대로 쓰면 odom->body 트리와
     // aft_mapped->base_link 트리가 서로 안 이어져 Nav2가 TF를 못 찾는다.
     odomAftMapped.child_frame_id = "aft_mapped";
     odomAftMapped.header.stamp = get_ros_time(lidar_end_time);
@@ -629,7 +632,7 @@ void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPt
     }
 
     geometry_msgs::msg::TransformStamped trans;
-    trans.header.frame_id = "camera_init";
+    trans.header.frame_id = "odom";
     trans.header.stamp = odomAftMapped.header.stamp;
     trans.child_frame_id = "aft_mapped";
     trans.transform.translation.x = odomAftMapped.pose.pose.position.x;
@@ -646,7 +649,7 @@ void publish_path(rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath)
 {
     set_posestamp(msg_body_pose);
     msg_body_pose.header.stamp = get_ros_time(lidar_end_time); // ros::Time().fromSec(lidar_end_time);
-    msg_body_pose.header.frame_id = "camera_init";
+    msg_body_pose.header.frame_id = "odom";
 
     /*** if path is too large, the rvis will crash ***/
     static int jjj = 0;
@@ -857,7 +860,7 @@ public:
         RCLCPP_INFO(this->get_logger(), "p_pre->lidar_type %d", p_pre->lidar_type);
 
         path.header.stamp = this->get_clock()->now();
-        path.header.frame_id ="camera_init";
+        path.header.frame_id ="odom";
 
         // /*** variables definition ***/
         // int effect_feat_num = 0, frame_num = 0;
@@ -1124,6 +1127,31 @@ private:
                     RCLCPP_WARN(this->get_logger(), "Position jump clamped: %.2fm -> 1.5m", pos_jump);
                 }
                 position_last = state_point.pos;
+
+                // Ground-vehicle planar constraint: this robot is wheeled and cannot
+                // physically pitch/roll beyond normal terrain slope. If ICP degeneracy
+                // (e.g. "No Effective Points!") lets the orientation drift off-plane,
+                // the tilted attitude corrupts every downstream 2D consumer (AMCL,
+                // costmaps, collision_monitor) via pointcloud_to_laserscan's
+                // base_link-relative height slicing. Clamp tilt-from-vertical to keep
+                // yaw free but pull roll/pitch back toward level.
+                Eigen::Matrix3d R_cur = state_point.rot.toRotationMatrix();
+                double r22_clamped = R_cur(2, 2);
+                if (r22_clamped > 1.0) r22_clamped = 1.0;
+                if (r22_clamped < -1.0) r22_clamped = -1.0;
+                double tilt = std::acos(r22_clamped);
+                constexpr double kMaxTiltRad = 0.436332; // 25 degrees
+                if (tilt > kMaxTiltRad)
+                {
+                    double yaw = std::atan2(R_cur(1, 0), R_cur(0, 0));
+                    Eigen::Matrix3d R_yaw_only;
+                    R_yaw_only << std::cos(yaw), -std::sin(yaw), 0,
+                                  std::sin(yaw),  std::cos(yaw), 0,
+                                  0,               0,             1;
+                    state_point.rot = SO3(R_yaw_only);
+                    kf.change_x(state_point);
+                    RCLCPP_WARN(this->get_logger(), "Orientation tilt clamped: %.1f deg -> 0 deg (yaw preserved)", tilt * 57.29578);
+                }
             }
             euler_cur = SO3ToEuler(state_point.rot);
             pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;