Commit 0347a213 authored by adrianamor's avatar adrianamor

debuged after prop_nom using read_from_file

parent 773cd26d
......@@ -100,12 +100,17 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
#ifdef READ_FROM_FILE
int count_;
int imu_idx_;
int px4_idx_;
int llstatus_idx_;
double t_ini_imu_;
double t_ini_px4_;
double t_ini_llstatus_;
vector<bool> ll_flying_; // To store bool due to different Hz
double max_freq_;
// vector<bool> ll_flying_; // To store bool due to different Hz
vector <vector<double>> a_;
vector <vector<double>> w_;
......
......@@ -13,15 +13,15 @@ using namespace Eigen;
EskfOdomAlgNode::EskfOdomAlgNode(void) :
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
{
//init class attributes if necessary
this->loop_rate_ = 100;//in [Hz]
this->flying_ = false;
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
#ifndef READ_FROM_FILE
this->loop_rate_ = 100;//in [Hz]
// Initialize vars
this->flying_ = false;
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
this->is_first_ = true;
this->is_first_ = true;
this->new_imu_ = false;
this->new_px4_ = false;
......@@ -44,7 +44,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
#ifdef READ_FROM_FILE
this->count_ = 0;
this->imu_idx_ = 0;
this->px4_idx_ = 0;
this->llstatus_idx_ = 0;
this->a_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/linear_acceleration.txt");
this->w_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/angular_velocity.txt");
......@@ -56,42 +58,37 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->t_ini_llstatus_ = this->llstatus_[0][0];
// Done to solve ll_status different frew r.t. px4 ____________TODO FIX this______
double t_fly;
double t_landed;
bool found = false;
for (int ii = 0; ii < this->llstatus_.size(); ++ii)
{
if (this->llstatus_[ii][10] == 1.0 && !found)
{
found = true;
t_fly = (this->llstatus_[ii][0]-this->t_ini_llstatus_)*1e-9;
}
if (this->llstatus_[ii][10] == 0.0 && found)
{
t_landed = (this->llstatus_[ii][0]-this->t_ini_llstatus_)*1e-9;
}
}
// The first IMU value is not used (integration purposes)
double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
this->ll_flying_.resize(this->px4_.size());
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
for (int ii = 0; ii < this->px4_.size(); ++ii)
{
double tpx4 = (this->px4_[ii][0]-t_ini_px4_)*1e-9;
if((tpx4 < t_fly) || (tpx4 > t_landed))
this->ll_flying_[ii] = false;
else
this->ll_flying_[ii] = true;
}
//________________________________
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_.unlock();
++this->imu_idx_;
// Find message with max frequency
// cout << "t fly: " << t_fly << ", t land: " << t_landed << endl;
double imu_freq = this->a_.size()/((this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9);
double px4_freq = this->px4_.size()/((this->px4_[this->px4_.size()-1][0]-this->t_ini_px4_)*1e-9);
double llstatus_freq = this->llstatus_.size()/((this->llstatus_[this->llstatus_.size()-1][0]-this->t_ini_llstatus_)*1e-9);
// double imu_freq = (this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9/this->a_.size();
// double px4_freq = (this->px4_[this->px4_.size()-1][0]-this->t_ini_px4_)*1e-9/this->px4_.size();
// double llstatus_freq = (this->llstatus_[this->llstatus_.size()-1][0]-this->t_ini_llstatus_)*1e-9/this->llstatus_.size();
this->max_freq_ = max(max(imu_freq,px4_freq),llstatus_freq);
// cout << "imu: " << imu_freq << ", px4: " << px4_freq << ", llstatus: " << llstatus_freq << endl;
this->loop_rate_ = this->max_freq_;//in [Hz]
#endif
......@@ -247,20 +244,70 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Set Quadrotor State
#ifdef READ_FROM_FILE
ROS_DEBUG("time: %f",this->a_[this->count_][0]*1e-9);
ROS_DEBUG("ax: %f, ay: %f, az: %f", this->a_[this->count_][1],this->a_[this->count_][2],this->a_[this->count_][3]);
ROS_DEBUG("wx: %f, wy: %f, wz: %f", this->w_[this->count_][1],this->w_[this->count_][2],this->w_[this->count_][3]);
ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[this->count_][4],this->px4_[this->count_][7],this->px4_[this->count_][8]);
ROS_DEBUG("flying: %f", this->llstatus_[this->count_][10]);
// ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[this->imu_idx_][4],this->px4_[this->imu_idx_][7],this->px4_[this->imu_idx_][8]);
// ROS_DEBUG("flying: %f", this->llstatus_[this->imu_idx_][10]);
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
Vector3d val;
val << this->gnd_dist_,vx,vy;
this->gnd_dist_ = this->px4_[this->count_][4];
this->alg_.lock();
this->alg_.set_px4_reading(t_px4,val); // Set values into filter object
this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
this->alg_.unlock();
++this->px4_idx_;
}
this->flying_ = this->ll_flying_[this->count_];
// LLStatus process ****************
double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
// double run_t = (this->a_[this->count_][0]-this->t_ini_imu_)*1e-9;
// cout << "Running time: " << run_t << ", g_dist: " << this->gnd_dist_ << ", flying: " << this->llstatus_[this->count_][10] << endl;
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_;
}
++this->count_;
#endif
this->alg_.lock(); // protect algorithm
......@@ -290,6 +337,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
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();
......@@ -301,11 +351,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
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"));
printf("MAIN %2.10f \n",ros::Time::now().toSec());
#endif
}
#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