TAG: working with icra15_paper bags and read_from_files

parent 03cace2a
......@@ -334,7 +334,7 @@ void EskfOdomAlgNode::mainNodeThread(void)
#ifndef READ_FROM_FILE
if (this->imu_queue_.size() > 0 && this->px4_queue_.size() > 0)
if (this->imu_queue_.size() > 1 && this->px4_queue_.size() > 0)
{
double t_imu = this->imu_queue_.front()->header.stamp.toSec() - this->t_ini_imu_;
......@@ -342,8 +342,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
if (t_imu < t_px4)
{
//std::cout << "Process IMU with time: " << t_imu << std::endl;
// Set IMU data
set_imu_reading(this->imu_queue_.front(), t_imu);
this->imu_queue_.pop();
......@@ -352,9 +350,15 @@ void EskfOdomAlgNode::mainNodeThread(void)
if (this->is_first_imu_)
{
this->is_first_imu_ = false;
std::cout << "First IMU!" << std::endl;
//std::cout << "First IMU!" << std::endl;
// release next px4 beacuse first imu is not used.
this->px4_queue_.pop();
}
else{
//std::cout << "Process IMU with time: " << t_imu << std::endl;
// Propagate Filter
this->alg_.lock(); // protect algorithm
this->alg_.prop_nominal(); // Propagate Nominal-State
......@@ -363,7 +367,7 @@ void EskfOdomAlgNode::mainNodeThread(void)
}
else
{
//std::cout << "Process PX4 with time: " << t_px4 << std::endl;
std::cout << "Process PX4 with time: " << t_px4 << std::endl;
//Set PX4 data
set_px4_reading(this->px4_queue_.front(), t_px4);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment