Add odometry input to pose sensorhandler

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

@@ -87,8 +87,13 @@ 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_p_.setZero();
   z_q_.setIdentity();
 
   if (distortmeas) {
@@ -197,6 +202,38 @@ 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() << " ***");
+
+        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());
+
+        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) {

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