commented all main node thread to debug. sending to robot

src/eskf_odom_alg_node.cpp

@@ -235,125 +235,125 @@ VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long
 
 void EskfOdomAlgNode::mainNodeThread(void)
 {
-  // [fill msg structures]
-  
-  // [fill srv structure and make request to the server]
-  
-  // [fill action structure and make request to the action server]
-
-  // [publish messages]
-
-  // Set Quadrotor State
-  this->alg_.lock();  // protect algorithm
-  this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
-  this->alg_.unlock(); 
-
-#ifdef READ_FROM_FILE
-
-    ROS_DEBUG("IMU process");  
-
-    double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
-
-    double ax = this->a_[this->imu_idx_][1];
-    double ay = -this->a_[this->imu_idx_][2]; // axis change according to simulation
-    double az = -this->a_[this->imu_idx_][3]; // axis change according to simulation
-    double wx = this->w_[this->imu_idx_][1];
-    double wy = -this->w_[this->imu_idx_][2]; // axis change according to simulation
-    double wz = -this->w_[this->imu_idx_][3];// axis change according to simulation
-
-    Vector3d a;
-    a << ax,ay,az;
-    Vector3d w;
-    w << wx,wy,wz;
-
-    this->alg_.lock();
-    this->alg_.set_imu_reading(t_imu,a,w); // Set values into filter object
-    this->alg_.prop_nominal(); // Propagate Nominal-State 
-    this->alg_.unlock(); 
-
-    ++this->imu_idx_;
-
-    double next_t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
-
-    // PX4 process ***************
-    double t_px4 = (this->px4_[this->px4_idx_][0]-t_ini_px4_)*1e-9;
-
-    if (this->px4_idx_ < this->px4_.size() && t_px4 < next_t_imu)
-    {
-      ROS_DEBUG("process PX4");
-
-      this->gnd_dist_ = this->px4_[this->px4_idx_][4];
-      double vx = this->px4_[this->px4_idx_][7];
-      double vy = -this->px4_[this->px4_idx_][8]; // axis change according to simulation
-      double flowx = this->px4_[this->px4_idx_][5];
-      double flowy = -this->px4_[this->px4_idx_][6]; // axis change according to simulation
-
-      Vector3d val;
-      val << this->gnd_dist_,vx,vy;
-
-      Vector2d flow;
-      flow << flowx,flowy;
-
-      this->alg_.lock(); 
-      this->alg_.set_px4_reading(t_px4,val,flow); // Set values into filter object
-      this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
-  
-      this->alg_.unlock();
-
-      ++this->px4_idx_;
-    }
-
-    // LLStatus process ****************
-    double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
-
-    if (this->llstatus_idx_ < this->llstatus_.size() && t_llstatus < next_t_imu)
-    {
-      ROS_DEBUG("process ll_status");
-
-      this->flying_ = this->llstatus_[this->llstatus_idx_][10];
-
-      ++this->llstatus_idx_;
-    }
-
-#endif
-
-#ifndef READ_FROM_FILE 
-
-  if (this->new_imu_)
-  {   
-    this->new_imu_ = false;
-    if (this->is_first_imu_)
-      this->is_first_imu_ = false;
-    else 
-    {
-      this->alg_.lock();  // protect algorithm
-      this->alg_.prop_nominal(); // Propagate Nominal-State 
-      this->alg_.unlock(); 
-    }
-  }
-
-  if (this->new_px4_)
-  {
-    this->new_px4_ = false;
-    this->alg_.lock();  // protect algorithm
-    this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
-    this->alg_.unlock(); 
-  }
-
-#endif
-
-  // Get state and publish TF **********************
-  this->alg_.lock();  // protect algorithm
-  VectorXd state = this->alg_.get_x_state(); // get state
-  this->alg_.unlock(); 
-
-  // Broadcast TF with state
-  static tf::TransformBroadcaster br;
-  tf::Transform transform;
-  transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
-  tf::Quaternion q(state(7),state(8),state(9),state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
-  transform.setRotation(q);
-  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
+//  // [fill msg structures]
+//
+//  // [fill srv structure and make request to the server]
+//
+//  // [fill action structure and make request to the action server]
+//
+//  // [publish messages]
+//
+//  // Set Quadrotor State
+//  this->alg_.lock();  // protect algorithm
+//  this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
+//  this->alg_.unlock();
+//
+//#ifdef READ_FROM_FILE
+//
+//    ROS_DEBUG("IMU process");
+//
+//    double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
+//
+//    double ax = this->a_[this->imu_idx_][1];
+//    double ay = -this->a_[this->imu_idx_][2]; // axis change according to simulation
+//    double az = -this->a_[this->imu_idx_][3]; // axis change according to simulation
+//    double wx = this->w_[this->imu_idx_][1];
+//    double wy = -this->w_[this->imu_idx_][2]; // axis change according to simulation
+//    double wz = -this->w_[this->imu_idx_][3];// axis change according to simulation
+//
+//    Vector3d a;
+//    a << ax,ay,az;
+//    Vector3d w;
+//    w << wx,wy,wz;
+//
+//    this->alg_.lock();
+//    this->alg_.set_imu_reading(t_imu,a,w); // Set values into filter object
+//    this->alg_.prop_nominal(); // Propagate Nominal-State
+//    this->alg_.unlock();
+//
+//    ++this->imu_idx_;
+//
+//    double next_t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
+//
+//    // PX4 process ***************
+//    double t_px4 = (this->px4_[this->px4_idx_][0]-t_ini_px4_)*1e-9;
+//
+//    if (this->px4_idx_ < this->px4_.size() && t_px4 < next_t_imu)
+//    {
+//      ROS_DEBUG("process PX4");
+//
+//      this->gnd_dist_ = this->px4_[this->px4_idx_][4];
+//      double vx = this->px4_[this->px4_idx_][7];
+//      double vy = -this->px4_[this->px4_idx_][8]; // axis change according to simulation
+//      double flowx = this->px4_[this->px4_idx_][5];
+//      double flowy = -this->px4_[this->px4_idx_][6]; // axis change according to simulation
+//
+//      Vector3d val;
+//      val << this->gnd_dist_,vx,vy;
+//
+//      Vector2d flow;
+//      flow << flowx,flowy;
+//
+//      this->alg_.lock();
+//      this->alg_.set_px4_reading(t_px4,val,flow); // Set values into filter object
+//      this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
+//
+//      this->alg_.unlock();
+//
+//      ++this->px4_idx_;
+//    }
+//
+//    // LLStatus process ****************
+//    double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
+//
+//    if (this->llstatus_idx_ < this->llstatus_.size() && t_llstatus < next_t_imu)
+//    {
+//      ROS_DEBUG("process ll_status");
+//
+//      this->flying_ = this->llstatus_[this->llstatus_idx_][10];
+//
+//      ++this->llstatus_idx_;
+//    }
+//
+//#endif
+//
+//#ifndef READ_FROM_FILE
+//
+//  if (this->new_imu_)
+//  {
+//    this->new_imu_ = false;
+//    if (this->is_first_imu_)
+//      this->is_first_imu_ = false;
+//    else
+//    {
+//      this->alg_.lock();  // protect algorithm
+//      this->alg_.prop_nominal(); // Propagate Nominal-State
+//      this->alg_.unlock();
+//    }
+//  }
+//
+//  if (this->new_px4_)
+//  {
+//    this->new_px4_ = false;
+//    this->alg_.lock();  // protect algorithm
+//    this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
+//    this->alg_.unlock();
+//  }
+//
+//#endif
+//
+//  // Get state and publish TF **********************
+//  this->alg_.lock();  // protect algorithm
+//  VectorXd state = this->alg_.get_x_state(); // get state
+//  this->alg_.unlock();
+//
+//  // Broadcast TF with state
+//  static tf::TransformBroadcaster br;
+//  tf::Transform transform;
+//  transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
+//  tf::Quaternion q(state(7),state(8),state(9),state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
+//  transform.setRotation(q);
+//  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
 }
 
 #ifndef READ_FROM_FILE