commented all main node thread to debug. sending to robot

parent 4bc450f2
......@@ -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
......
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