Commit 82b46b30 authored by asantamaria's avatar asantamaria

added RANGE subscriber

parent 49478460
......@@ -35,6 +35,7 @@
#include <eigen3/Eigen/Dense>
// [publisher subscriber headers]
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <px_comm/OpticalFlow.h>
#include <sensor_msgs/Imu.h>
......@@ -55,14 +56,17 @@ private:
bool flying_; // Quadrotor landed or flying information.
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.
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_range_; // 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.
bool new_imu_; // Flag indicating a new IMU message is received.
bool new_range_; // Flag indicating a new RANGE 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.
double t_ini_imu_; // Initial IMU sensor time.
double t_ini_range_; // Initial RANGE sensor time.
double t_ini_px4_; // Initial PX4 sensor time.
int seq_; // Odometry sequence counter.
std::string odom_frame_id_; // Odometry frame ID.
......@@ -73,6 +77,12 @@ private:
nav_msgs::Odometry odom_msg_;
// [subscriber attributes]
ros::Subscriber range_subscriber_;
void range_callback(const sensor_msgs::Range::ConstPtr& msg);
pthread_mutex_t range_mutex_;
void range_mutex_enter(void);
void range_mutex_exit(void);
ros::Subscriber px4_of_subscriber_;
void px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg);
pthread_mutex_t px4_of_mutex_;
......
......@@ -33,6 +33,8 @@ linvel_std_insidebounds: [0.01,0.01,0.01]
# Range STD values
range_std_outsidebounds: 10.0
range_std_insidebounds: 0.01
range_min: 0.31
range_max: 4.0
# PX4 STD values
px4_std_outsidebounds: [10.0,10.0,10.0]
......
......@@ -17,8 +17,10 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->flying_ = false;
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
this->is_first_imu_ = true;
this->is_first_range_ = true;
this->is_first_px4_ = true;
this->new_imu_ = false;
this->new_range_ = false;
this->new_px4_ = false;
// Read initial parameters from yaml file and set themn to sensors and filter
......@@ -28,6 +30,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
// [init subscribers]
this->range_subscriber_ = this->public_node_handle_.subscribe("range", 1, &EskfOdomAlgNode::range_callback, this);
pthread_mutex_init(&this->range_mutex_,NULL);
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 10,&EskfOdomAlgNode::px4_of_callback, this);
pthread_mutex_init(&this->px4_of_mutex_, NULL);
......@@ -48,6 +53,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
EskfOdomAlgNode::~EskfOdomAlgNode(void)
{
// [free dynamic memory]
pthread_mutex_destroy(&this->range_mutex_);
pthread_mutex_destroy(&this->flying_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
......@@ -118,12 +124,17 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
range_params.std_outsidebounds = static_cast<float>(std_outsidebounds);
range_params.std_insidebounds = static_cast<float>(std_insidebounds);
range_params.std = range_params.std_outsidebounds; // Considering initially landed
double range_min, range_max;
this->public_node_handle_.param<double>("range_min", range_min, 4.0);
this->public_node_handle_.param<double>("range_max", range_max, 0.31);
range_params.range_min = static_cast<float>(range_min);
range_params.range_max = static_cast<float>(range_max);
// PX4 STD values
Sensor::px4_params px4_params;
px4_params.std_outsidebounds = read_vec("px4_std_outsidebounds", 3);
px4_params.std_insidebounds = read_vec("px4_std_insidebounds", 3);
px4_params.std = px4_params.std_outsidebounds;
px4_params.std = px4_params.std_outsidebounds;
// Flow2d STD values
Sensor::flow2d_params flow2d_params;
......@@ -236,20 +247,59 @@ void EskfOdomAlgNode::mainNodeThread(void)
}
/* [subscriber callbacks] */
void EskfOdomAlgNode::range_callback(const sensor_msgs::Range::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::range_callback: New Message Received");
this->range_mutex_enter();
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
if (this->is_first_range_)
{
this->t_ini_range_ = t;
this->is_first_range_ = false;
}
this->gnd_dist_ = msg->range;
// // OPTIONAL: Set the range limits dynamically
// Sensor::range_params range_params = this->alg_.get_range_params();
// if (std::isfinite(msg->min_range)) // Only set if known
// range_params.range_min = msg->min_range;
// if (std::isfinite(msg->max_range)) // Only set if known
// range_params.range_min = msg->max_range;
// this->alg_.lock();
// this->alg_.set_range_params(range_params);
// this->alg_.unlock();
this->range_mutex_exit();
this->alg_.lock();
this->alg_.set_range_reading(static_cast<float>(t-this->t_ini_range_), this->gnd_dist_); // Set values into filter object
this->alg_.unlock();
}
void EskfOdomAlgNode::range_mutex_enter(void)
{
pthread_mutex_lock(&this->range_mutex_);
}
//TODO: Add a range subscriber
void EskfOdomAlgNode::range_mutex_exit(void)
{
pthread_mutex_unlock(&this->range_mutex_);
}
void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
this->px4_of_mutex_enter();
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
if (this->is_first_px4_)
{
this->t_ini_px4_ = t;
this->is_first_px4_ = false;
}
this->px4_of_mutex_exit();
set_px4_reading(msg, static_cast<float>(t-this->t_ini_px4_));
}
......@@ -264,15 +314,17 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
this->imu_mutex_enter();
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
if (this->is_first_imu_)
{
this->t_ini_imu_ = t;
this->is_first_imu_ = false;
}
this->imu_mutex_exit();
set_imu_reading(msg, static_cast<float>(t-this->t_ini_imu_));
}
......
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