Commit 24280d88 authored by adrianamor's avatar adrianamor

parameters initialized and printed

parent 3b32a01e
......@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm px_comm sensor_msgs t
# ********************************************************************
find_package(eskf_odometry REQUIRED)
find_package(Eigen REQUIRED)
find_package(atools REQUIRED)
# ********************************************************************
# Add topic, service and action definition here
......@@ -75,7 +76,7 @@ catkin_package(
# ********************************************************************
# Add the include directories
# ********************************************************************
include_directories(include ${catkin_INCLUDE_DIRS} ${eskf_odometry_INCLUDE_DIR} ${Eigen_INCLUDE_DIRS})
include_directories(include ${catkin_INCLUDE_DIRS} ${eskf_odometry_INCLUDE_DIR} ${Eigen_INCLUDE_DIRS} ${atools_INCLUDE_DIR})
MESSAGE(${eskf_odometry_INCLUDE_DIR})
......@@ -88,7 +89,7 @@ add_executable(${PROJECT_NAME} src/eskf_odom_alg.cpp src/eskf_odom_alg_node.cpp)
# ********************************************************************
# Add the libraries
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRARY} ${Eigen_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRARY} ${Eigen_LIBRARIES} ${atools_LIBRARY})
# ********************************************************************
# Add message headers dependencies
......
......@@ -48,7 +48,7 @@ class EskfOdomAlgorithm
pthread_mutex_t access_;
// private attributes and methods
// CEskf filter_;
CEskf filter_;
public:
/**
......@@ -119,8 +119,23 @@ class EskfOdomAlgorithm
*/
void config_update(Config& new_cfg, uint32_t level=0);
// here define all eskf_odom_alg interface methods to retrieve and set
// the driver parameters
/**
* \brief Set filter initial parameters
*
* This function calls the low-level library to set the initial values of the filter, imu, px4,
* 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);
/**
* \brief Print initial parameters
*
* This function is used to print the parameters set of the filter, IMU, PX4
* and nominal-state and error-state vectors. Wrapper of the low-level library
*
*/
void print_ini_params(void);
/**
* \brief Destructor
......@@ -129,6 +144,8 @@ class EskfOdomAlgorithm
*
*/
~EskfOdomAlgorithm(void);
};
#endif
......@@ -20,4 +20,12 @@ void EskfOdomAlgorithm::config_update(Config& new_cfg, uint32_t level)
this->unlock();
}
// EskfOdomAlgorithm Public API
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)
{
this->filter_.set_init_params(f_params,x0,dx0,imu,px4);
}
void EskfOdomAlgorithm::print_ini_params()
{
this->filter_.print_ini_params();
}
\ No newline at end of file
#include "eskf_odom_alg_node.h"
//TODO: Quadrotor phases (taking-off, flying, landing, landed)
//this means adding subscription to low-level quadrotor driver
using namespace std;
using namespace Eigen;
......@@ -18,7 +21,8 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 1, &EskfOdomAlgNode::imu_callback, this);
pthread_mutex_init(&this->imu_mutex_,NULL);
read_params();
// Read initial parameters from yaml file and set themn to sensors and filter
read_and_set_ini_params();
// [init services]
......@@ -36,7 +40,7 @@ EskfOdomAlgNode::~EskfOdomAlgNode(void)
pthread_mutex_destroy(&this->imu_mutex_);
}
void EskfOdomAlgNode::read_params(void)
void EskfOdomAlgNode::read_and_set_ini_params(void)
{
// Initial STD Error-State values (filter parameters)
params f_params;
......@@ -79,40 +83,11 @@ void EskfOdomAlgNode::read_params(void)
// Initialize Error-State vector
VectorXd dx_state = read_vec("dxstate0",18);
//TODO: CALL low-level library to initialize parameters
// Print read values
cout << ">> ESKF_odom, loaded parameters: " << endl;
cout << " ______Filter parameters" << endl;
cout << " dp0_std: " << f_params.dp0_std(0) << "," << f_params.dp0_std(1) << "," << f_params.dp0_std(2)<< endl;
cout << " dv0_std: " << f_params.dv0_std(0) << "," << f_params.dv0_std(1) << "," << f_params.dv0_std(2)<< endl;
cout << " dtheta0_std: " << f_params.dtheta0_std(0) << "," << f_params.dtheta0_std(1) << "," << f_params.dtheta0_std(2)<< endl;
cout << " dab0_std: " << f_params.dab0_std(0) << "," << f_params.dab0_std(1) << "," << f_params.dab0_std(2)<< endl;
cout << " dwb0_std: " << f_params.dwb0_std(0) << "," << f_params.dwb0_std(1) << "," << f_params.dwb0_std(2)<< endl;
cout << " dg0_std: " << f_params.dg0_std(0) << "," << f_params.dg0_std(1) << "," << f_params.dg0_std(2)<< endl;
cout << " frame: " << f_params.frame << endl;
cout << " ______IMU parameters" << endl;
cout << " a_std: " << imu_params.a_std(0) << "," << imu_params.a_std(1) << "," << imu_params.a_std(2)<< endl;
cout << " w_std: " << imu_params.w_std(0) << "," << imu_params.w_std(1) << "," << imu_params.w_std(2)<< endl;
cout << " ab_std: " << imu_params.ab_std(0) << "," << imu_params.ab_std(1) << "," << imu_params.ab_std(2)<< endl;
cout << " wb_std: " << imu_params.wb_std(0) << "," << imu_params.wb_std(1) << "," << imu_params.wb_std(2)<< endl;
cout << " method: " << imu_params.met << endl;
cout << " ______Initial Nominal-State Vector" << endl;
cout << " p0: " << x_state(0) << "," << x_state(1) << "," << x_state(2) << endl;
cout << " v0: " << x_state(3) << "," << x_state(4) << "," << x_state(5) << endl;
cout << " q0: " << x_state(6) << "," << x_state(7) << "," << x_state(8) <<"," << x_state(9)<< endl;
cout << " ab0: " << x_state(10) << "," << x_state(11) << "," << x_state(12) << endl;
cout << " wb0: " << x_state(13) << "," << x_state(14) << "," << x_state(15) << endl;
cout << " g0: " << x_state(16) << "," << x_state(17) << "," << x_state(18) << endl;
cout << " ______Initial Error-State Vector" << endl;
cout << " dp0: " << dx_state(0) << "," << dx_state(1) << "," << dx_state(2) << endl;
cout << " dv0: " << dx_state(3) << "," << dx_state(4) << "," << dx_state(5) << endl;
cout << " dtheta0: " << dx_state(6) << "," << dx_state(7) << "," << dx_state(8) << endl;
cout << " dab0: " << dx_state(9) << "," << dx_state(10) << "," << dx_state(11) << endl;
cout << " dwb0: " << dx_state(12) << "," << dx_state(13) << "," << dx_state(14) << endl;
cout << " dg0: " << dx_state(15) << "," << dx_state(16) << "," << dx_state(17) << endl;
// Set filter and sensors initial parameters
this->alg_.set_init_params(f_params,x_state,dx_state,imu_params,px4_params);
// Print already set values of filter and sensors initial parameters
this->alg_.print_ini_params();
}
VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long)
......
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