lidar_IMU_calib
lidar_IMU_calib copied to clipboard
Published
20 hours ago •
APRIL-ZJU
APRIL-ZJU
a bug need to fix
bool read(const std::string path,
const std::string imu_topic,
const std::string lidar_topic,
const double bag_start = -1.0,
const double bag_durr = -1.0) {
data_.reset(new LioDataset(lidar_model_));
data_->bag_.reset(new rosbag::Bag);
data_->bag_->open(path, rosbag::bagmode::Read);
init();
rosbag::View view;
{
std::vector<std::string> topics;
topics.push_back(imu_topic);
topics.push_back(lidar_topic);
rosbag::View view_full;
view_full.addQuery(*data_->bag_);
ros::Time time_init = view_full.getBeginTime();
time_init += ros::Duration(bag_start);
ros::Time time_finish = (bag_durr < 0)?
view_full.getEndTime() : time_init + ros::Duration(bag_durr);
view.addQuery(*data_->bag_, rosbag::TopicQuery(topics), time_init, time_finish);
}
for (rosbag::MessageInstance const m : view) {
const std::string &topic = m.getTopic();
if (lidar_topic == topic) {
TPointCloud pointCloudPtr;
double timestamp = 0;
std::cout << "find lidar topic data type " << m.getDataType() << std::endl;
if (m.getDataType() == std::string("velodyne_msgs/VelodyneScan")) {
velodyne_msgs::VelodyneScan::ConstPtr vlp_msg =
m.instantiate<velodyne_msgs::VelodyneScan>();
timestamp = vlp_msg->header.stamp.toSec();
p_VelodyneLidarConvert_->unpack_scan(vlp_msg, pointCloudPtr);
std::cout << "velodyne_msgs/VelodyneScan" << std::endl;
}
if (m.getDataType() == std::string("sensor_msgs/PointCloud2")) {
sensor_msgs::PointCloud2::ConstPtr scan_msg =
m.instantiate<sensor_msgs::PointCloud2>();
timestamp = scan_msg->header.stamp.toSec();
p_VelodyneLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);
std::cout << "add sensor_msgs/PointCloud2" << std::endl;
}
if(m.getDataType() == std::string("livox_ros_driver/CustomMsg")) {
li_calib::LivoxPointCloud ::ConstPtr scan_msg =
m.instantiate<li_calib::LivoxPointCloud>();
timestamp = scan_msg->header.stamp.toSec();
p_LivoxLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);
std::cout << "scan_msg header stamp: "<< scan_msg->header.stamp << std::endl;
std::cout << "scan msg header frame id: " << scan_msg->header.frame_id << std::endl;
std::cout << "scan msg point num: " << scan_msg->point_num << std::endl;
std::cout << "scan msg point size: " << scan_msg->points.size() << std::endl;
std::cout << "add livox_ros_driver/CustomMsg" << std::endl;
}
data_->scan_data_.emplace_back(pointCloudPtr);
data_->scan_timestamps_.emplace_back(timestamp);
}
if (imu_topic == topic) {
sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
double time = imu_msg->header.stamp.toSec();
data_->imu_data_.emplace_back();
data_->imu_data_.back().timestamp = time;
data_->imu_data_.back().accel = Eigen::Vector3d(
imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,
imu_msg->linear_acceleration.z);
data_->imu_data_.back().gyro = Eigen::Vector3d(
imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,
imu_msg->angular_velocity.z);
}
}
std::cout << lidar_topic << ": " << data_->scan_data_.size() << std::endl;
std::cout << imu_topic << ": " << data_->imu_data_.size() << std::endl;
return true;
}
this function must return.
bool read(const std::string path, const std::string imu_topic, const std::string lidar_topic, const double bag_start = -1.0, const double bag_durr = -1.0) { data_.reset(new LioDataset(lidar_model_)); data_->bag_.reset(new rosbag::Bag); data_->bag_->open(path, rosbag::bagmode::Read); init(); rosbag::View view; { std::vector<std::string> topics; topics.push_back(imu_topic); topics.push_back(lidar_topic); rosbag::View view_full; view_full.addQuery(*data_->bag_); ros::Time time_init = view_full.getBeginTime(); time_init += ros::Duration(bag_start); ros::Time time_finish = (bag_durr < 0)? view_full.getEndTime() : time_init + ros::Duration(bag_durr); view.addQuery(*data_->bag_, rosbag::TopicQuery(topics), time_init, time_finish); } for (rosbag::MessageInstance const m : view) { const std::string &topic = m.getTopic(); if (lidar_topic == topic) { TPointCloud pointCloudPtr; double timestamp = 0; std::cout << "find lidar topic data type " << m.getDataType() << std::endl; if (m.getDataType() == std::string("velodyne_msgs/VelodyneScan")) { velodyne_msgs::VelodyneScan::ConstPtr vlp_msg = m.instantiate<velodyne_msgs::VelodyneScan>(); timestamp = vlp_msg->header.stamp.toSec(); p_VelodyneLidarConvert_->unpack_scan(vlp_msg, pointCloudPtr); std::cout << "velodyne_msgs/VelodyneScan" << std::endl; } if (m.getDataType() == std::string("sensor_msgs/PointCloud2")) { sensor_msgs::PointCloud2::ConstPtr scan_msg = m.instantiate<sensor_msgs::PointCloud2>(); timestamp = scan_msg->header.stamp.toSec(); p_VelodyneLidarConvert_->unpack_scan(scan_msg, pointCloudPtr); std::cout << "add sensor_msgs/PointCloud2" << std::endl; } if(m.getDataType() == std::string("livox_ros_driver/CustomMsg")) { li_calib::LivoxPointCloud ::ConstPtr scan_msg = m.instantiate<li_calib::LivoxPointCloud>(); timestamp = scan_msg->header.stamp.toSec(); p_LivoxLidarConvert_->unpack_scan(scan_msg, pointCloudPtr); std::cout << "scan_msg header stamp: "<< scan_msg->header.stamp << std::endl; std::cout << "scan msg header frame id: " << scan_msg->header.frame_id << std::endl; std::cout << "scan msg point num: " << scan_msg->point_num << std::endl; std::cout << "scan msg point size: " << scan_msg->points.size() << std::endl; std::cout << "add livox_ros_driver/CustomMsg" << std::endl; } data_->scan_data_.emplace_back(pointCloudPtr); data_->scan_timestamps_.emplace_back(timestamp); } if (imu_topic == topic) { sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>(); double time = imu_msg->header.stamp.toSec(); data_->imu_data_.emplace_back(); data_->imu_data_.back().timestamp = time; data_->imu_data_.back().accel = Eigen::Vector3d( imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z); data_->imu_data_.back().gyro = Eigen::Vector3d( imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); } } std::cout << lidar_topic << ": " << data_->scan_data_.size() << std::endl; std::cout << imu_topic << ": " << data_->imu_data_.size() << std::endl; return true; }this function must return.
请问下，我用ubuntu16.04编译运行没有问题，用ubuntu20.04编译运行会报错，加了return true之后就好了，请问这是什么原因呢？
我这边也是，开始的时候会出现内存报错的问题，后面是定位到这个函数，然后加return就好了
好的，谢谢老哥，我用debug模式运行的时候提示的是boost指针没有初始化，可是我看里面的boost指针明明都初始化了，很想再问下这个加return 跟boost指针有啥关系呢？ 你当时是怎么定位到用return就可以解决的呢？
我这边也是，开始的时候会出现内存报错的问题，后面是定位到这个函数，然后加return就好了
好的，谢谢老哥，我用debug模式运行的时候提示的是boost指针没有初始化，可是我看里面的boost指针明明都初始化了，很想再问下这个加return 跟boost指针有啥关系呢？ 你当时是怎么定位到用return就可以解决的呢？
不知道你这个目前使用的怎么样啦，貌似作者没有维护了，目前提了这么多issue没人维护了
我用的clion的内存调试工具叫Valgrind,会显示这个函数有问题
有个题外的问题想请教一下。clion里面的valgrind怎么调试roslaunch呢
