Commit 968200a8 authored by asantamaria's avatar asantamaria

TAG: Working as previous tag. Added function to get processing time

parent 2872b5b1
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm asctec_hl_comm asctec_msgs px_comm sensor_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm roslib asctec_hl_comm asctec_msgs px_comm sensor_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm asctec_hl_comm asctec_msgs px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm roslib asctec_hl_comm asctec_msgs px_comm sensor_msgs tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
......
......@@ -140,6 +140,14 @@ class EskfOdomAlgorithm
*/
void print_ini_params(void);
/**
* \brief Get processing time.
*
* Get filter current processing time.
*
*/
double get_proc_time(void);
/**
* \brief Set Observation phase
*
......
......@@ -39,6 +39,8 @@
#include <eigen3/Eigen/Dense>
#include <ros/package.h>
// [publisher subscriber headers]
#include <asctec_hl_comm/mav_status.h>
#include <asctec_msgs/LLStatus.h>
......@@ -51,7 +53,7 @@
using namespace std;
//#define READ_FROM_FILE
// #define READ_FROM_FILE
/**
* \brief IRI ROS Specific Algorithm Class
......@@ -134,19 +136,28 @@ private:
std::string data_path_;
// double time_fix_;
// double dt_;
double max_freq_;
// vector<bool> ll_flying_; // To store bool due to different Hz
vector <vector<double>> a_;
vector <vector<double>> w_;
vector <vector<double>> px4_;
vector <vector<double>> llstatus_;
vector <vector<double>> read_from_file(const string& path);
/**
* \brief Read all data from files
*
* This function read all sensor and robot state data from files and
* sets them into the filter.
*/
void read_all_data_from_files(void);
/**
* \brief Read data from files
*
* This function reads data from a .txt file (ROS Matlab format).
*/
vector <vector<double>> read_from_file(const string& path);
#endif
public:
......
......@@ -26,4 +26,4 @@ xstate0: [0.0,0.0,0.1,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.0005,-0.0026,-0
dxstate0: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# Read from files in path:
data_path: /home/asantamaria/catkin_ws/src/iri/kinton_robot/eskf_odom/data/kinton_outdoor_paper/2
data_path: data/kinton_outdoor_paper/2
......@@ -9,6 +9,7 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>roslib</build_depend>
<build_depend>asctec_hl_comm</build_depend>
<build_depend>asctec_msgs</build_depend>
<build_depend>px_comm</build_depend>
......@@ -17,6 +18,7 @@
<build_depend>eskf_odometry</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>roslib</run_depend>
<run_depend>asctec_hl_comm</run_depend>
<run_depend>asctec_msgs</run_depend>
<run_depend>px_comm</run_depend>
......
......@@ -42,9 +42,14 @@ void EskfOdomAlgorithm::set_imu_reading(const double& t, const Eigen::Vector3d&
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow)
{
this->filter_.set_px4_reading(t,val,flow);
this->filter_.set_px4_reading(t,val,flow);
}
double EskfOdomAlgorithm::get_proc_time(void)
{
double proc_time = this->filter_.get_proc_time();
return proc_time;
}
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state)
{
return this->filter_.run_next_step(state);
......
......@@ -28,84 +28,59 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
// [init publishers]
// [init subscribers]
this->hl_status_subscriber_ = this->public_node_handle_.subscribe("hl_status",
1, &EskfOdomAlgNode::hl_status_callback, this);
this->hl_status_subscriber_ = this->public_node_handle_.subscribe("hl_status",1, &EskfOdomAlgNode::hl_status_callback, this);
pthread_mutex_init(&this->hl_status_mutex_, NULL);
this->ll_status_subscriber_ = this->public_node_handle_.subscribe("ll_status",
1, &EskfOdomAlgNode::ll_status_callback, this);
this->ll_status_subscriber_ = this->public_node_handle_.subscribe("ll_status",1, &EskfOdomAlgNode::ll_status_callback, this);
pthread_mutex_init(&this->ll_status_mutex_, NULL);
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 10,
&EskfOdomAlgNode::px4_of_callback, this);
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 10,&EskfOdomAlgNode::px4_of_callback, this);
pthread_mutex_init(&this->px4_of_mutex_, NULL);
this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 10,
&EskfOdomAlgNode::imu_callback, this);
this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 10,&EskfOdomAlgNode::imu_callback, this);
pthread_mutex_init(&this->imu_mutex_, NULL);
#endif
#ifdef READ_FROM_FILE
read_all_data_from_files();
#endif
// [init services]
// [init clients]
// this->time_fix = 0.0;
// this->dt_=dt = 0.00999999;
// [init action servers]
// [init action clients]
}
#ifdef READ_FROM_FILE
void EskfOdomAlgNode::read_all_data_from_files(void)
{
this->imu_idx_ = 0.0;
this->px4_idx_ = 0.0;
this->llstatus_idx_ = 0.0;
this->a_ = read_from_file(this->data_path_ + "/linear_acceleration.txt");
this->w_ = read_from_file(this->data_path_ +"/angular_velocity.txt");
this->px4_ = read_from_file(this->data_path_ +"/opt_flow.txt");
this->llstatus_ = read_from_file(this->data_path_ +"/ll_status.txt");
std::string node_path = ros::package::getPath("eskf_odom");
this->a_ = read_from_file(node_path + "/" + this->data_path_ + "/linear_acceleration.txt");
this->w_ = read_from_file(node_path + "/" + this->data_path_ +"/angular_velocity.txt");
this->px4_ = read_from_file(node_path + "/" + this->data_path_ +"/opt_flow.txt");
this->llstatus_ = read_from_file(node_path + "/" + this->data_path_ +"/ll_status.txt");
this->t_ini_imu_ = this->a_[0][0];
this->t_ini_px4_ = this->px4_[0][0];
this->t_ini_llstatus_ = this->llstatus_[0][0];
// The first IMU value is not used (integration purposes)
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
Eigen::Vector3d a;
a << ax,ay,az;
Eigen::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
// Find message with max frequency to set loop rate
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);
this->max_freq_ = max(max(imu_freq,px4_freq),llstatus_freq);
this->loop_rate_ = this->max_freq_;//in [Hz]
}
#endif
// [init services]
// [init clients]
// [init action servers]
// [init action clients]
}
EskfOdomAlgNode::~EskfOdomAlgNode(void)
{
......@@ -212,7 +187,6 @@ Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,
vector <vector<double>> EskfOdomAlgNode::read_from_file(const string& path)
{
vector <vector<double>> data;
ifstream infile(path.c_str());
......@@ -257,12 +231,20 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [publish messages]
this->alg_.lock();
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
this->alg_.unlock();
Eigen::VectorXd state(19,1);
bool step_done = false;
#ifdef READ_FROM_FILE
// IMU propagation ***************
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
......@@ -277,40 +259,34 @@ void EskfOdomAlgNode::mainNodeThread(void)
this->alg_.lock();
this->alg_.set_imu_reading(t_imu,a,w);// Set values into filter object
this->alg_.prop_nominal();// Propagate Nominal-State
step_done = this->alg_.run_next_step(state); // 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 ***************
// PX4 Innovation ***************
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");
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
Eigen::Vector3d val;
val << this->gnd_dist_,vx,vy;
Eigen::Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
this->alg_.set_px4_reading(t_px4,val,flow);// Set values into filter object
this->alg_.px4_obs_error_and_up_nominal();// Error observation and Nominal-State Update
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
Eigen::Vector3d val;
val << this->gnd_dist_,vx,vy;
Eigen::Vector2d flow;
flow << flowx,flowy;
this->alg_.unlock();
this->alg_.lock();
this->alg_.set_px4_reading(t_px4,val,flow);// Set values into filter object
step_done = this->alg_.run_next_step(state); // Propagate Nominal-State
this->alg_.unlock();
++this->px4_idx_;
}
++this->px4_idx_;
// LLStatus process ****************
double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
......@@ -318,9 +294,7 @@ void EskfOdomAlgNode::mainNodeThread(void)
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_;
}
......@@ -328,18 +302,12 @@ void EskfOdomAlgNode::mainNodeThread(void)
#ifndef READ_FROM_FILE
this->alg_.lock();
// Set observation phase (Landed, toff, Flying or Landing)
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
this->alg_.lock();
step_done = this->alg_.run_next_step(state);
this->alg_.unlock();
#endif
Eigen::VectorXd state(19,1);
this->alg_.lock();
bool step_done = this->alg_.run_next_step(state);
this->alg_.unlock();
if (step_done)
{
// Broadcast state with TF
......
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