Commit d485d078 authored by asantamaria's avatar asantamaria

added FLOW2D subscriber

parent 82b46b30
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs roscpp roslib px_comm sensor_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs nav_msgs roscpp roslib px_comm sensor_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm nav_msgs roscpp roslib px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm std_msgs nav_msgs roscpp roslib px_comm sensor_msgs tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
......@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRAR
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} nav_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} roscpp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} px_comm_generate_messages_cpp)
......
......@@ -35,6 +35,7 @@
#include <eigen3/Eigen/Dense>
// [publisher subscriber headers]
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <px_comm/OpticalFlow.h>
......@@ -56,16 +57,19 @@ 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_range_; // 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_flow2d_; // 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_range_; // Flag indicating a new RANGE 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 FLOW2D message is received.
bool new_flow2d_; // Flag indicating a new RANGE message is received.
bool new_px4_; // Flag indicating a new PX4 message is received.
double t_ini_imu_; // Initial IMU sensor time.
double t_ini_range_; // Initial RANGE sensor time.
double t_ini_flow2d_; // Initial RANGE sensor time.
double t_ini_px4_; // Initial PX4 sensor time.
int seq_; // Odometry sequence counter.
......@@ -77,6 +81,12 @@ private:
nav_msgs::Odometry odom_msg_;
// [subscriber attributes]
ros::Subscriber flow2d_subscriber_;
void flow2d_callback(const std_msgs::Float32MultiArray::ConstPtr& msg);
pthread_mutex_t flow2d_mutex_;
void flow2d_mutex_enter(void);
void flow2d_mutex_exit(void);
ros::Subscriber range_subscriber_;
void range_callback(const sensor_msgs::Range::ConstPtr& msg);
pthread_mutex_t range_mutex_;
......
......@@ -9,6 +9,7 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
......@@ -17,6 +18,7 @@
<build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
......
......@@ -18,9 +18,11 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
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_flow2d_ = true;
this->is_first_px4_ = true;
this->new_imu_ = false;
this->new_range_ = false;
this->new_flow2d_ = false;
this->new_px4_ = false;
// Read initial parameters from yaml file and set themn to sensors and filter
......@@ -30,6 +32,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
// [init subscribers]
this->flow2d_subscriber_ = this->public_node_handle_.subscribe("flow2d", 1, &EskfOdomAlgNode::flow2d_callback, this);
pthread_mutex_init(&this->flow2d_mutex_,NULL);
this->range_subscriber_ = this->public_node_handle_.subscribe("range", 1, &EskfOdomAlgNode::range_callback, this);
pthread_mutex_init(&this->range_mutex_,NULL);
......@@ -53,6 +58,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
EskfOdomAlgNode::~EskfOdomAlgNode(void)
{
// [free dynamic memory]
pthread_mutex_destroy(&this->flow2d_mutex_);
pthread_mutex_destroy(&this->range_mutex_);
pthread_mutex_destroy(&this->flying_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
......@@ -247,6 +253,92 @@ void EskfOdomAlgNode::mainNodeThread(void)
}
/* [subscriber callbacks] */
void EskfOdomAlgNode::flow2d_callback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::flow2d_callback: New Message Received");
// Expected Message Structure:
// layout -> dim[3] -> dim[0].label = "stamp"
// | |-> dim[0].size = 1
// | |-> dim[0].stride = 4*2*1
// | |-> dim[1].label = "optical_flow"
// | |-> dim[1].size = 2
// | |-> dim[1].stride = 4*2
// | |-> dim[2].label = "covariance"
// | |-> dim[2].size = 4
// | |-> dim[2].stride = 4
// -> data[7] -> data[0] = stamp
// |-> data[1] = Flow_X_data
// |-> data[2] = Flow_Y_data
// |-> data[3] = Cov_xx
// |-> data[4] = Cov_xy
// |-> data[5] = Cov_yx
// |-> data[6] = Cov_yy
this->flow2d_mutex_enter();
if (msg->layout.dim.size()==3)
{
// TIME
double t;
bool time_ok = false;
if (msg->layout.dim[0].label=="stamp" && msg->layout.dim[0].size == 1)
{
t = msg->data[0];
if (this->is_first_flow2d_)
{
this->t_ini_flow2d_ = t;
this->is_first_flow2d_ = false;
}
time_ok = true;
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Stamp Message Received. Possible Causes: Label or Data Size.");
// FLOW
if (msg->layout.dim[1].label=="optical_flow" && msg->layout.dim[1].size == 2 && time_ok)
{
Eigen::Vector2f flow2d;
flow2d << msg->data[1],msg->data[2];
this->alg_.lock();
this->alg_.set_flow2d_reading(static_cast<float>(t-this->t_ini_flow2d_), flow2d);
this->alg_.unlock();
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Optical Flow Message Received. Possible Causes: Stamp, Label or Data Size.");
//OPTIONAL: COVARIANCE from the message.
if (msg->layout.dim[2].label=="covariance" && msg->layout.dim[2].size == 4)
{
this->alg_.lock();
Sensor::flow2d_params flow2d_params = this->alg_.get_flow2d_params();
if (flow2d_params.std == flow2d_params.std_insidebounds)
{
flow2d_params.std << msg->data[3],msg->data[6];
this->alg_.set_flow2d_params(flow2d_params);
}
this->alg_.unlock();
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Covariance Message Received. Possible Causes: Label or Data Size.");
}
else
ROS_ERROR("EskfOdomAlgNode::flow2d_callback: Wrong Message Layout Received");
this->flow2d_mutex_exit();
}
void EskfOdomAlgNode::flow2d_mutex_enter(void)
{
pthread_mutex_lock(&this->flow2d_mutex_);
}
void EskfOdomAlgNode::flow2d_mutex_exit(void)
{
pthread_mutex_unlock(&this->flow2d_mutex_);
}
void EskfOdomAlgNode::range_callback(const sensor_msgs::Range::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::range_callback: New Message Received");
......
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