Manually adding second spin for odometry and IMU data that is passed …

…to message filters
arece-fs · Nov 25, 2015 · 247a7c6 · 247a7c6
1 parent d35dbac
commit 247a7c6
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 1 deletion.
diff --git a/include/robot_localization/ros_filter.h b/include/robot_localization/ros_filter.h
@@ -445,6 +445,12 @@ template<class T> class RosFilter
     //!
     MeasurementQueue measurementQueue_;
 
+    //! @brief If we receive an odometry or IMU message, we put it into a message
+    //! filter queue. Those won't get processed until the next spin cycle, but we
+    //! can detect that event and manually do a second spin.
+    //!
+    bool messageFiltersEmpty_;
+
     //! @brief Node handle
     //!
     ros::NodeHandle nh_;

diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
@@ -54,6 +54,7 @@ namespace RobotLocalization
       filter_(args),
       frequency_(30.0),
       lastSetPoseTime_(0),
+      messageFiltersEmpty_(true),
       nhLocal_("~"),
       printDiagnostics_(true),
       twoDMode_(false)
@@ -323,6 +324,7 @@ namespace RobotLocalization
 
         geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
         poseMessageFilters_[imuPoseTopicName]->add(pptr);
+        messageFiltersEmpty_ = false;
       }
     }
 
@@ -354,6 +356,7 @@ namespace RobotLocalization
 
         geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
         twistMessageFilters_[imuTwistTopicName]->add(tptr);
+        messageFiltersEmpty_ = false;
       }
     }
 
@@ -372,6 +375,7 @@ namespace RobotLocalization
         // actually have a good container message for it, so just pass
         // the IMU message on through a message filter.
         accelerationMessageFilters_[imuAccelTopicName]->add(msg);
+        messageFiltersEmpty_ = false;
       }
     }
 
@@ -1365,7 +1369,7 @@ namespace RobotLocalization
 
   template<typename T>
   void RosFilter<T>::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg,
-                                   const std::string &topicName)
+                                      const std::string &topicName)
   {
     // If we've just reset the filter, then we want to ignore any messages
     // that arrive with an older timestamp
@@ -1387,6 +1391,7 @@ namespace RobotLocalization
 
       geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
       poseMessageFilters_[odomPoseTopicName]->add(pptr);
+      messageFiltersEmpty_ = false;
     }
 
     std::string odomTwistTopicName = topicName + "_twist";
@@ -1400,6 +1405,7 @@ namespace RobotLocalization
 
       geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
       twistMessageFilters_[odomTwistTopicName]->add(tptr);
+      messageFiltersEmpty_ = false;
     }
 
     RF_DEBUG("\n----- /RosFilter::odometryCallback (" << topicName << ") ------\n");
@@ -1539,6 +1545,12 @@ namespace RobotLocalization
       // their received measurements
       ros::spinOnce();
 
+      if(!messageFiltersEmpty_)
+      {
+        ros::spinOnce();
+        messageFiltersEmpty_ = true;
+      }
+
       // Now we'll integrate any measurements we've received
       curTime = ros::Time::now();
       integrateMeasurements(curTime.toSec());