TAG: Working with read_from_file and ROS real-time, going to test it into the robot

parent 45fca492
......@@ -32,12 +32,13 @@
#include <iostream>
#include <sstream>
#include <string>
//TODO: delete this queue
#include <queue> // std::queue
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>
// [publisher subscriber headers]
#include <asctec_hl_comm/mav_status.h>
#include <asctec_msgs/LLStatus.h>
......@@ -57,154 +58,168 @@ using namespace Eigen;
* \brief IRI ROS Specific Algorithm Class
*
*/
class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>
{
private:
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.
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
#ifndef READ_FROM_FILE
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.
bool new_imu_; // Flag indicating a new IMU message is received.
bool new_px4_; // Flag indicating a new PX4 message is received.
double t_ini_px4_; // Initial PX4 sensor time.
double t_ini_imu_; // Initial IMU sensor time.
// [publisher attributes]
// [subscriber attributes]
ros::Subscriber hl_status_subscriber_;
void hl_status_callback(const asctec_hl_comm::mav_status::ConstPtr& msg);
pthread_mutex_t hl_status_mutex_;
void hl_status_mutex_enter(void);
void hl_status_mutex_exit(void);
ros::Subscriber ll_status_subscriber_;
void ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg);
pthread_mutex_t ll_status_mutex_;
void ll_status_mutex_enter(void);
void ll_status_mutex_exit(void);
ros::Subscriber px4_of_subscriber_;
void px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg);
pthread_mutex_t px4_of_mutex_;
void px4_of_mutex_enter(void);
void px4_of_mutex_exit(void);
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);
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.
bool new_imu_; // Flag indicating a new IMU message is received.
bool new_px4_; // Flag indicating a new PX4 message is received.
double t_ini_px4_; // Initial PX4 sensor time.
double t_ini_imu_; // Initial IMU sensor time.
//TODO: delete this doubles
double tlast_imu_;
double tlast_px4_;
//TODO: delete this queues
std::queue<sensor_msgs::Imu::ConstPtr> imu_queue_;
std::queue<px_comm::OpticalFlow::ConstPtr> px4_queue_;
// [publisher attributes]
// [subscriber attributes]
ros::Subscriber hl_status_subscriber_;
void hl_status_callback(const asctec_hl_comm::mav_status::ConstPtr& msg);
pthread_mutex_t hl_status_mutex_;
void hl_status_mutex_enter(void);
void hl_status_mutex_exit(void);
ros::Subscriber ll_status_subscriber_;
void ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg);
pthread_mutex_t ll_status_mutex_;
void ll_status_mutex_enter(void);
void ll_status_mutex_exit(void);
ros::Subscriber px4_of_subscriber_;
void px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg);
pthread_mutex_t px4_of_mutex_;
void px4_of_mutex_enter(void);
void px4_of_mutex_exit(void);
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_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
const double& msg_time);
#endif
// [service attributes]
// [service attributes]
// [client attributes]
// [client attributes]
// [action server attributes]
// [action server attributes]
// [action client attributes]
// [action client attributes]
#ifdef READ_FROM_FILE
int imu_idx_;
int px4_idx_;
int llstatus_idx_;
int imu_idx_;
int px4_idx_;
int llstatus_idx_;
double t_ini_imu_;
double t_ini_px4_;
double t_ini_llstatus_;
double t_ini_imu_;
double t_ini_px4_;
double t_ini_llstatus_;
std::string data_path_;
// double time_fix_;
// double dt_;
// double time_fix_;
// double dt_;
double max_freq_;
double max_freq_;
// vector<bool> ll_flying_; // To store bool due to different Hz
// vector<bool> ll_flying_; // To store bool due to different Hz
vector <vector<double>> a_;
vector <vector<double>> w_;
vector <vector<double>> px4_;
vector <vector<double>> llstatus_;
vector <vector<double>> a_;
vector <vector<double>> w_;
vector <vector<double>> px4_;
vector <vector<double>> llstatus_;
vector <vector<double>> read_from_file(const string& path);
vector <vector<double>> read_from_file(const string& path);
#endif
public:
/**
* \brief Constructor
*
* This constructor initializes specific class attributes and all ROS
* communications variables to enable message exchange.
*/
EskfOdomAlgNode(void);
/**
* \brief Destructor
*
* This destructor frees all necessary dynamic memory allocated within this
* this class.
*/
~EskfOdomAlgNode(void);
protected:
/**
* \brief main node thread
*
* This is the main thread node function. Code written here will be executed
* in every node loop while the algorithm is on running state. Loop frequency
* can be tuned by modifying loop_rate attribute.
*
* Here data related to the process loop or to ROS topics (mainly data structs
* related to the MSG and SRV files) must be updated. ROS publisher objects
* must publish their data in this process. ROS client servers may also
* request data to the corresponding server topics.
*/
void mainNodeThread(void);
/**
* \brief dynamic reconfigure server callback
*
* This method is called whenever a new configuration is received through
* the dynamic reconfigure. The derivated generic algorithm class must
* implement it.
*
* \param config an object with new configuration from all algorithm
* parameters defined in the config file.
* \param level integer referring the level in which the configuration
* has been changed.
*/
void node_config_update(Config &config, uint32_t level);
/**
* \brief node add diagnostics
*
* In this abstract function additional ROS diagnostics applied to the
* specific algorithms may be added.
*/
void addNodeDiagnostics(void);
// [diagnostic functions]
// [test functions]
/**
* \brief Read Parameters
*
* This function reads the parameters from the YAML file specified in the launch file
*/
void read_and_set_ini_params(void);
/**
* \brief Read 3-Vector parameter
*
* This function reads a 3-Vector parameter specified by param_name
*/
VectorXd read_vec(const string& param_name, const int& exp_long);
public:
/**
* \brief Constructor
*
* This constructor initializes specific class attributes and all ROS
* communications variables to enable message exchange.
*/
EskfOdomAlgNode(void);
/**
* \brief Destructor
*
* This destructor frees all necessary dynamic memory allocated within this
* this class.
*/
~EskfOdomAlgNode(void);
protected:
/**
* \brief main node thread
*
* This is the main thread node function. Code written here will be executed
* in every node loop while the algorithm is on running state. Loop frequency
* can be tuned by modifying loop_rate attribute.
*
* Here data related to the process loop or to ROS topics (mainly data structs
* related to the MSG and SRV files) must be updated. ROS publisher objects
* must publish their data in this process. ROS client servers may also
* request data to the corresponding server topics.
*/
void mainNodeThread(void);
/**
* \brief dynamic reconfigure server callback
*
* This method is called whenever a new configuration is received through
* the dynamic reconfigure. The derivated generic algorithm class must
* implement it.
*
* \param config an object with new configuration from all algorithm
* parameters defined in the config file.
* \param level integer referring the level in which the configuration
* has been changed.
*/
void node_config_update(Config &config, uint32_t level);
/**
* \brief node add diagnostics
*
* In this abstract function additional ROS diagnostics applied to the
* specific algorithms may be added.
*/
void addNodeDiagnostics(void);
// [diagnostic functions]
// [test functions]
/**
* \brief Read Parameters
*
* This function reads the parameters from the YAML file specified in the launch file
*/
void read_and_set_ini_params(void);
/**
* \brief Read 3-Vector parameter
*
* This function reads a 3-Vector parameter specified by param_name
*/
VectorXd read_vec(const string& param_name, const int& exp_long);
};
#endif
......@@ -23,4 +23,7 @@ px4_std_30cmto5m: [0.01,0.15,0.15]
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]
# Initial Error-State vector (dp,dv,dtheta,dab,dwb,dg)
dxstate0: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
\ No newline at end of file
dxstate0: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# Read from files in path:
data_path: /home/asantamaria/catkin_ws/src/iri/kinton_robot/eskf_odom/data/kinton_outdoor_paper/2
This diff is collapsed.
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