TAG: working with icra15_paper bags and read_from_files

src/eskf_odom_alg_node.cpp

@@ -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);