fixed timing issues when used with ext propagation. Now working with

pressure sensor and height init.
MarcGyongyosi · Jan 30, 2013 · 5af6cae · 5af6cae
1 parent 47ada13
commit 5af6cae
Show file tree

Hide file tree

Showing 10 changed files with 59 additions and 104 deletions.
diff --git a/msf_core/.gitignore b/msf_core/.gitignore
@@ -0,0 +1 @@
+/docs
diff --git a/msf_core/include/msf_core/implementation/msf_core.hpp b/msf_core/include/msf_core/implementation/msf_core.hpp
@@ -36,7 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <msf_core/msf_sensormanager.h>
 #include <msf_core/msf_tools.h>
 #include <msf_core/msf_measurement.h>
-#include <boost/thread.hpp>
+#include <chrono>
+#include <thread>
 
 namespace msf_core
 {
@@ -52,7 +53,6 @@ MSF_Core<EKFState_T>::MSF_Core(MSF_SensorManager<EKFState_T>& usercalc):usercalc
   g_ << 0, 0, 9.81;
 
   //TODO later: move all this to the external file and derive from this class. We could by this allow compilation on platforms withour ROS
-  // ros stuff
   ros::NodeHandle nh("msf_core");
   ros::NodeHandle pnh("~");
 
@@ -99,7 +99,7 @@ void MSF_Core<EKFState_T>::initExternalPropagation(boost::shared_ptr<EKFState_T>
   msgCorrect_.linear_acceleration.y = 0;
   msgCorrect_.linear_acceleration.z = 0;
 
-  msgCorrect_.state.resize(HLI_EKF_STATE_SIZE); //make sure this is correctly sized
+  msgCorrect_.state.resize(HLI_EKF_STATE_SIZE);
   boost::fusion::for_each(
       state->statevars,
       msf_tmp::CoreStatetoDoubleArray<std::vector<float>, StateSequence_T >(msgCorrect_.state)
@@ -122,18 +122,14 @@ void MSF_Core<EKFState_T>::imuCallback(const sensor_msgs::ImuConstPtr & msg)
     return; // // early abort // //
   }
 
-
   boost::shared_ptr<EKFState_T> currentState(new EKFState_T);
-
   currentState->time = msg->header.stamp.toSec();
 
   static int seq = 0;
-
   // get inputs
   currentState->a_m << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
   currentState->w_m << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
 
-
   // remove acc spikes (TODO: find a cleaner way to do this)
   static Eigen::Matrix<double, 3, 1> last_am = Eigen::Matrix<double, 3, 1>(0, 0, 0);
   if (currentState->a_m.norm() > 50)
@@ -166,8 +162,8 @@ void MSF_Core<EKFState_T>::imuCallback(const sensor_msgs::ImuConstPtr & msg)
     return;
   }
 
+  //propagate state and covariance
   propagateState(lastState, currentState);
-
   propagatePOneStep();
 
   if(StateBuffer_.size() > 3) //making sure the hl propagation has provided some data to apply measurements to
@@ -195,20 +191,18 @@ void MSF_Core<EKFState_T>::imuCallback(const sensor_msgs::ImuConstPtr & msg)
 template<typename EKFState_T>
 void MSF_Core<EKFState_T>::stateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg)
 {
-
   if(!initialized_)
     return;
 
+  //get the closest state and check validity
   boost::shared_ptr<EKFState_T> lastState = StateBuffer_.getClosestBefore(msg->header.stamp.toSec());
-
   if(lastState->time == -1){
     ROS_WARN_STREAM_THROTTLE(2, "StateCallback: closest state is invalid\n");
     return; // // early abort // //
   }
 
-
+  //create a new state
   boost::shared_ptr<EKFState_T> currentState(new EKFState_T);
-
   currentState->time = msg->header.stamp.toSec();
 
   // get inputs
@@ -266,15 +260,12 @@ void MSF_Core<EKFState_T>::stateCallback(const sensor_fusion_comm::ExtEkfConstPt
   isnumeric = checkForNumeric((double*)(&currentState-> template get<StateDefinition_T::p>()[0]), 3, "prediction p");
   isnumeric = checkForNumeric((double*)(&currentState->P(0)), nErrorStatesAtCompileTime * nErrorStatesAtCompileTime, "prediction done P");
 
-  if(!predictionMade_){ //first prediction
-    ROS_WARN_STREAM("first prediction, buffer has "<<StateBuffer_.size()<<" states.");
-    if(StateBuffer_.size() != 0){
-      for(typename stateBufferT::iterator_T it = StateBuffer_.getIteratorBegin(); it!= StateBuffer_.getIteratorEnd(); ++it){
-        ROS_WARN_STREAM("state "<<it->second->toEigenVector());
-      }
-      ROS_WARN_STREAM("current propagated state "<<currentState->toEigenVector());
-    }
+  if(!predictionMade_){ //we do another clean reset of state and measurement buffer, before we start propagation
     StateBuffer_.clear();
+    MeasurementBuffer_.clear();
+    while(!queueFutureMeasurements_.empty()){
+      queueFutureMeasurements_.pop();
+    }
   }
   predictionMade_ = true;
 
@@ -352,7 +343,7 @@ void MSF_Core<EKFState_T>::propagateState(boost::shared_ptr<EKFState_T>& state_o
 template<typename EKFState_T>
 void MSF_Core<EKFState_T>::propagatePOneStep(){
   //also propagate the covariance one step further, to distribute the processing load over time
-  typename stateBufferT::iterator_T stateIteratorPLastPropagated = StateBuffer_.getIteratorAtValue(time_P_propagated);
+  typename stateBufferT::iterator_T stateIteratorPLastPropagated = StateBuffer_.getIteratorAtValue(time_P_propagated, false);
   typename stateBufferT::iterator_T stateIteratorPLastPropagatedNext = stateIteratorPLastPropagated;
   ++stateIteratorPLastPropagatedNext;
   if(stateIteratorPLastPropagatedNext != StateBuffer_.getIteratorEnd()){ //might happen if there is a measurement in the future
@@ -447,7 +438,7 @@ void MSF_Core<EKFState_T>::init(boost::shared_ptr<MSF_MeasurementBase<EKFState_T
   MeasurementBuffer_.clear();
   StateBuffer_.clear();
 
-  boost::this_thread::sleep(boost::posix_time::milliseconds(10)); //hackish thread sync
+  std::this_thread::sleep_for(std::chrono::milliseconds(10)); //hackish thread sync
 
   while(!queueFutureMeasurements_.empty())
     queueFutureMeasurements_.pop();
@@ -468,8 +459,12 @@ void MSF_Core<EKFState_T>::init(boost::shared_ptr<MSF_MeasurementBase<EKFState_T
   //apply init measurement, where the user can provide additional values for P
   measurement->apply(state, *this);
 
+  std::this_thread::sleep_for(std::chrono::milliseconds(100)); //wait for the external propagation to get the init message
+
+  assert(state->time != 0);
+
   StateBuffer_.insert(state);
-  time_P_propagated = 0; //will be set upon first IMU message
+  time_P_propagated = state->time; //will be set upon first IMU message
 
   //echo params
   ROS_INFO_STREAM("Core parameters:"<<std::endl<<
@@ -480,6 +475,7 @@ void MSF_Core<EKFState_T>::init(boost::shared_ptr<MSF_MeasurementBase<EKFState_T
                   "\tnoise_gyr:\t"<<usercalc_.getParam_noise_gyr()<<std::endl<<
                   "\tnoise_gyrbias:\t"<<usercalc_.getParam_noise_gyrbias()<<std::endl);
 
+//  ROS_INFO_STREAM("Initial state: \n"<<state->toEigenVector());
   initialized_ = true;
 }
 
@@ -503,6 +499,9 @@ void MSF_Core<EKFState_T>::addMeasurement(boost::shared_ptr<MSF_MeasurementBase<
 
   for( ; it_meas != it_meas_end; ++it_meas){
 
+    if(it_meas->second->time <= 0) //valid?
+      continue;
+
     boost::shared_ptr<EKFState_T> state = getClosestState(it_meas->second->time); //propagates covariance to state
 
     if(state->time <= 0){
@@ -628,7 +627,7 @@ boost::shared_ptr<EKFState_T> MSF_Core<EKFState_T>::getClosestState(double tstam
   //do state interpolation if state is too far away from the measurement
   double tdiff = fabs(closestState->time - timenow); //timediff to closest state
 
-  if(tdiff > 0.001){ // if time diff too large, insert new state and do state interpolation
+  if(tdiff > 0.001 && false){ // if time diff too large, insert new state and do state interpolation
     boost::shared_ptr<EKFState_T> lastState = StateBuffer_.getClosestBefore(timenow);
     boost::shared_ptr<EKFState_T> nextState = StateBuffer_.getClosestAfter(timenow);
 
@@ -661,6 +660,11 @@ boost::shared_ptr<EKFState_T> MSF_Core<EKFState_T>::getClosestState(double tstam
 
   propPToState(closestState); // catch up with covariance propagation if necessary
 
+  if(!closestState->checkStateForNumeric()){
+    ROS_ERROR_STREAM("State interpolation: interpolated state is invalid (nan)");
+    return StateBuffer_.getInvalid(); // // early abort // //
+  }
+
   static int janitorRun = 0;
   if(janitorRun++ > 100){
     CleanUpBuffers(); //remove very old states and measurements from the buffers
@@ -673,7 +677,7 @@ template<typename EKFState_T>
 void MSF_Core<EKFState_T>::propPToState(boost::shared_ptr<EKFState_T>& state)
 {
   // propagate cov matrix until the current states time
-  typename stateBufferT::iterator_T it = StateBuffer_.getIteratorAtValue(time_P_propagated);
+  typename stateBufferT::iterator_T it = StateBuffer_.getIteratorAtValue(time_P_propagated, false);
   typename stateBufferT::iterator_T itMinus = it;
   ++it;
   //until we reached the current state or the end of the state list

diff --git a/msf_core/include/msf_core/implementation/msf_measurement.hpp b/msf_core/include/msf_core/implementation/msf_measurement.hpp
@@ -64,8 +64,8 @@ void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(boost::shared_
   core.applyCorrection(state, correction_);
 }
 template<typename EKFState_T>
-void MSF_InitMeasurement<EKFState_T>::apply(boost::shared_ptr<EKFState_T> stateWithCovariance, MSF_Core<EKFState_T>& core){
-
+void MSF_InitMeasurement<EKFState_T>::apply(boost::shared_ptr<EKFState_T> stateWithCovariance, MSF_Core<EKFState_T>& core)
+{
 
   stateWithCovariance->time = ros::Time::now().toSec(); //makes this state a valid starting point
 
@@ -88,6 +88,7 @@ void MSF_InitMeasurement<EKFState_T>::apply(boost::shared_ptr<EKFState_T> stateW
     stateWithCovariance->a_m.setZero();
     stateWithCovariance->w_m.setZero();
   }
+
   core.initExternalPropagation(stateWithCovariance);
 
 }

diff --git a/msf_core/include/msf_core/msf_sortedContainer.h b/msf_core/include/msf_core/msf_sortedContainer.h
@@ -84,7 +84,7 @@ class SortedContainer{
     std::pair<typename ListT::iterator,bool> itpr =
         stateList.insert(std::pair<double, boost::shared_ptr<T> >(value->time, value));
     if(!itpr.second){
-      ROS_WARN_STREAM("Wanted to insert a value to the sorted container at time "<<timehuman(value->time)<<" but the map already contained a value at this time. discarding.");
+      ROS_WARN_STREAM("Wanted to insert a value to the sorted container at time "<<std::fixed<<std::setprecision(9)<<value->time<<" but the map already contained a value at this time. discarding.");
     }
     return itpr.first;
   }
@@ -109,10 +109,11 @@ class SortedContainer{
    * \param value the value to get the iterator for
    * \returns iterator
    */
-  inline typename ListT::iterator getIteratorAtValue(const boost::shared_ptr<T>& value){
+  inline typename ListT::iterator getIteratorAtValue(const boost::shared_ptr<T>& value, bool warnIfNotExistant = true){
     typename ListT::iterator it = stateList.find(value->time);
     if(it==stateList.end()){ //there is no value in the map with this time
-      ROS_WARN_STREAM("getIteratorAtValue(state): Could not find value for time "<<value->time<<"");
+      if(warnIfNotExistant)
+        ROS_WARN_STREAM("getIteratorAtValue(state): Could not find value for time "<<std::fixed<<std::setprecision(9)<<value->time<<"");
       it = stateList.lower_bound(value->time);
     }
     return it;
@@ -125,10 +126,11 @@ class SortedContainer{
    * \param time the time where we want to get an iterator at
    * \returns iterator
    */
-  inline typename ListT::iterator getIteratorAtValue(const double& time){
+  inline typename ListT::iterator getIteratorAtValue(const double& time, bool warnIfNotExistant = true){
     typename ListT::iterator it = stateList.find(time);
     if(it==stateList.end()){ //there is no value in the map with this time
-      ROS_WARN_STREAM("getIteratorAtValue(double): Could not find value for time "<<time<<"");
+      if(warnIfNotExistant)
+        ROS_WARN_STREAM("getIteratorAtValue(double): Could not find value for time "<<std::fixed<<std::setprecision(9)<<time<<"");
       it = stateList.lower_bound(time);
     }
     return it;
@@ -296,7 +298,7 @@ class SortedContainer{
     if(it == stateList.end()){
       std::stringstream ss;
       ss<<"Wanted to update a states/measurements time, but could not find the old state, "
-          "for which the time was asked to be updated. time "<<msf_core::timehuman(timeOld)<<std::endl;
+          "for which the time was asked to be updated. time "<<std::fixed<<std::setprecision(9)<<timeOld<<std::endl;
 
       ss<<"Map: "<<std::endl;
       for(typename ListT::iterator it2 = stateList.begin(); it2!= stateList.end(); ++it2){
@@ -321,7 +323,7 @@ class SortedContainer{
     std::stringstream ss;
 
     for(typename ListT::iterator it = getIteratorBegin();it!=getIteratorEnd();++it){
-      ss<<msf_core::timehuman(it->second->time)<<std::endl;
+      ss<<std::fixed<<std::setprecision(9)<<it->second->time<<std::endl;
     }
     return ss.str();
 

diff --git a/msf_core/src/.gitignore b/msf_core/src/.gitignore
@@ -0,0 +1 @@
+/msf_core
diff --git a/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h b/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h
@@ -79,8 +79,8 @@ struct PoseMeasurement : public msf_core::MSF_Measurement<geometry_msgs::PoseWit
       R_.block<6, 6>(0, 0) = Eigen::Matrix<double, 6, 6>(&msg->pose.covariance[0]);
 
       if(msg->header.seq % 100 == 0){ //only do this check from time to time
-        if(R_.block<6, 6>(0, 0).determinant()<=0)
-          ROS_ERROR_STREAM("The covariance matrix you provided is not positive definite");
+        if(R_.block<6, 6>(0, 0).determinant() < 0)
+          ROS_WARN_STREAM_THROTTLE(5,"The covariance matrix you provided is not positive definite");
       }
 
       //clear cross-correlations between q and p

diff --git a/msf_updates/launch/pose_pressure_sensor.launch b/msf_updates/launch/pose_pressure_sensor.launch
@@ -1,8 +1,10 @@
 <launch>
     <node  launch-prefix="gdb -ex run --args" name="msf_pose_pressure_sensor" pkg="msf_updates" type="pose_pressure_sensor" clear_params="true" output="screen">
-            <remap from="msf_core/imu_state_input" to="/odroidx/fcu/imu" />
-            <remap from="msf_updates/pose_with_covariance_input" to="/odroidx/down/vslam/pose" />
-            <remap from="msf_updates/pressure_input" to="/odroidx/fcu/imu_custom" />
+            <!--remap from="msf_core/imu_state_input" to="/bluebird/fcu/imu" /-->
+            <remap from="msf_core/hl_state_input" to="/bluebird/fcu/ekf_state_out" />
+            <remap from="msf_updates/pose_with_covariance_input" to="/bluebird/down/vslam/pose" />
+            <remap from="msf_updates/pressure_input" to="/bluebird/fcu/imu_custom" />
+            <remap from="msf_core/correction" to="/bluebird/fcu/ekf_state_in"/>
 
             <rosparam file="$(find msf_updates)/pose_pressure_sensor_fix.yaml"/>
     </node>

diff --git a/msf_updates/launch/pose_sensor.launch b/msf_updates/launch/pose_sensor.launch
@@ -1,13 +1,8 @@
 <launch>
     <node  launch-prefix="gdb -ex run --args" name="pose_sensor" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen">
-          <remap from="msf_core/imu_state_input" to="/odroidx/fcu/imu" />
-            <remap from="msf_updates/pose_with_covariance_input" to="/odroidx/down/vslam/pose" />
+          <remap from="msf_core/imu_state_input" to="/bluebird/fcu/imu" />
+          <remap from="msf_updates/pose_with_covariance_input" to="/bluebird/down/vslam/pose" />
 
-
-	<!--remap from="/msf_core/imu_state_input" to="/mav1/fcu/imu"/ -->
-		<!--remap from="/msf_core/hl_state_input" to="/mav1/fcu/ekf_state_out"/ -->
-		<!--remap from="/msf_updates/pose_input" to="/mav1/vslam/pose"/ -->
-
-            <rosparam file="$(find msf_updates)/pose_sensor_fix.yaml"/>
+          <rosparam file="$(find msf_updates)/pose_sensor_fix.yaml"/>
     </node>
 </launch>