Commit 6a331fb5 authored by asantamaria's avatar asantamaria

Working. trying to solve transition TOFF to FLYING

parent 442626aa
......@@ -51,6 +51,7 @@ class EskfOdomAlgorithm
CEskf filter_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
......@@ -153,6 +154,7 @@ class EskfOdomAlgorithm
* There are no specific inputs due to all are class variables.
*/
void set_obs_phase(const bool& flying, const double& gnd_dist);
// void set_obs_phase(const bool& flying);
/**
* \brief Set IMU Readings
......@@ -179,33 +181,15 @@ class EskfOdomAlgorithm
void set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow);
/**
* \brief Nominal State Propagation
* \brief Run next step
*
* Nominal State propagation using IMU model.
* Run filter next step (either propagation or update and correction)
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void prop_nominal(void);
* Input/Ouput:
* state: Robot state (6D).
*/
bool run_next_step(Eigen::VectorXd& state);
/**
* \brief Observe estimation error and Update Nominal state
*
* Observe estimation error and Update Nominal state.
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void obs_error_and_up_nominal(void);
/**
* \brief Get nominal-state vector
*
* Get nominal-state vector. Function to prevent external modifications.
*
*/
Eigen::VectorXd get_x_state(void);
/**
* \brief Destructor
*
......
......@@ -58,6 +58,7 @@ using namespace std;
*
*/
class EskfOdomAlgNode: public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm> {
private:
bool flying_; // Quadrotor landed or flying information.
......
......@@ -39,6 +39,11 @@ void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist
this->filter_.set_obs_phase(flying,gnd_dist);
}
// void EskfOdomAlgorithm::set_obs_phase(const bool& flying)
// {
// this->filter_.set_obs_phase(flying);
// }
void EskfOdomAlgorithm::set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w)
{
this->filter_.set_imu_reading(t,a,w);
......@@ -49,17 +54,7 @@ void EskfOdomAlgorithm::set_px4_reading(const double& t, const Eigen::Vector3d&
this->filter_.set_px4_reading(t,val,flow);
}
void EskfOdomAlgorithm::prop_nominal(void)
{
this->filter_.prop_nominal();
}
void EskfOdomAlgorithm::obs_error_and_up_nominal(void)
{
this->filter_.px4_obs_error_and_up_nominal();
}
VectorXd EskfOdomAlgorithm::get_x_state(void)
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state)
{
return this->filter_.get_x_state();
return this->filter_.run_next_step(state);
}
\ No newline at end of file
......@@ -172,8 +172,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->alg_.lock(); // protect algorithm
// Set filter and sensors initial parameters
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params,
px4_params);
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params, px4_params);
#ifdef READ_FROM_FILE
this->public_node_handle_.param<string>("data_path", this->data_path_, "");
......@@ -258,11 +257,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [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");
......@@ -334,66 +328,28 @@ void EskfOdomAlgNode::mainNodeThread(void)
#ifndef READ_FROM_FILE
if (this->imu_queue_.size() > 1 && this->px4_queue_.size() > 0)
{
double t_imu = this->imu_queue_.front()->header.stamp.toSec() - this->t_ini_imu_;
double t_px4 = this->px4_queue_.front()->header.stamp.toSec() - this->t_ini_px4_;
if (t_imu < t_px4)
{
// Set IMU data
set_imu_reading(this->imu_queue_.front(), t_imu);
this->imu_queue_.pop();
// first imu reading not used due to it is needed for the first nominal state integration
if (this->is_first_imu_)
{
this->is_first_imu_ = false;
//std::cout << "First IMU!" << std::endl;
// release next px4 beacuse first imu is not used.
this->px4_queue_.pop();
}
else{
//std::cout << "Process IMU with time: " << t_imu << std::endl;
// Propagate Filter
this->alg_.lock(); // protect algorithm
this->alg_.prop_nominal(); // Propagate Nominal-State
this->alg_.unlock();
}
}
else
{
//std::cout << "Process PX4 with time: " << t_px4 << std::endl;
//Set PX4 data
set_px4_reading(this->px4_queue_.front(), t_px4);
this->px4_queue_.pop();
//Filter update
this->alg_.lock(); // protect algorithm
this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
this->alg_.unlock();
}
}
this->alg_.lock();
// Set observation phase (Landed, toff, Flying or Landing)
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
this->alg_.unlock();
#endif
// Get state and publish TF **********************
this->alg_.lock(); // protect algorithm
Eigen::VectorXd state = this->alg_.get_x_state(); // get state
Eigen::VectorXd state(19,1);
this->alg_.lock();
bool step_done = this->alg_.run_next_step(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"));
if (step_done)
{
// Broadcast state with TF
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
......@@ -420,10 +376,10 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
pthread_mutex_unlock(&this->hl_status_mutex_);
}
void EskfOdomAlgNode::ll_status_callback(
const asctec_msgs::LLStatus::ConstPtr& msg)
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();
......@@ -439,8 +395,7 @@ void EskfOdomAlgNode::ll_status_mutex_exit(void)
void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
// ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
// ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
......@@ -450,7 +405,7 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
this->is_first_px4_ = false;
}
this->px4_queue_.push(msg);
set_px4_reading(msg, t-this->t_ini_px4_);
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
{
......@@ -463,16 +418,18 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
// ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
if (this->is_first_imu_)
{
this->t_ini_imu_ = t;
//this->is_first_imu_ = false;
this->is_first_imu_ = false;
}
this->imu_queue_.push(msg);
// this->imu_queue_.push(msg);
set_imu_reading(msg, t-this->t_ini_imu_);
}
void EskfOdomAlgNode::imu_mutex_enter(void)
{
......@@ -483,8 +440,7 @@ void EskfOdomAlgNode::imu_mutex_exit(void)
pthread_mutex_unlock(&this->imu_mutex_);
}
void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
const double& msg_time)
void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const double& msg_time)
{
// Get sensor values
this->imu_mutex_enter();
......@@ -506,8 +462,7 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
this->alg_.unlock();
}
void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
const double& msg_time)
void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,const double& msg_time)
{
// Get sensor values
this->px4_of_mutex_enter();
......
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