Commented callbacks to test into the robot

include/eskf_odom_alg_node.h

@@ -51,7 +51,7 @@
 using namespace std;
 using namespace Eigen;
 
-#define READ_FROM_FILE
+//#define READ_FROM_FILE
 
 /**
  * \brief IRI ROS Specific Algorithm Class

src/eskf_odom_alg_node.cpp

@@ -400,42 +400,44 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
   }
   void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg) 
   { 
-    ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received"); 
-
-    // Get sensor values
-    this->px4_of_mutex_enter(); 
-    this->gnd_dist_ = msg->ground_distance;
-    double vx = msg->velocity_x;
-    double vy = -msg->velocity_y; // axis change according to simulation
-    // ros::Time stamp = msg->header.stamp;
-    ros::Time stamp = ros::Time::now();
-    this->px4_of_mutex_exit(); 
-
-    Vector3d val;
-    val << this->gnd_dist_,vx,vy;
-    double t = stamp.toSec();
-
-    if (this->is_first_px4_){
-      this->t_ini_px4_= t;
-      this->is_first_px4_ = false;
-    }
-
-    double curr_t = t-this->t_ini_px4_;
-
-    double flowx = msg->flow_x;
-    double flowy = -msg->flow_y; // axis change according to simulation
-
-    Vector2d flow;
-    flow << flowx,flowy;
-
-    this->alg_.lock(); 
-    this->alg_.set_px4_reading(curr_t,val,flow); // Set values into filter object
-    this->alg_.unlock();
-
-    if (!this->is_first_imu_) // We first want to propagate with the IMU
-      this->new_px4_ = true;
-
-    // printf("PX4 %2.10f \n",t); 
+	  std::cout << "<<< PX4 at: " << msg->header.stamp.toSec()*1e-9 <<"\n";
+
+//    ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
+//
+//    // Get sensor values
+//    this->px4_of_mutex_enter();
+//    this->gnd_dist_ = msg->ground_distance;
+//    double vx = msg->velocity_x;
+//    double vy = -msg->velocity_y; // axis change according to simulation
+//    // ros::Time stamp = msg->header.stamp;
+//    ros::Time stamp = ros::Time::now();
+//    this->px4_of_mutex_exit();
+//
+//    Vector3d val;
+//    val << this->gnd_dist_,vx,vy;
+//    double t = stamp.toSec();
+//
+//    if (this->is_first_px4_){
+//      this->t_ini_px4_= t;
+//      this->is_first_px4_ = false;
+//    }
+//
+//    double curr_t = t-this->t_ini_px4_;
+//
+//    double flowx = msg->flow_x;
+//    double flowy = -msg->flow_y; // axis change according to simulation
+//
+//    Vector2d flow;
+//    flow << flowx,flowy;
+//
+//    this->alg_.lock();
+//    this->alg_.set_px4_reading(curr_t,val,flow); // Set values into filter object
+//    this->alg_.unlock();
+//
+//    if (!this->is_first_imu_) // We first want to propagate with the IMU
+//      this->new_px4_ = true;
+//
+//    // printf("PX4 %2.10f \n",t);
 
   } 
 
@@ -450,39 +452,41 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
   }
   void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg) 
   { 
-    ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received"); 
-
-    // Get sensor values
-    this->imu_mutex_enter(); 
-    double ax = msg->linear_acceleration.x;
-    double ay = -msg->linear_acceleration.y; // axis change according to simulation
-    double az = -msg->linear_acceleration.z; // axis change according to simulation
-    double wx = msg->angular_velocity.x;
-    double wy = -msg->angular_velocity.y; // axis change according to simulation
-    double wz = -msg->angular_velocity.z;// axis change according to simulation
-    // ros::Time stamp = msg->header.stamp;
-    ros::Time stamp = ros::Time::now();
-    this->imu_mutex_exit(); 
-
-    double t = stamp.toSec();
-
-    if (this->is_first_imu_)
-      this->t_ini_imu_= t;
-
-    double curr_t = t-this->t_ini_imu_;
-
-    Vector3d a;
-    a << ax,ay,az;
-    Vector3d w;
-    w << wx,wy,wz;
-
-    this->alg_.lock();
-    this->alg_.set_imu_reading(curr_t,a,w); // Set values into filter object
-    this->alg_.unlock(); 
-
-    this->new_imu_ = true;
-
-    // printf("IMU %2.10f \n",t); 
+	std::cout << ">>> IMU at: " << msg->header.stamp.toSec()*1e-9 <<"\n";
+
+//    ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
+//
+//    // Get sensor values
+//    this->imu_mutex_enter();
+//    double ax = msg->linear_acceleration.x;
+//    double ay = -msg->linear_acceleration.y; // axis change according to simulation
+//    double az = -msg->linear_acceleration.z; // axis change according to simulation
+//    double wx = msg->angular_velocity.x;
+//    double wy = -msg->angular_velocity.y; // axis change according to simulation
+//    double wz = -msg->angular_velocity.z;// axis change according to simulation
+//    // ros::Time stamp = msg->header.stamp;
+//    ros::Time stamp = ros::Time::now();
+//    this->imu_mutex_exit();
+//
+//    double t = stamp.toSec();
+//
+//    if (this->is_first_imu_)
+//      this->t_ini_imu_= t;
+//
+//    double curr_t = t-this->t_ini_imu_;
+//
+//    Vector3d a;
+//    a << ax,ay,az;
+//    Vector3d w;
+//    w << wx,wy,wz;
+//
+//    this->alg_.lock();
+//    this->alg_.set_imu_reading(curr_t,a,w); // Set values into filter object
+//    this->alg_.unlock();
+//
+//    this->new_imu_ = true;
+//
+//    // printf("IMU %2.10f \n",t);
   } 
 
   void EskfOdomAlgNode::imu_mutex_enter(void)