diff --git a/include/preprocess.h b/include/preprocess.h index 8e99866..9830303 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -70,13 +70,13 @@ struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; - std::uint32_t t; + float time; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, t, t)(std::uint16_t, ring, ring)) + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring)) /****************/ /*** Ouster ***/ @@ -146,7 +146,7 @@ class Preprocess PointCloudXYZI pl_full, pl_corn, pl_surf; PointCloudXYZI pl_buff[128]; // maximum 128 line lidar vector typess[128]; // maximum 128 line lidar - int lidar_type, point_filter_num, N_SCANS; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; double blind, blind_sqr; bool feature_enabled, given_offset_time; diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index f558d00..d8bc2e6 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -105,6 +105,7 @@ void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node) try_declare.template operator()("preprocess.lidar_type", AVIA); try_declare.template operator()("preprocess.scan_line",6); try_declare.template operator()("preprocess.point_filter_num", 3); + try_declare.template operator()("preprocess.scan_rate", 10); try_declare.template operator()("preprocess.feature_extract_enabled", false); try_declare.template operator()("pcd_save.interval", -1); @@ -161,6 +162,7 @@ void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node) this->node->get_parameter("preprocess.filter_size_surf", filter_size_surf_min); this->node->get_parameter("preprocess.lidar_type", p_pre->lidar_type); this->node->get_parameter("preprocess.scan_line", p_pre->N_SCANS); + this->node->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE); this->node->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num); this->node->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 9e528ca..e3a74fd 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -19,6 +19,7 @@ Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), po { inf_bound = 10; N_SCANS = 6; + SCAN_RATE = 10; group_size = 8; disA = 0.01; disA = 0.1; // B? @@ -347,15 +348,16 @@ void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstShar pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); + if (plsize == 0) return; pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point - double omega_l = 3.61; // scan angular velocity + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - if (pl_orig.points[plsize - 1].t > 0) { given_offset_time = true; } + if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } else { given_offset_time = false; @@ -393,7 +395,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstShar added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].t / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms if (!given_offset_time) { @@ -456,7 +458,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstShar added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].t / 1000.0; + added_pt.curvature = pl_orig.points[i].time / 1000.0; if (!given_offset_time) {