Skip to content

Commit

Permalink
Merge pull request hku-mars#100 from Es1erda/main
Browse files Browse the repository at this point in the history 
[Enh] Adapted for HesaiXT32 and added PCD saving functionality.
  • Loading branch information
@xuankuzcr
xuankuzcr authored Jun 20, 2024
2 parents ec193e3 + 3ef8f1a commit 17db337
Show file tree
Hide file tree
Showing 5 changed files with 164 additions and 17 deletions.
4 changes: 4 additions & 0 deletions config/NTU_VIRAL.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,10 @@ mapping:
0, 1, 0,
0, 0, 1]

pcd_save:
pcd_save_en: true
interval: -1

camera:
img_topic: /left/image_raw
# NTU_VIRAL
Expand Down
14 changes: 9 additions & 5 deletions config/avia_resize.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,17 +15,17 @@ ncc_en: false
ncc_thre: 0
img_point_cov : 100 # 1000
laser_point_cov : 0.001 # 0.001
cam_fx: 453.483063
cam_fy: 453.254913
cam_cx: 318.908851
cam_cy: 234.238189
cam_fx: 431.795259219
cam_fy: 431.550090267
cam_cx: 310.833037316
cam_cy: 266.985989326

common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"

preprocess:
lidar_type: 1 # Livox Avia LiDAR
lidar_type: 1 # 1:Livox Avia LiDAR 2:VELO16 3:OUST64 4:XT32
scan_line: 6
blind: 5 # blind x m disable

Expand All @@ -38,6 +38,10 @@ mapping:
0, 1, 0,
0, 0, 1]

pcd_save:
pcd_save_en: true
interval: -1

camera:
# img_topic: /usb_cam/image_raw
# img_topic: /camera/image_color
Expand Down
20 changes: 18 additions & 2 deletions include/preprocess.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ using namespace std;
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;

enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
enum LID_TYPE{AVIA = 1, VELO16, OUST64, XT32}; //{1, 2, 3}
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
enum Surround{Prev, Next};
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
Expand Down Expand Up @@ -62,6 +62,21 @@ namespace ouster_ros {
};
} // namespace ouster_ros

namespace xt32_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
float intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace xt32_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(xt32_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(uint16_t, ring, ring))


// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
Expand Down Expand Up @@ -94,14 +109,15 @@ class Preprocess
vector<orgtype> typess[128]; //maximum 128 line lidar
int lidar_type, point_filter_num, N_SCANS;;
double blind;
bool feature_enabled;
bool feature_enabled,given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn;


private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
Expand Down
47 changes: 43 additions & 4 deletions src/laserMapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,13 @@ geometry_msgs::PoseStamped msg_body_pose;

shared_ptr<Preprocess> p_pre(new Preprocess());

PointCloudXYZRGB::Ptr pcl_wait_save(new PointCloudXYZRGB()); //add save rbg map
PointCloudXYZI::Ptr pcl_wait_save_lidar(new PointCloudXYZI()); //add save xyzi map

bool pcd_save_en = true;
int pcd_save_interval = 20, pcd_index = 0;


void SigHandle(int sig)
{
flg_exit = true;
Expand Down Expand Up @@ -766,6 +773,10 @@ void publish_frame_world_rgb(const ros::Publisher & pubLaserCloudFullRes, lidar_
// pcl_wait_pub->clear();
}
// mtx_buffer_pointcloud.unlock();
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. noted that pcd save will influence the real-time performences **/
if (pcd_save_en) *pcl_wait_save += *laserCloudWorldRGB;
}

void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
Expand Down Expand Up @@ -800,6 +811,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
// pcl_wait_pub->clear();
}
// mtx_buffer_pointcloud.unlock();
if (pcd_save_en) *pcl_wait_save_lidar += *pcl_wait_pub;
}

void publish_visual_world_map(const ros::Publisher & pubVisualCloud)
Expand Down Expand Up @@ -1122,6 +1134,9 @@ void readParameters(ros::NodeHandle &nh)
nh.param<int>("patch_size", patch_size, 4);
nh.param<double>("outlier_threshold",outlier_threshold,100);
nh.param<double>("ncc_thre", ncc_thre, 100);

nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);

}

int main(int argc, char** argv)
Expand Down Expand Up @@ -1373,7 +1388,7 @@ int main(int argc, char** argv)
out_msg.image = img_rgb;
img_pub.publish(out_msg.toImageMsg());

publish_frame_world_rgb(pubLaserCloudFullResRgb, lidar_selector);
if(img_en) publish_frame_world_rgb(pubLaserCloudFullResRgb, lidar_selector);
publish_visual_world_sub_map(pubSubVisualCloud);

// *map_cur_frame_point = *pcl_wait_pub;
Expand Down Expand Up @@ -1758,7 +1773,7 @@ int main(int argc, char** argv)
}
*pcl_wait_pub = *laserCloudWorld;

publish_frame_world(pubLaserCloudFullRes);
if(!img_en) publish_frame_world(pubLaserCloudFullRes);
// publish_visual_world_map(pubVisualCloud);
publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap);
Expand Down Expand Up @@ -1820,6 +1835,32 @@ int main(int argc, char** argv)
// pcd_writer.writeBinary(corner_filename, corner_points);
// }

/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en)
{
// string file_name = string("saved.pcd");
// string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
string all_points_dir = "/home/sheng/Downloads/yty_bag/rgb_saved.pcd";
pcl::PCDWriter pcd_writer;
cout << "current rgb scan saved" << endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
}

if (pcl_wait_save_lidar->size() > 0 && pcd_save_en)
{
// string file_name = string("saved.pcd");
// string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
string all_points_dir = "/home/sheng/Downloads/yty_bag/xyzi_saved.pcd";
pcl::PCDWriter pcd_writer;
cout << "current xyzi scan saved" << endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_lidar);
}

fout_out.close();
fout_pre.close();

#ifndef DEPLOY
vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
FILE *fp2;
Expand Down Expand Up @@ -1849,8 +1890,6 @@ int main(int argc, char** argv)
// plt::pause(0.5);
// plt::close();
}
cout << "no points saved" << endl;
#endif

return 0;
}
96 changes: 90 additions & 6 deletions src/preprocess.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ Preprocess::Preprocess()
edgeb = 0.1;
smallp_intersect = 172.5;
smallp_ratio = 1.2;
given_offset_time = false;

jump_up_limit = cos(jump_up_limit/180*M_PI);
jump_down_limit = cos(jump_down_limit/180*M_PI);
Expand Down Expand Up @@ -56,6 +57,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
case VELO16:
velodyne_handler(msg);
break;

case XT32:
xt32_handler(msg);
break;

default:
printf("Error LiDAR Type");
Expand All @@ -64,6 +69,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
*pcl_out = pl_surf;
}


void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
pl_surf.clear();
Expand Down Expand Up @@ -255,6 +261,8 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}

#define MAX_LINE_NUM 64

void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pl_surf.clear();
Expand All @@ -266,14 +274,14 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
uint plsize = pl_orig.points.size();
pl_surf.reserve(plsize);

bool is_first[16];
bool is_jump[16]={false}; // if jump point
double yaw_fp[20]={0}; // yaw of first scan point
bool is_first[MAX_LINE_NUM];
bool is_jump[MAX_LINE_NUM]={false}; // if jump point
double yaw_fp[MAX_LINE_NUM]={0}; // yaw of first scan point
int layer; // layer number
double omega_l=3.61; // scan angular velocity
float yaw_last[16]={0.0}; // yaw of last scan point
float time_last[16]={0.0}; // last offset time
float time_jump[16]={0.0}; // offset time before jump
float yaw_last[MAX_LINE_NUM]={0.0}; // yaw of last scan point
float time_last[MAX_LINE_NUM]={0.0}; // last offset time
float time_jump[MAX_LINE_NUM]={0.0}; // offset time before jump
memset(is_first, true, sizeof(is_first));

double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
Expand Down Expand Up @@ -425,6 +433,82 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}

void Preprocess::xt32_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();

pcl::PointCloud<xt32_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.points.size();
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
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].timestamp > 0) { given_offset_time = true; }
else
{
given_offset_time = false;
memset(is_first, true, sizeof(is_first));
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
double yaw_end = yaw_first;
int layer_first = pl_orig.points[0].ring;
for (uint i = plsize - 1; i > 0; i--)
{
if (pl_orig.points[i].ring == layer_first)
{
yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
break;
}
}
}

double time_head = pl_orig.points[0].timestamp;

for (int i = 0; i < plsize; i++)
{
PointType added_pt;
// cout<<"!!!!!!"<<i<<" "<<plsize<<endl;

added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.x = pl_orig.points[i].x;
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].timestamp - time_head) * 1000.f;

// printf("added_pt.curvature: %lf %lf \n", added_pt.curvature,
// pl_orig.points[i].timestamp);

// if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time:
// %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature,
// prints);
if (i % point_filter_num == 0)
{
if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind)
{
pl_surf.points.push_back(added_pt);
// printf("time mode: %d time: %d \n", given_offset_time,
// pl_orig.points[i].t);
}
}
}

// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_surf, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}




void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
{
uint plsize = pl.size();
Expand Down

0 comments on commit 17db337

Please sign in to comment.