diff --git a/config/mid360s.yaml b/config/mid360s.yaml index a0ab010..82bde43 100644 --- a/config/mid360s.yaml +++ b/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 diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index d6f7ad7..2528832 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -485,7 +485,7 @@ void publish_frame_world(rclcpp::Publisher::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::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::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::SharedPtr pubOdomAftMapped, std::unique_ptr & 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::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::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;