changed to priority queue so measurements are in temporal order

inekf_ros/config/settings.yaml

@@ -7,7 +7,7 @@ settings:
   state_topic: "/state"
   publish_visualization_markers: true
   visualization_markers_topic: "/markers"
-  enable_landmarks: true
+  enable_landmarks: false
   landmarks_topic: "/landmarks"
   enable_kinematics: true
   kinematics_topic: "/kinematics"

inekf_ros/include/InEKF_ROS.h

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   InEKF_ROS.h
+ *  @author Ross Hartley
+ *  @brief  Header file for a ROS wrapper of the Invariant EKF 
+ *  @date   September 27, 2018
+ **/
+
 #ifndef INEKF_ROS_H
 #define INEKF_ROS_H 
 #include <Eigen/Dense>
@@ -19,7 +32,9 @@
 #include "apriltag_msgs/AprilTagDetectionArray.h"
 #include <mutex>
 
-#define MAX_QUEUE_SIZE 100
+#define QUEUE_BUFFER_SIZE 100
+#define MAX_QUEUE_SIZE 150
+
 
 class InEKF_ROS {
     public:
@@ -36,7 +51,7 @@ class InEKF_ROS {
         ros::Subscriber contact_sub_;
         std::thread filtering_thread_;
         std::thread output_thread_;
-        Queue<std::shared_ptr<Measurement>> m_queue_;
+        Queue<std::shared_ptr<Measurement>, std::vector<std::shared_ptr<Measurement>>, MeasurementCompare> m_queue_;
 
         std::string imu_frame_id_;
         std::string map_frame_id_;

inekf_ros/include/Measurement.h

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   Measurement.h
+ *  @author Ross Hartley
+ *  @brief  Header file for Measurement class
+ *  @date   September 27, 2018
+ **/
+
 #ifndef MEASUREMENT_H
 #define MEASUREMENT_H 
 #include <Eigen/Dense>
@@ -12,6 +25,7 @@
 
 enum MeasurementType {EMPTY, IMU, LANDMARK, KINEMATIC, CONTACT};
 
+
 class Measurement {
 
     public:
@@ -29,6 +43,13 @@ class Measurement {
         MeasurementType type_;
 };
 
+struct MeasurementCompare {
+  bool operator()(const std::shared_ptr<Measurement> lhs, const std::shared_ptr<Measurement> rhs) const {
+    return lhs->getTime() > rhs->getTime();
+  }
+};
+
+
 class ImuMeasurement : public Measurement {
 
     public:

inekf_ros/include/Queue.h

@@ -1,21 +1,34 @@
+/*
+  Copyright (c) 2013, Juan Palacios <[email protected]>
+  All rights reserved.
+*/
+
+/**
+ *  @file   Queue.h
+ *  @author Juan Palacios
+ *  @brief  Thread-safe queue class (https://github.com/juanchopanza)
+ *  @date   September 27, 2018
+ **/
+
 #include <queue>
 #include <thread>
 #include <mutex>
+#include <vector>
 #include <condition_variable>
 
-template <typename T>
+template <class T, class Container = std::vector<T>, class Compare = std::less<typename Container::value_type> >
 class Queue
 {
  public:
- 
+
   T pop()
   {
     std::unique_lock<std::mutex> mlock(mutex_);
     while (queue_.empty())
     {
       cond_.wait(mlock);
     }
-    auto item = queue_.front();
+    auto item = queue_.top();
     queue_.pop();
     return item;
   }
@@ -27,7 +40,7 @@ class Queue
     {
       cond_.wait(mlock);
     }
-    item = queue_.front();
+    item = queue_.top();
     queue_.pop();
   }
 
@@ -60,7 +73,7 @@ class Queue
   }
 
  private:
-  std::queue<T> queue_;
+  std::priority_queue<T,Container,Compare> queue_;
   std::mutex mutex_;
   std::condition_variable cond_;
 };

inekf_ros/launch/data_file_test.launch

@@ -11,7 +11,7 @@
 	<!--<node pkg="apriltag2ROS" type="apriltag2_pub" name="apriltag2_pub" />-->
 
   <!-- Create data publisher-->
-  <node name="data_file_publisher" pkg="inekf_ros" type="data_file_publisher_node">
+  <node name="data_file_publisher" pkg="inekf_ros" type="data_file_publisher_node" >
     <param name="data_file" type="string" value="$(find inekf_ros)/data/imu_landmark_kinematic_measurements.txt" />
   </node>
 

inekf_ros/src/InEKF_ROS.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   InEKF_ROS.cpp
+ *  @author Ross Hartley
+ *  @brief  Source file for a ROS wrapper of the Invariant EKF 
+ *  @date   September 27, 2018
+ **/
+
 #include "InEKF_ROS.h"
 using namespace std;
 using namespace inekf;
@@ -258,7 +271,7 @@ void InEKF_ROS::mainFilteringThread() {
     double t, t_last;
 
     // Block until first IMU measurement is received
-    while (true) {
+    while (ros::ok()) {
         // Try next item (Blocking)
         m_queue_.pop(m_ptr);
         if (m_ptr->getType() == IMU) {
@@ -271,13 +284,18 @@ void InEKF_ROS::mainFilteringThread() {
     }
 
     // Main loop
-    while (true) {
+    while (ros::ok()) {
         // Throw warning if measurement queue is getting too large
         if (m_queue_.size() > MAX_QUEUE_SIZE) {
             ROS_WARN("Measurement queue size (%d) is greater than MAX_QUEUE_SIZE. Filter is not realtime!", m_queue_.size());
         }   
+        // Wait until buffer is full
+        while(m_queue_.size() < QUEUE_BUFFER_SIZE) {
+            this_thread::sleep_for(chrono::milliseconds(10));
+        }
         // Retrieve next measurement (Blocking)
         m_queue_.pop(m_ptr);
+        // ROS_ERROR("Time: %f", m_ptr->getTime());
         // Handle measurement
         switch (m_ptr->getType()) {
             case IMU: {

inekf_ros/src/Measurement.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   Measurement.cpp
+ *  @author Ross Hartley
+ *  @brief  Source file for Measurement class
+ *  @date   September 27, 2018
+ **/
+
 #include "Measurement.h"
 using namespace std;
 using namespace inekf;

inekf_ros/src/inekf_node.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   inekf_node.cpp
+ *  @author Ross Hartley
+ *  @brief  Creates ROS node that runs the invariant-ekf
+ *  @date   September 27, 2018
+ **/
+
 #include "ros/ros.h"
 #include "InEKF_ROS.h"
 #include <chrono>
@@ -17,26 +30,3 @@ int main(int argc, char **argv) {
 
     return 0;
 }
-
-
-    // Test InEKF filter speed
-    // int N = 10000;
-    // InEKF filter;
-    // cout << filter.getNoiseParams() << endl;
-    // cout << filter.getState() << endl;
-    // Eigen::Matrix<double,6,1> m; m << 0,0,0, 0,0,9.81;
-    // double dt = 0.01;
-    // cout << "Propagating " << N << " IMU measurements...\n";
-    // int64_t max_duration = 0;
-    // int64_t sum_duration = 0;
-    // for(int i=0; i<N; ++i){
-    //     chrono::high_resolution_clock::time_point start_time = chrono::high_resolution_clock::now();
-    //     filter.Propagate(m, dt);
-    //     chrono::high_resolution_clock::time_point end_time = chrono::high_resolution_clock::now();
-    //     auto duration = chrono::duration_cast<chrono::microseconds>( end_time - start_time ).count();
-    //     sum_duration += duration;
-    //     if (duration > max_duration)
-    //         max_duration = duration;
-    // }
-    // cout << "max duration: " <<  max_duration << endl;
-    // cout << "average duration: " <<  double(sum_duration)/double(N) << endl;

inekf_ros/tests/data_file_publisher.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   data_file_publisher.cpp
+ *  @author Ross Hartley
+ *  @brief  Publishes data from a text file
+ *  @date   September 27, 2018
+ **/
+
 #include "ros/ros.h"
 #include "sensor_msgs/Imu.h"
 #include "inekf_msgs/LandmarkArray.h"

inekf_ros/tests/fake_contact_kinematics_publisher.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   fake_contact_kinematics_publisher.cpp
+ *  @author Ross Hartley
+ *  @brief  Publishes fake contact and kinematic data over the /contacts and /kinematics topic
+ *  @date   September 27, 2018
+ **/
+
 #include "ros/ros.h"
 #include "inekf_msgs/ContactArray.h"
 #include "inekf_msgs/KinematicsArray.h"

inekf_ros/tests/fake_imu_publisher.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   fake_imu_publisher.cpp
+ *  @author Ross Hartley
+ *  @brief  Publishes fake imu data over the /imu topic
+ *  @date   September 27, 2018
+ **/
+
 #include "ros/ros.h"
 #include "sensor_msgs/Imu.h"
 #include <random>

inekf_ros/tests/fake_landmark_publisher.cpp

@@ -1,3 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2018, Ross Hartley <[email protected]>
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   fake_landmark_publisher.cpp
+ *  @author Ross Hartley
+ *  @brief  Publishes fake landmark data over the /landmarks topic
+ *  @date   September 27, 2018
+ **/
+
 #include "ros/ros.h"
 #include "inekf_msgs/Landmark.h"
 #include "inekf_msgs/LandmarkArray.h"