Skip to content

GitLab

eskf_odometry_ros
eskf_odometry_ros
Commit 5aefc037 authored by asantamaria's avatar asantamaria
Browse files
Options

Moved all to floats.

parent e72422e1
Hide whitespace changes
Inline Side-by-side
Showing with 57 additions and 51 deletions
include/eskf_odom_alg.h
View file @ 5aefc037
......@@ -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,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d);
void set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& 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
......@@ -145,7 +145,7 @@ class EskfOdomAlgorithm
* Get filter current processing time.
*
*/
double get_proc_time(void);
float get_proc_time(void);
/**
* \brief Set Observation phase
......@@ -160,7 +160,7 @@ class EskfOdomAlgorithm
* Inputs:
* 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, const float& gnd_dist);
// void set_obs_phase(const bool& flying);
/**
......@@ -173,7 +173,7 @@ class EskfOdomAlgorithm
* a: Acc. readings (m/s^2). a = [ax,ay,az].
* w: Gyro. readings (rad/s). w = [wx,wy,wz].
*/
void set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w);
void set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w);
/**
* \brief Set PX4 Optical Flow Readings
......@@ -185,7 +185,7 @@ class EskfOdomAlgorithm
* val: Sensor readings: val = [pz;vx;vy].
* flow: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow);
void set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow);
/**
* \brief Set Range Readings
......@@ -196,7 +196,7 @@ class EskfOdomAlgorithm
* t: Time stamp.
* val: Range reading.
*/
void set_range_reading(const double& t, const double& val);
void set_range_reading(const float& t, const float& val);
/**
* \brief Set Flow2d Readings
......@@ -207,7 +207,7 @@ class EskfOdomAlgorithm
* t: Time stamp.
* val: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_flow2d_reading(const double& t, const Eigen::Vector2d& val);
void set_flow2d_reading(const float& t, const Eigen::Vector2f& val);
/**
* \brief Run next step
......@@ -218,7 +218,7 @@ class EskfOdomAlgorithm
* state: Robot state (6D).
* ang_vel: Robot Angular Velocity (currently not included in the state).
*/
bool run_next_step(Eigen::VectorXd& state, Eigen::Vector3d& ang_vel);
bool run_next_step(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel);
/**
* \brief Destructor
......
include/eskf_odom_alg_node.h
View file @ 5aefc037
......@@ -53,7 +53,7 @@ class EskfOdomAlgNode: public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm
private:
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
float gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
bool is_first_imu_; // First IMU reading should not contribute to propagate nominal (integration requirements). Also used to get initial sensor time.
bool is_first_px4_; // Used to get initial sensor time.
......@@ -78,14 +78,14 @@ private:
pthread_mutex_t px4_of_mutex_;
void px4_of_mutex_enter(void);
void px4_of_mutex_exit(void);
void set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,const double& msg_time);
void set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,const float& msg_time);
ros::Subscriber imu_subscriber_;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
pthread_mutex_t imu_mutex_;
void imu_mutex_enter(void);
void imu_mutex_exit(void);
void set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const double& msg_time);
void set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const float& msg_time);
// [service attributes]
ros::ServiceServer flying_server_;
......@@ -173,7 +173,7 @@ protected:
*
* This function reads a 3-Vector parameter specified by param_name
*/
Eigen::VectorXd read_vec(const std::string& param_name, const int& exp_long);
Eigen::VectorXf read_vec(const std::string& param_name, const int& exp_long);
};
#endif
src/eskf_odom_alg.cpp
View file @ 5aefc037
......@@ -20,9 +20,8 @@ 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,const Sensor::range_params& range,const Sensor::flow2d_params& flow2d)
void EskfOdomAlgorithm::set_init_params(const params& f_params, const Eigen::VectorXf& x0, const Eigen::VectorXf& 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,range);
this->filter_.set_init_params(f_params,x0,dx0,imu,px4,range,flow2d);
}
......@@ -31,37 +30,36 @@ void EskfOdomAlgorithm::print_ini_params(void)
this->filter_.print_ini_params();
}
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist)
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const float& gnd_dist)
{
this->filter_.set_obs_phase(flying,gnd_dist);
}
void EskfOdomAlgorithm::set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w)
void EskfOdomAlgorithm::set_imu_reading(const float& t, const Eigen::Vector3f& a, const Eigen::Vector3f& w)
{
this->filter_.set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow)
void EskfOdomAlgorithm::set_px4_reading(const float& t, const Eigen::Vector3f& val, const Eigen::Vector2f& flow)
{
this->filter_.set_px4_reading(t,val,flow);
}
void EskfOdomAlgorithm::set_range_reading(const double& t, const double& val)
void EskfOdomAlgorithm::set_range_reading(const float& t, const float& val)
{
this->filter_.set_range_reading(t,val);
}
void EskfOdomAlgorithm::set_flow2d_reading(const double& t, const Eigen::Vector2d& val)
void EskfOdomAlgorithm::set_flow2d_reading(const float& t, const Eigen::Vector2f& val)
{
this->filter_.set_flow2d_reading(t,val);
}
double EskfOdomAlgorithm::get_proc_time(void)
float EskfOdomAlgorithm::get_proc_time(void)
{
double proc_time = this->filter_.get_proc_time();
return proc_time;
return this->filter_.get_proc_time();
}
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state, Eigen::Vector3d& ang_vel)
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel)
{
return this->filter_.run_next_step(state,ang_vel);
}
\ No newline at end of file
src/eskf_odom_alg_node.cpp
View file @ 5aefc037
......@@ -101,9 +101,13 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
// 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);
double std_landed, std_outsidebounds, std_insidebounds;
this->public_node_handle_.param<double>("range_std_landed", std_landed, 0.1);
this->public_node_handle_.param<double>("range_std_outsidebounds", std_outsidebounds, 10.0);
this->public_node_handle_.param<double>("range_std_insidebounds", std_insidebounds, 0.01);
range_params.std_landed = static_cast<float>(std_landed);
range_params.std_outsidebounds = static_cast<float>(std_outsidebounds);
range_params.std_insidebounds = static_cast<float>(std_insidebounds);
range_params.std = range_params.std_landed; // Considering initially landed
// Flow2d STD values
......@@ -112,13 +116,15 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
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);
double focal_length;
this->public_node_handle_.param<double>("flow2d_focal_length", focal_length, 0.016);
flow2d_params.focal_length = static_cast<float>(focal_length);
// Initialize Nominal-State vector
Eigen::VectorXd x_state = read_vec("xstate0", 19);
Eigen::VectorXf x_state = read_vec("xstate0", 19);
// Initialize Error-State vector
Eigen::VectorXd dx_state = read_vec("dxstate0", 18);
Eigen::VectorXf dx_state = read_vec("dxstate0", 18);
this->alg_.lock(); // protect algorithm
......@@ -131,7 +137,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->alg_.unlock();
}
Eigen::VectorXd EskfOdomAlgNode::read_vec(const std::string& param_name,const int& exp_long)
Eigen::VectorXf EskfOdomAlgNode::read_vec(const std::string& param_name,const int& exp_long)
{
Eigen::VectorXd params = Eigen::VectorXd::Zero(exp_long);
......@@ -151,7 +157,9 @@ Eigen::VectorXd EskfOdomAlgNode::read_vec(const std::string& param_name,const in
}
else
std::cout << "ERROR loading " << param_name << ": Wrong element type" << std::endl;
return params;
Eigen::VectorXf paramsf = params.cast<float>();
return paramsf;
}
void EskfOdomAlgNode::mainNodeThread(void)
......@@ -172,8 +180,8 @@ void EskfOdomAlgNode::mainNodeThread(void)
this->alg_.set_obs_phase(is_flying,this->gnd_dist_);
this->alg_.unlock();
Eigen::VectorXd state(19,1);
Eigen::Vector3d ang_vel;
Eigen::VectorXf state(19,1);
Eigen::Vector3f ang_vel;
bool step_done = false;
this->alg_.lock();
......@@ -229,7 +237,7 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
this->is_first_px4_ = false;
}
set_px4_reading(msg, t-this->t_ini_px4_);
set_px4_reading(msg, static_cast<float>(t-this->t_ini_px4_));
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
{
......@@ -252,7 +260,7 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
this->is_first_imu_ = false;
}
set_imu_reading(msg, t-this->t_ini_imu_);
set_imu_reading(msg, static_cast<float>(t-this->t_ini_imu_));
}
void EskfOdomAlgNode::imu_mutex_enter(void)
{
......@@ -263,21 +271,21 @@ 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 float& msg_time)
{
// 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
float ax = msg->linear_acceleration.x;
float ay = -msg->linear_acceleration.y; // axis change according to simulation
float az = -msg->linear_acceleration.z; // axis change according to simulation
float wx = msg->angular_velocity.x;
float wy = -msg->angular_velocity.y; // axis change according to simulation
float wz = -msg->angular_velocity.z; // axis change according to simulation
this->imu_mutex_exit();
Eigen::Vector3d a;
Eigen::Vector3f a;
a << ax, ay, az;
Eigen::Vector3d w;
Eigen::Vector3f w;
w << wx, wy, wz;
this->alg_.lock();
......@@ -285,22 +293,22 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,cons
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 float& msg_time)
{
// 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
float vx = msg->velocity_x;
float vy = -msg->velocity_y; // axis change according to simulation
this->px4_of_mutex_exit();
Eigen::Vector3d val;
Eigen::Vector3f val;
val << this->gnd_dist_, vx, vy;
double flowx = msg->flow_x;
double flowy = -msg->flow_y; // axis change according to simulation
float flowx = msg->flow_x;
float flowy = -msg->flow_y; // axis change according to simulation
Eigen::Vector2d flow;
Eigen::Vector2f flow;
flow << flowx, flowy;
this->alg_.lock();
......
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