Commit 442626aa authored by asantamaria's avatar asantamaria

Working with queue in sensors. moved robot from kinton to quadrotor names

parent a78d28ea
......@@ -51,6 +51,8 @@ class EskfOdomAlgorithm
CEskf filter_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief define config type
*
......@@ -126,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 VectorXd& x0, const 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);
/**
* \brief Print initial parameters
......@@ -162,7 +164,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 Vector3d& a, const Vector3d& w);
void set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w);
/**
* \brief Set PX4 Optical Flow Readings
......@@ -174,7 +176,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 Vector3d& val, const Vector2d& flow);
void set_px4_reading(const double& t, const Eigen::Vector3d& val, const Eigen::Vector2d& flow);
/**
* \brief Nominal State Propagation
......@@ -202,7 +204,7 @@ class EskfOdomAlgorithm
* Get nominal-state vector. Function to prevent external modifications.
*
*/
VectorXd get_x_state(void);
Eigen::VectorXd get_x_state(void);
/**
* \brief Destructor
......@@ -211,8 +213,6 @@ class EskfOdomAlgorithm
*
*/
~EskfOdomAlgorithm(void);
};
#endif
......@@ -50,7 +50,6 @@
// [action server client headers]
using namespace std;
using namespace Eigen;
//#define READ_FROM_FILE
......@@ -150,6 +149,9 @@ private:
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor
*
......@@ -219,7 +221,7 @@ protected:
*
* This function reads a 3-Vector parameter specified by param_name
*/
VectorXd read_vec(const string& param_name, const int& exp_long);
Eigen::VectorXd read_vec(const string& param_name, const int& exp_long);
};
#endif
......@@ -24,7 +24,7 @@ void EskfOdomAlgorithm::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
void EskfOdomAlgorithm::set_init_params(const params& f_params, const VectorXd& x0, const 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)
{
this->filter_.set_init_params(f_params,x0,dx0,imu,px4);
}
......@@ -39,14 +39,14 @@ void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist
this->filter_.set_obs_phase(flying,gnd_dist);
}
void EskfOdomAlgorithm::set_imu_reading(const double& t, const Vector3d& a, const Vector3d& w)
void EskfOdomAlgorithm::set_imu_reading(const double& t, const Eigen::Vector3d& a, const Eigen::Vector3d& w)
{
this->filter_.set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d& val, const Vector2d& flow)
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);
}
void EskfOdomAlgorithm::prop_nominal(void)
......
......@@ -75,9 +75,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
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
Vector3d a;
Eigen::Vector3d a;
a << ax,ay,az;
Vector3d w;
Eigen::Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
......@@ -164,10 +164,10 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
px4_params.std = px4_params.std_landed; // Considering initially landed
// Initialize Nominal-State vector
VectorXd x_state = read_vec("xstate0", 19);
Eigen::VectorXd x_state = read_vec("xstate0", 19);
// Initialize Error-State vector
VectorXd dx_state = read_vec("dxstate0", 18);
Eigen::VectorXd dx_state = read_vec("dxstate0", 18);
this->alg_.lock(); // protect algorithm
......@@ -184,10 +184,10 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->alg_.unlock();
}
VectorXd EskfOdomAlgNode::read_vec(const string& param_name,
Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,
const int& exp_long)
{
VectorXd params = VectorXd::Zero(exp_long);
Eigen::VectorXd params = Eigen::VectorXd::Zero(exp_long);
XmlRpc::XmlRpcValue my_list;
this->public_node_handle_.getParam(param_name, my_list);
......@@ -276,9 +276,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
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
Vector3d a;
Eigen::Vector3d a;
a << ax,ay,az;
Vector3d w;
Eigen::Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
......@@ -303,10 +303,10 @@ void EskfOdomAlgNode::mainNodeThread(void)
double flowx = this->px4_[this->px4_idx_][5];
double flowy = -this->px4_[this->px4_idx_][6];// axis change according to simulation
Vector3d val;
Eigen::Vector3d val;
val << this->gnd_dist_,vx,vy;
Vector2d flow;
Eigen::Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
......@@ -384,7 +384,7 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Get state and publish TF **********************
this->alg_.lock(); // protect algorithm
VectorXd state = this->alg_.get_x_state(); // get state
Eigen::VectorXd state = this->alg_.get_x_state(); // get state
this->alg_.unlock();
// Broadcast TF with state
......@@ -496,9 +496,9 @@ void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
double wz = -msg->angular_velocity.z; // axis change according to simulation
this->imu_mutex_exit();
Vector3d a;
Eigen::Vector3d a;
a << ax, ay, az;
Vector3d w;
Eigen::Vector3d w;
w << wx, wy, wz;
this->alg_.lock();
......@@ -516,13 +516,13 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
double vy = -msg->velocity_y; // axis change according to simulation
this->px4_of_mutex_exit();
Vector3d val;
Eigen::Vector3d val;
val << this->gnd_dist_, vx, vy;
double flowx = msg->flow_x;
double flowy = -msg->flow_y; // axis change according to simulation
Vector2d flow;
Eigen::Vector2d 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