gpst timestamp

include/gnss/capture/capture_gnss_fix.h

@@ -18,6 +18,7 @@ class CaptureGnssFix : public CaptureBase
     protected:
         Eigen::VectorXs data_; ///< Raw data.
         Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+        TimeStamp ts_GPST_; ///< Time stamp in GPS time
 
     public:
         CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
@@ -25,6 +26,14 @@ class CaptureGnssFix : public CaptureBase
         const Eigen::VectorXs& getData() const;
         const Eigen::MatrixXs& getDataCovariance() const;
         void getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const;
+        const TimeStamp& getGpsTimeStamp() const
+        {
+            return ts_GPST_;
+        }
+        void setGpsTimeStamp(const TimeStamp& _ts_GPST)
+        {
+            ts_GPST_ = _ts_GPST;
+        }
 };
 
 inline const Eigen::VectorXs& CaptureGnssFix::getData() const

include/gnss/capture/capture_gnss_single_diff.h

@@ -19,6 +19,7 @@ class CaptureGnssSingleDiff : public CaptureBase
         Eigen::Vector3s data_; ///< Raw data.
         Eigen::Matrix3s data_covariance_; ///< Noise of the capture.
         FrameBasePtr    origin_frame_ptr_;
+        TimeStamp ts_GPST_; ///< Time stamp in GPS time
 
     public:
         CaptureGnssSingleDiff(const TimeStamp& _ts,
@@ -36,6 +37,14 @@ class CaptureGnssSingleDiff : public CaptureBase
         const Eigen::Matrix3s& getDataCovariance() const;
         void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const;
         FrameBasePtr getOriginFrame() const;
+        const TimeStamp& getGpsTimeStamp() const
+        {
+            return ts_GPST_;
+        }
+        void setGpsTimeStamp(const TimeStamp& _ts_GPST)
+        {
+            ts_GPST_ = _ts_GPST;
+        }
 };
 
 inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const

include/gnss/sensor/sensor_gnss.h

@@ -144,22 +144,22 @@ inline const Eigen::Vector3s& SensorGnss::gettEnuEcef() const
 
 inline Eigen::Vector3s SensorGnss::gettEnuMap() const
 {
-    return getStateBlockPtrStatic(3)->getState();
+    return getEnuMapTranslation()->getState();
 }
 
 template<typename T>
 inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
 {
-    Eigen::Matrix<T,3,3> R_ENU_MAP = (Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX())
-                                    * Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY())
-                                    * Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ())).toRotationMatrix();
-
-    return R_ENU_MAP;
+    return Eigen::Matrix<T,3,3>(Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX()) *
+                                Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY()) *
+                                Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ()));
+}
 
-    // TODO: store R's when T=Scalar
-    //Eigen::Matrix<T,3,3> R_roll_T, R_pitch_T, R_yaw_T;
-    //if (!getRollState()->isFixed())
-    //    *R_roll_ptr_ = Eigen::AngleAxis<Scalar>(0.25*M_PI, Eigen::Vector3s::UnitX());
+inline Eigen::Matrix3s SensorGnss::getREnuMap() const
+{
+    return computeREnuMap(getEnuMapRoll() ->getState()(0),
+                          getEnuMapPitch()->getState()(0),
+                          getEnuMapYaw()  ->getState()(0));
 }
 
 } // namespace wolf

src/sensor/sensor_gnss.cpp

@@ -245,13 +245,6 @@ void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
     R_ENU_MAP_initialized_ = true;
 }
 
-Eigen::Matrix3s SensorGnss::getREnuMap() const
-{
-    return Eigen::Matrix3s(Eigen::AngleAxis<Scalar>(getEnuMapRoll() ->getState()(0), Eigen::Vector3s::UnitX()) *
-                           Eigen::AngleAxis<Scalar>(getEnuMapPitch()->getState()(0), Eigen::Vector3s::UnitY()) *
-                           Eigen::AngleAxis<Scalar>(getEnuMapYaw()  ->getState()(0), Eigen::Vector3s::UnitZ()));
-}
-
 // Define the factory method
 SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
                               const IntrinsicsBasePtr _intrinsics)