Commit 2ea7bea9 authored by asantamaria's avatar asantamaria

Added frequency check

parent 4d576f38
......@@ -54,6 +54,10 @@ class EskfOdomAlgNode: public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm
private:
ros::Time ros_t_init_; // Initial ROS time.
ros::Time ros_t_last_; // ROS time in last loop step.
ros::Time ros_t_curr_; // ROS time in current loop step.
bool flying_; // Quadrotor landed or flying information.
Eigen::VectorXf range_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
......
......@@ -25,6 +25,11 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->new_flow2d_ = false;
this->new_px4_ = false;
// Initialize ROS Time objects
this->ros_t_init_ = ros::Time::now();
this->ros_t_last_ = this->ros_t_init_;
this->ros_t_curr_ = this->ros_t_init_;
// Read initial parameters from yaml file and set themn to sensors and filter
read_and_set_ini_params();
......@@ -197,6 +202,14 @@ Eigen::VectorXf EskfOdomAlgNode::read_vec(const std::string& param_name,const in
void EskfOdomAlgNode::mainNodeThread(void)
{
// // DEBUG: Frequency check
// this->ros_t_curr_ = ros::Time::now();
// double t_last = (this->ros_t_last_ - this->ros_t_init_).toSec();
// double t_curr = (this->ros_t_curr_ - this->ros_t_init_).toSec();
// double dt = t_curr - t_last;
// this->ros_t_last_ = this->ros_t_curr_;
// std::cout << "Current freq: " << 1/dt << " Hz" << std::endl;
// [fill msg structures]
// [fill srv structure and make request to the server]
......
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