Commit e72422e1 authored by asantamaria's avatar asantamaria

Added new range and flow sensors. not working yet. example needs to be fixed

parent 585bd696
......@@ -128,7 +128,7 @@ class EskfOdomAlgorithm
* nominal-state vector and error-state vector. Wrapper of the low-level library
*
*/
void set_init_params(const params& f_params, const Eigen::VectorXd& x0, const Eigen::VectorXd& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4);
void set_init_params(const params& f_params, const Eigen::VectorXd& x0, const Eigen::VectorXd& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d);
/**
* \brief Print initial parameters
......@@ -187,6 +187,28 @@ class EskfOdomAlgorithm
*/
void set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow);
/**
* \brief Set Range Readings
*
* Store new Range readings
*
* Input:
* t: Time stamp.
* val: Range reading.
*/
void set_range_reading(const double& t, const double& val);
/**
* \brief Set Flow2d Readings
*
* Store new Flow2d readings
*
* Input:
* t: Time stamp.
* val: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_flow2d_reading(const double& t, const Eigen::Vector2d& val);
/**
* \brief Run next step
*
......
......@@ -23,6 +23,17 @@ px4_std_landed: [0.1,0.001,0.001]
px4_std_below30cm: [10.0,10.0,10.0]
px4_std_30cmto5m: [0.01,0.15,0.15]
# Range STD values
range_std_landed: 0.1
range_std_outsidebounds: 10.0
range_std_insidebounds: 0.01
# Flow2d STD values
flow2d_std_landed: [0.001,0.001]
flow2d_std_outsidebounds: [10.0,10.0]
flow2d_std_insidebounds: [0.15,0.15]
flow2d_focal_length: 0.016
# Initial Nominal-State vector (p,v,q,ab,wb,g)
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.0004,0.0,0.0,-9.803057]
......
......@@ -20,9 +20,10 @@ void EskfOdomAlgorithm::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
void EskfOdomAlgorithm::set_init_params(const params& f_params, const Eigen::VectorXd& x0, const Eigen::VectorXd& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4)
void EskfOdomAlgorithm::set_init_params(const params& f_params, const Eigen::VectorXd& x0, const Eigen::VectorXd& dx0, const Sensor::imu_params& imu,const Sensor::px4_params& px4,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d)
{
this->filter_.set_init_params(f_params,x0,dx0,imu,px4);
// this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range);
this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range,flow2d);
}
void EskfOdomAlgorithm::print_ini_params(void)
......@@ -45,6 +46,16 @@ void EskfOdomAlgorithm::set_px4_reading(const double& t, const Eigen::Vector3d&
this->filter_.set_px4_reading(t,val,flow);
}
void EskfOdomAlgorithm::set_range_reading(const double& t, const double& val)
{
this->filter_.set_range_reading(t,val);
}
void EskfOdomAlgorithm::set_flow2d_reading(const double& t, const Eigen::Vector2d& val)
{
this->filter_.set_flow2d_reading(t,val);
}
double EskfOdomAlgorithm::get_proc_time(void)
{
double proc_time = this->filter_.get_proc_time();
......
......@@ -99,6 +99,21 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
px4_params.std_30cmto5m = read_vec("px4_std_30cmto5m", 3);
px4_params.std = px4_params.std_landed; // Considering initially landed
// Range STD values
Sensor::range_params range_params;
this->public_node_handle_.param<double>("range_std_landed", range_params.std_landed, 0.1);
this->public_node_handle_.param<double>("range_std_outsidebounds", range_params.std_outsidebounds, 10.0);
this->public_node_handle_.param<double>("range_std_insidebounds", range_params.std_insidebounds, 0.01);
range_params.std = range_params.std_landed; // Considering initially landed
// Flow2d STD values
Sensor::flow2d_params flow2d_params;
flow2d_params.std_landed = read_vec("flow2d_std_landed", 2);
flow2d_params.std_outsidebounds = read_vec("flow2d_std_outsidebounds", 2);
flow2d_params.std_insidebounds = read_vec("flow2d_std_insidebounds", 2);
flow2d_params.std = flow2d_params.std_landed; // Considering initially landed
this->public_node_handle_.param<double>("flow2d_focal_length", flow2d_params.focal_length, 0.016);
// Initialize Nominal-State vector
Eigen::VectorXd x_state = read_vec("xstate0", 19);
......@@ -108,7 +123,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, range_params, flow2d_params);
// Print already set values of filter and sensors initial parameters
//this->alg_.print_ini_params();
......@@ -200,6 +215,8 @@ void EskfOdomAlgNode::mainNodeThread(void)
/* [subscriber callbacks] */
//TODO: Add a range subscriber
void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
......@@ -287,8 +304,10 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
flow << flowx, flowy;
this->alg_.lock();
this->alg_.set_px4_reading(msg_time, val, flow); // Set values into filter object
this->alg_.unlock();
this->alg_.set_px4_reading(msg_time, val, flow); // Set values into filter object
// this->alg_.set_flow2d_reading(msg_time, flow); // Set values into filter object
// this->alg_.set_range_reading(msg_time, this->gnd_dist_); // Set values into filter object
this->alg_.unlock();
}
/* [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