mirrors/FAST_LIO
1
0
Fork 0
mirror of https://github.com/hku-mars/FAST_LIO.git synced 2023-04-06 16:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

add pcl_viewer tips

Browse Source
This commit is contained in:
xw
2021-07-07 04:05:59 -04:00
parent c05f2ad77d
commit 8c50d839dc
2 changed files with 20 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/laserMapping.cpp
View File

@@ -289,6 +289,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
mtx_buffer.lock();
scan_count ++;
double preprocess_start_time = omp_get_wtime();
cout<<"lidar got at: "<<msg->header.stamp.toSec()<<endl;
if (msg->header.stamp.toSec() < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
@@ -307,6 +308,7 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
{
publish_count ++;
cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
double timestamp = msg->header.stamp.toSec();
@@ -347,6 +349,11 @@ bool sync_packages(MeasureGroup &meas)
lidar_pushed = true;
}
if (abs(last_timestamp_imu - lidar_end_time) > 10.0)
{
printf("IMU and LiDAR not Synced, IMU time: %lf, lidar scan end time: %lf",last_timestamp_imu, lidar_end_time);
}
if (last_timestamp_imu < lidar_end_time)
{
return false;
@@ -432,6 +439,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
{
RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
&laserCloudWorld->points[i]);
cout<<laserCloudWorld->points[i].intensity<<endl;
}
// *pcl_wait_pub += *laserCloudWorld;