Feature/init biases from param

msf_core/include/msf_core/msf_sortedContainer.h

@@ -302,9 +302,10 @@ class SortedContainer {
    */
   inline void ClearOlderThan(double age) {
     double newest = GetLast()->time;
-    iterator_T it = GetIteratorClosest(newest - age);
-    if (newest - it->second->time < age)
+    double oldest = GetFirst()->time;
+    if (newest - oldest < age)
       return;  //there is no state older than time
+    iterator_T it = GetIteratorClosest(newest - age);
     if (it->second->time > stateList.begin()->second->time)
       stateList.erase(stateList.begin(), it);
   }

msf_updates/include/msf_updates/pose_sensor_handler/implementation/pose_sensorhandler.hpp

@@ -87,6 +87,10 @@ PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::PoseSensorHandler(
       "pose_input", 20, &PoseSensorHandler::MeasurementCallback, this,
       enable_tcp_no_delay ? ros::TransportHints().tcpNoDelay()
                           : ros::TransportHints());
+  subOdometry_ = nh.subscribe<nav_msgs::Odometry>(
+      "odometry_input", 20, &PoseSensorHandler::MeasurementCallback, this,
+      enable_tcp_no_delay ? ros::TransportHints().tcpNoDelay()
+                          : ros::TransportHints());
 
   z_p_.setZero();
   z_q_.setIdentity();
@@ -140,6 +144,20 @@ void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(
     const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) {
   received_first_measurement_ = true;
 
+  // Measurement throttling.
+  double time_now = msg->header.stamp.toSec();
+  const double epsilon = 0.001; // Small time correction to avoid rounding
+  // errors in the timestamps.
+  if (time_now - timestamp_previous_pose_ <=
+      pose_measurement_minimum_dt_ - epsilon) {
+    MSF_WARN_STREAM_THROTTLE(
+        30, "Pose measurement throttling is on, dropping messages"
+            "to be below " +
+                std::to_string(1 / pose_measurement_minimum_dt_) + " Hz");
+    return;
+  }
+  timestamp_previous_pose_ = time_now;
+
   // Get the fixed states.
   int fixedstates = 0;
   static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < 32, "Your state "
@@ -184,6 +202,7 @@ void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(
 
   this->manager_.msf_core_->AddMeasurement(meas);
 }
+
 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
     const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) {
@@ -197,6 +216,23 @@ void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
   ProcessPoseMeasurement(msg);
 }
 
+template <typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
+void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
+    const nav_msgs::OdometryConstPtr &msg) {
+  this->SequenceWatchDog(msg->header.seq, subOdometry_.getTopic());
+  MSF_INFO_STREAM_ONCE("*** pose sensor got first measurement from topic "
+                       << this->topic_namespace_ << "/"
+                       << subOdometry_.getTopic() << " ***");
+
+  geometry_msgs::PoseWithCovarianceStampedPtr pose(
+      new geometry_msgs::PoseWithCovarianceStamped());
+
+  pose->header = msg->header;
+  pose->pose = msg->pose;
+
+  ProcessPoseMeasurement(pose);
+}
+
 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
     const geometry_msgs::TransformStampedConstPtr & msg) {
@@ -206,17 +242,6 @@ void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
           << this->topic_namespace_ << "/" << subTransformStamped_.getTopic()
           << " ***");
 
-  double time_now = msg->header.stamp.toSec();
-  const double epsilon = 0.001; // Small time correction to avoid rounding errors in the timestamps.
-  if (time_now - timestamp_previous_pose_ <= pose_measurement_minimum_dt_ - epsilon) {
-    MSF_WARN_STREAM_THROTTLE(30, "Pose measurement throttling is on, dropping messages"
-                             "to be below " +
-                             std::to_string(1/pose_measurement_minimum_dt_) + " Hz");
-    return;
-  }
-
-  timestamp_previous_pose_ = time_now;
-
   geometry_msgs::PoseWithCovarianceStampedPtr pose(
       new geometry_msgs::PoseWithCovarianceStamped());
 

msf_updates/include/msf_updates/pose_sensor_handler/pose_sensorhandler.h

@@ -23,6 +23,7 @@
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
 #include <msf_updates/PoseDistorter.h>
 
 namespace msf_pose_sensor {
@@ -32,15 +33,16 @@ class PoseSensorHandler : public msf_core::SensorHandler<
     typename msf_updates::EKFState> {
  private:
 
-  Eigen::Quaternion<double> z_q_;  ///< Attitude measurement camera seen from world.
-  Eigen::Matrix<double, 3, 1> z_p_;  ///< Position measurement camera seen from world.
+  Eigen::Quaternion<double> z_q_;  ///< Attitude measurement camera seen from world. (q_vc)
+  Eigen::Matrix<double, 3, 1> z_p_;  ///< Position measurement camera seen from world. (v_p_vc)
   double n_zp_, n_zq_;  ///< Position and attitude measurement noise.
   double delay_;        ///< Delay to be subtracted from the ros-timestamp of
   // the measurement provided by this sensor.
 
   ros::Subscriber subPoseWithCovarianceStamped_;
   ros::Subscriber subTransformStamped_;
   ros::Subscriber subPoseStamped_;
+  ros::Subscriber subOdometry_;
 
   bool measurement_world_sensor_;  ///< Defines if the pose of the sensor is
                                    // measured in world coordinates (true, default)
@@ -62,8 +64,10 @@ class PoseSensorHandler : public msf_core::SensorHandler<
       const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
   void MeasurementCallback(const geometry_msgs::PoseStampedConstPtr & msg);
   void MeasurementCallback(const geometry_msgs::TransformStampedConstPtr & msg);
+  void MeasurementCallback(const nav_msgs::OdometryConstPtr & msg);
 
- public:
+
+public:
   typedef MEASUREMENT_TYPE measurement_t;
   PoseSensorHandler(MANAGER_TYPE& meas, std::string topic_namespace,
                     std::string parameternamespace, bool distortmeas);

msf_updates/include/msf_updates/position_sensor_handler/implementation/position_sensorhandler.hpp

@@ -55,7 +55,7 @@ PositionSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::PositionSensorHandler(
   MSF_INFO_STREAM_COND(!provides_absolute_measurements_, "Position sensor is "
                        "handling measurements as relative values");
 
-  ros::NodeHandle nh("msf_updates");
+  ros::NodeHandle nh("msf_updates/" + topic_namespace);
 
   subPointStamped_ = nh.subscribe<geometry_msgs::PointStamped>(
       "position_input", 20, &PositionSensorHandler::MeasurementCallback, this,

msf_updates/src/pose_msf/pose_sensormanager.h

@@ -158,8 +158,6 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
 
     // init values
     g << 0, 0, 9.81;	        /// Gravity.
-    b_w << 0, 0, 0;		/// Bias gyroscopes.
-    b_a << 0, 0, 0;		/// Bias accelerometer.
 
     v << 0, 0, 0;			/// Robot velocity (IMU centered).
     w_m << 0, 0, 0;		/// Initial angular velocity.
@@ -184,6 +182,17 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
           "No measurements received yet to initialize attitude - using [1 0 0 0]");
 
     ros::NodeHandle pnh("~");
+    pnh.param("core/init/b_w/x", b_w[0], 0.0);
+    pnh.param("core/init/b_w/y", b_w[1], 0.0);
+    pnh.param("core/init/b_w/z", b_w[2], 0.0);
+
+    pnh.param("core/init/b_a/x", b_a[0], 0.0);
+    pnh.param("core/init/b_a/y", b_a[1], 0.0);
+    pnh.param("core/init/b_a/z", b_a[2], 0.0);
+
+    MSF_INFO_STREAM("b_a: " << b_a.transpose());
+    MSF_INFO_STREAM("b_w: " << b_w.transpose());
+
     pnh.param("pose_sensor/init/p_ic/x", p_ic[0], 0.0);
     pnh.param("pose_sensor/init/p_ic/y", p_ic[1], 0.0);
     pnh.param("pose_sensor/init/p_ic/z", p_ic[2], 0.0);
@@ -205,7 +214,7 @@ class PoseSensorManager : public msf_core::MSF_SensorManagerROS<
     p = p_wv + q_wv.conjugate().toRotationMatrix() * p_vc / scale
         - q.toRotationMatrix() * p_ic;
 
-    a_m = q.inverse() * g;			/// Initial acceleration.
+    a_m = (q.inverse() * g) - b_a;			/// De-biased initial acceleration.
 
     // Prepare init "measurement"
     // True means that this message contains initial sensor readings.

msf_updates/src/pose_pressure_msf/pose_pressure_sensormanager.h

@@ -28,6 +28,9 @@
 #include <msf_updates/PosePressureSensorConfig.h>
 #include <msf_updates/SinglePoseSensorConfig.h>
 
+#include "sensor_fusion_comm/InitScale.h"
+
+
 namespace msf_pose_pressure_sensor {
 
 typedef msf_updates::PosePressureSensorConfig Config_T;
@@ -64,6 +67,9 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
     ReconfigureServer::CallbackType f = boost::bind(
         &PosePressureSensorManager::Config, this, _1, _2);
     reconf_server_->setCallback(f);
+
+    init_scale_srv_ = pnh.advertiseService("initialize_msf_scale",
+                                           &PosePressureSensorManager::InitScale, this);
   }
 
   virtual ~PosePressureSensorManager() {
@@ -80,6 +86,7 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
 
   Config_T config_;
   ReconfigureServerPtr reconf_server_;
+  ros::ServiceServer init_scale_srv_;
 
   /**
    * \brief Dynamic reconfigure callback.
@@ -112,6 +119,14 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
     pressure_handler_->SetNoises(config.press_noise_meas_p);
   }
 
+  bool InitScale(sensor_fusion_comm::InitScale::Request &req,
+                 sensor_fusion_comm::InitScale::Response &res) {
+    ROS_INFO("Initialize filter with scale %f", req.scale);
+    Init(req.scale);
+    res.result = "Initialized scale";
+    return true;
+  }
+
   void Init(double scale) const {
     Eigen::Matrix<double, 3, 1> p, v, b_w, b_a, g, w_m, a_m, p_ic, p_vc;
     Eigen::Quaternion<double> q, q_wv, q_ic, q_vc;
@@ -120,8 +135,6 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
 
     // init values
     g << 0, 0, 9.81;	/// Gravity.
-    b_w << 0, 0, 0;		/// Bias gyroscopes.
-    b_a << 0, 0, 0;		/// Bias accelerometer.
 
     v << 0, 0, 0;			/// Robot velocity (IMU centered).
     w_m << 0, 0, 0;		/// Initial angular velocity.
@@ -145,6 +158,17 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
           "using [0 0 0] and [1 0 0 0] respectively");
 
     ros::NodeHandle pnh("~");
+    pnh.param("core/init/b_w/x", b_w[0], 0.0);
+    pnh.param("core/init/b_w/y", b_w[1], 0.0);
+    pnh.param("core/init/b_w/z", b_w[2], 0.0);
+
+    pnh.param("core/init/b_a/x", b_a[0], 0.0);
+    pnh.param("core/init/b_a/y", b_a[1], 0.0);
+    pnh.param("core/init/b_a/z", b_a[2], 0.0);
+
+    MSF_INFO_STREAM("b_a: " << b_a.transpose());
+    MSF_INFO_STREAM("b_w: " << b_w.transpose());
+
     pnh.param("pose_sensor/init/p_ic/x", p_ic[0], 0.0);
     pnh.param("pose_sensor/init/p_ic/y", p_ic[1], 0.0);
     pnh.param("pose_sensor/init/p_ic/z", p_ic[2], 0.0);
@@ -165,7 +189,7 @@ class PosePressureSensorManager : public msf_core::MSF_SensorManagerROS<
     p = q_wv.conjugate().toRotationMatrix() * p_vc / scale
         - q.toRotationMatrix() * p_ic;
 
-    a_m = q.inverse() * g;			/// Initial acceleration.
+    a_m = (q.inverse() * g) - b_a;			/// De-biased initial acceleration.
 
     // Prepare init "measurement".
     // True menas that we will also set the initial sensor readings.

msf_updates/src/position_msf/position_sensormanager.h

@@ -29,6 +29,9 @@
 #include <msf_updates/position_sensor_handler/position_measurement.h>
 #include <msf_updates/SinglePositionSensorConfig.h>
 
+#include "sensor_fusion_comm/InitScale.h"
+
+
 namespace msf_position_sensor {
 
 typedef msf_updates::SinglePositionSensorConfig Config_T;
@@ -62,6 +65,9 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
     ReconfigureServer::CallbackType f = boost::bind(
         &PositionSensorManager::Config, this, _1, _2);
     reconf_server_->setCallback(f);
+
+    init_scale_srv_ = pnh.advertiseService("initialize_msf_scale",
+                                           &PositionSensorManager::InitScale, this);
   }
   virtual ~PositionSensorManager() {
   }
@@ -76,6 +82,7 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
 
   Config_T config_;
   ReconfigureServerPtr reconf_server_;
+  ros::ServiceServer init_scale_srv_;
 
   /**
    * \brief Dynamic reconfigure callback.
@@ -91,6 +98,14 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
     }
   }
 
+  bool InitScale(sensor_fusion_comm::InitScale::Request &req,
+                 sensor_fusion_comm::InitScale::Response &res) {
+    ROS_INFO("Initialize filter with scale %f", req.scale);
+    Init(req.scale);
+    res.result = "Initialized scale";
+    return true;
+  }
+
   void Init(double scale) const {
     if (scale < 0.001) {
       MSF_WARN_STREAM("init scale is "<<scale<<" correcting to 1");
@@ -103,8 +118,6 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
 
     // Init values.
     g << 0, 0, 9.81;  /// Gravity.
-    b_w << 0, 0, 0;		/// Bias gyroscopes.
-    b_a << 0, 0, 0;		/// Bias accelerometer.
 
     v << 0, 0, 0;			/// Robot velocity (IMU centered).
     w_m << 0, 0, 0;		/// Initial angular velocity.
@@ -131,14 +144,25 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
           "No measurements received yet to initialize position - using [0 0 0]");
 
     ros::NodeHandle pnh("~");
+    pnh.param("core/init/b_w/x", b_w[0], 0.0);
+    pnh.param("core/init/b_w/y", b_w[1], 0.0);
+    pnh.param("core/init/b_w/z", b_w[2], 0.0);
+
+    pnh.param("core/init/b_a/x", b_a[0], 0.0);
+    pnh.param("core/init/b_a/y", b_a[1], 0.0);
+    pnh.param("core/init/b_a/z", b_a[2], 0.0);
+
+    MSF_INFO_STREAM("b_a: " << b_a.transpose());
+    MSF_INFO_STREAM("b_w: " << b_w.transpose());
+
     pnh.param("position_sensor/init/p_ip/x", p_ip[0], 0.0);
     pnh.param("position_sensor/init/p_ip/y", p_ip[1], 0.0);
     pnh.param("position_sensor/init/p_ip/z", p_ip[2], 0.0);
 
     // Calculate initial attitude and position based on sensor measurements.
     p = p_vc - q.toRotationMatrix() * p_ip;
 
-    a_m = q.inverse() * g;			    /// Initial acceleration.
+    a_m = (q.inverse() * g) - b_a;			    /// Initial acceleration.
 
     //prepare init "measurement"
     // True means that we will also set the initialsensor readings.