Commit 773cd26d authored by adrianamor's avatar adrianamor

Adding read from file to solve callback asyncron problems. Read from file done...

Adding read from file to solve callback asyncron problems. Read from file done until set_robot_phase
parent 9bd1f1fa
......@@ -85,6 +85,7 @@ MESSAGE(${eskf_odometry_INCLUDE_DIR})
## Declare a cpp executable
add_executable(${PROJECT_NAME} src/eskf_odom_alg.cpp src/eskf_odom_alg_node.cpp)
# add_executable(${PROJECT_NAME} src/eskf_odom.cpp)
# ********************************************************************
# Add the libraries
......
This diff is collapsed.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This diff is collapsed.
......@@ -28,6 +28,16 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "eskf_odom_alg.h"
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>
// [publisher subscriber headers]
#include <asctec_msgs/LLStatus.h>
#include <px_comm/OpticalFlow.h>
......@@ -37,13 +47,11 @@
// [action server client headers]
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
#define READ_FROM_FILE
/**
* \brief IRI ROS Specific Algorithm Class
*
......@@ -54,6 +62,8 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
#ifndef READ_FROM_FILE
bool is_first_; // First reading should not contribute to propagate nominal (integration requirements)
bool new_imu_; // Flag indicating a new IMU message is received.
......@@ -78,6 +88,8 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
void imu_mutex_enter(void);
void imu_mutex_exit(void);
#endif
// [service attributes]
// [client attributes]
......@@ -86,6 +98,23 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
// [action client attributes]
#ifdef READ_FROM_FILE
int count_;
double t_ini_imu_;
double t_ini_px4_;
double t_ini_llstatus_;
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);
#endif
public:
/**
* \brief Constructor
......
......@@ -14,7 +14,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
{
//init class attributes if necessary
// this->loop_rate_ = 100;//in [Hz]
this->loop_rate_ = 100;//in [Hz]
#ifndef READ_FROM_FILE
// Initialize vars
this->flying_ = false;
......@@ -27,6 +29,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
// [init publishers]
// [init subscribers]
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);
......@@ -36,9 +39,65 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 1, &EskfOdomAlgNode::imu_callback, this);
pthread_mutex_init(&this->imu_mutex_,NULL);
#endif
#ifdef READ_FROM_FILE
this->count_ = 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");
this->px4_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/opt_flow.txt");
this->llstatus_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/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];
// 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;
}
}
this->ll_flying_.resize(this->px4_.size());
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;
}
//________________________________
// cout << "t fly: " << t_fly << ", t land: " << t_landed << endl;
// 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();
// cout << "imu: " << imu_freq << ", px4: " << px4_freq << ", llstatus: " << llstatus_freq << endl;
#endif
// Read initial parameters from yaml file and set themn to sensors and filter
read_and_set_ini_params();
// [init services]
// [init clients]
......@@ -50,10 +109,12 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
EskfOdomAlgNode::~EskfOdomAlgNode(void)
{
#ifndef READ_FROM_FILE
// [free dynamic memory]
pthread_mutex_destroy(&this->ll_status_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
#endif
}
void EskfOdomAlgNode::read_and_set_ini_params(void)
......@@ -111,7 +172,6 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
//this->alg_.print_ini_params();
this->alg_.unlock();
}
VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long)
......@@ -135,6 +195,45 @@ VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long
return params;
}
#ifdef READ_FROM_FILE
vector <vector<double>> EskfOdomAlgNode::read_from_file(const string& path)
{
vector <vector<double>> data;
ifstream infile(path.c_str());
string s;
getline(infile,s);
while (infile)
{
if (!getline(infile,s)) break;
istringstream ss(s);
vector<double> record;
while (ss)
{
string s;
if (!getline( ss, s, ',' )) break;
record.push_back(atof(s.c_str()));
}
data.push_back(record);
}
if (!infile.eof())
{
cerr << "Error reading file " << path.c_str() << "\n";
}
return data;
}
#endif
void EskfOdomAlgNode::mainNodeThread(void)
{
// [fill msg structures]
......@@ -147,15 +246,28 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Set Quadrotor State
// this->ll_status_mutex_enter(); // protect flying var
// this->px4_of_mutex_enter(); // protect gnd_dist
#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]);
this->gnd_dist_ = this->px4_[this->count_][4];
this->flying_ = this->ll_flying_[this->count_];
// 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;
++this->count_;
#endif
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();
// this->ll_status_mutex_exit();
// this->px4_of_mutex_exit();
#ifndef READ_FROM_FILE
if (this->new_imu_)
{
......@@ -190,106 +302,113 @@ void EskfOdomAlgNode::mainNodeThread(void)
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
cout << "MAIN" << endl;
}
/* [subscriber callbacks] */
void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");
this->ll_status_mutex_enter();
this->flying_ = msg->flying;
this->ll_status_mutex_exit();
}
printf("MAIN %2.10f \n",ros::Time::now().toSec());
void EskfOdomAlgNode::ll_status_mutex_enter(void)
{
pthread_mutex_lock(&this->ll_status_mutex_);
}
#endif
void EskfOdomAlgNode::ll_status_mutex_exit(void)
{
pthread_mutex_unlock(&this->ll_status_mutex_);
}
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;
this->px4_of_mutex_exit();
Vector3d val;
val << this->gnd_dist_,vx,vy;
double t = stamp.toSec();
this->alg_.lock();
this->alg_.set_px4_reading(t,val); // Set values into filter object
this->alg_.unlock();
if (!this->is_first_) // We first want to propagate with the IMU
this->new_px4_ = true;
#ifndef READ_FROM_FILE
cout << "PX4" << endl;
/* [subscriber callbacks] */
void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");
}
this->ll_status_mutex_enter();
this->flying_ = msg->flying;
this->ll_status_mutex_exit();
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
{
pthread_mutex_lock(&this->px4_of_mutex_);
}
void EskfOdomAlgNode::ll_status_mutex_enter(void)
{
pthread_mutex_lock(&this->ll_status_mutex_);
}
void EskfOdomAlgNode::px4_of_mutex_exit(void)
{
pthread_mutex_unlock(&this->px4_of_mutex_);
}
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;
this->imu_mutex_exit();
double t = stamp.toSec();
Vector3d a;
a << ax,ay,az;
Vector3d w;
w << wx,wy,wz;
void EskfOdomAlgNode::ll_status_mutex_exit(void)
{
pthread_mutex_unlock(&this->ll_status_mutex_);
}
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();
this->alg_.lock();
this->alg_.set_px4_reading(t,val); // Set values into filter object
this->alg_.unlock();
if (!this->is_first_) // We first want to propagate with the IMU
this->new_px4_ = true;
printf("PX4 %2.10f \n",t);
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
{
pthread_mutex_lock(&this->px4_of_mutex_);
}
void EskfOdomAlgNode::px4_of_mutex_exit(void)
{
pthread_mutex_unlock(&this->px4_of_mutex_);
}
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();
Vector3d a;
a << ax,ay,az;
Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
this->alg_.set_imu_reading(t,a,w); // Set values into filter object
this->alg_.unlock();
this->alg_.lock();
this->alg_.set_imu_reading(t,a,w); // Set values into filter object
this->alg_.unlock();
this->new_imu_ = true;
this->new_imu_ = true;
printf("IMU %2.10f \n",t);
}
cout << "IMU" << endl;
}
void EskfOdomAlgNode::imu_mutex_enter(void)
{
pthread_mutex_lock(&this->imu_mutex_);
}
void EskfOdomAlgNode::imu_mutex_enter(void)
{
pthread_mutex_lock(&this->imu_mutex_);
}
void EskfOdomAlgNode::imu_mutex_exit(void)
{
pthread_mutex_unlock(&this->imu_mutex_);
}
void EskfOdomAlgNode::imu_mutex_exit(void)
{
pthread_mutex_unlock(&this->imu_mutex_);
}
#endif
/* [service callbacks] */
......
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