added FLOW2D subscriber

CMakeLists.txt

@@ -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)

include/eskf_odom_alg_node.h

@@ -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_;

package.xml

@@ -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>

src/eskf_odom_alg_node.cpp

@@ -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");