MapIV · rsasaki0109 · Jun 14, 2022 · May 12, 2022 · May 12, 2022 · Jun 2, 2022
diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt
@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(eagleye_navigation)
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
@@ -20,6 +23,7 @@ include_directories(
   include
   ${PROJECT_SOURCE_DIR}/include
   ${catkin_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIRS}
 )
 
 add_library(navigation
@@ -41,10 +45,12 @@ add_library(navigation
   src/rtk_heading.cpp
   src/enable_additional_rolling.cpp
   src/rolling.cpp
+  src/velocity_estimator.cpp
 )
 
 target_link_libraries(navigation
   ${catkin_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
 )
 add_dependencies(navigation ${catkin_EXPORTED_TARGETS})
 

diff --git a/eagleye_core/navigation/include/navigation/navigation.hpp b/eagleye_core/navigation/include/navigation/navigation.hpp
@@ -23,6 +23,8 @@
 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#include <yaml-cpp/yaml.h>
+
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/NavSatFix.h"
 #include "geometry_msgs/TwistStamped.h"
@@ -32,6 +34,8 @@
 
 #include "rtklib_msgs/RtklibNav.h"
 
+#include "eagleye_msgs/Status.h"
+#include "eagleye_msgs/StatusStamped.h"
 #include "eagleye_msgs/Distance.h"
 #include "eagleye_msgs/YawrateOffset.h"
 #include "eagleye_msgs/VelocityScaleFactor.h"
@@ -454,54 +458,235 @@ struct RollingStatus
 };
 
 extern void velocity_scale_factor_estimate(const rtklib_msgs::RtklibNav, const geometry_msgs::TwistStamped, const VelocityScaleFactorParameter,
-  VelocityScaleFactorStatus*, eagleye_msgs::VelocityScaleFactor*);
  VelocityScaleFactorStatus*, geometry_msgs::TwistStamped*, eagleye_msgs::VelocityScaleFactor*);
 extern void velocity_scale_factor_estimate(const nmea_msgs::Gprmc, const geometry_msgs::TwistStamped, const VelocityScaleFactorParameter,
-  VelocityScaleFactorStatus*, eagleye_msgs::VelocityScaleFactor*);
-extern void distance_estimate(const eagleye_msgs::VelocityScaleFactor, DistanceStatus*,eagleye_msgs::Distance*);
+  VelocityScaleFactorStatus*, geometry_msgs::TwistStamped*, eagleye_msgs::VelocityScaleFactor*);
+extern void distance_estimate(const geometry_msgs::TwistStamped, DistanceStatus*,eagleye_msgs::Distance*);
 extern void yawrate_offset_stop_estimate(const geometry_msgs::TwistStamped, const sensor_msgs::Imu, const YawrateOffsetStopParameter,
   YawrateOffsetStopStatus*, eagleye_msgs::YawrateOffset*);
-extern void yawrate_offset_estimate(const eagleye_msgs::VelocityScaleFactor, const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,
+extern void yawrate_offset_estimate(const geometry_msgs::TwistStamped, const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,
   const sensor_msgs::Imu, const YawrateOffsetParameter, YawrateOffsetStatus*, eagleye_msgs::YawrateOffset*);
-extern void heading_estimate(const rtklib_msgs::RtklibNav, const sensor_msgs::Imu, const eagleye_msgs::VelocityScaleFactor,
+extern void heading_estimate(const rtklib_msgs::RtklibNav, const sensor_msgs::Imu, const geometry_msgs::TwistStamped,
   const eagleye_msgs::YawrateOffset, const eagleye_msgs::YawrateOffset,  const eagleye_msgs::SlipAngle, const eagleye_msgs::Heading, const HeadingParameter,
   HeadingStatus*,eagleye_msgs::Heading*);
-extern void heading_estimate(const nmea_msgs::Gprmc, const sensor_msgs::Imu, const eagleye_msgs::VelocityScaleFactor, const eagleye_msgs::YawrateOffset,
+extern void heading_estimate(const nmea_msgs::Gprmc, const sensor_msgs::Imu, const geometry_msgs::TwistStamped, const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,  const eagleye_msgs::SlipAngle, const eagleye_msgs::Heading, const HeadingParameter, HeadingStatus*,eagleye_msgs::Heading*);
-extern void position_estimate(const rtklib_msgs::RtklibNav, const eagleye_msgs::VelocityScaleFactor, const eagleye_msgs::Distance,
+extern void position_estimate(const rtklib_msgs::RtklibNav, const geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped, const eagleye_msgs::Distance,
   const eagleye_msgs::Heading, const geometry_msgs::Vector3Stamped, const PositionParameter, PositionStatus*, eagleye_msgs::Position*);
-extern void position_estimate(const nmea_msgs::Gpgga gga, const eagleye_msgs::VelocityScaleFactor, const eagleye_msgs::Distance, const eagleye_msgs::Heading,
+extern void position_estimate(const nmea_msgs::Gpgga gga, const geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped, const eagleye_msgs::Distance, const eagleye_msgs::Heading,
   const geometry_msgs::Vector3Stamped, const PositionParameter, PositionStatus*, eagleye_msgs::Position*);
-extern void slip_angle_estimate(const sensor_msgs::Imu,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::YawrateOffset,
+extern void slip_angle_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const SlipangleParameter,eagleye_msgs::SlipAngle*);
-extern void slip_coefficient_estimate(const sensor_msgs::Imu,const rtklib_msgs::RtklibNav,const eagleye_msgs::VelocityScaleFactor,
+extern void slip_coefficient_estimate(const sensor_msgs::Imu,const rtklib_msgs::RtklibNav,const geometry_msgs::TwistStamped,
   const eagleye_msgs::YawrateOffset,const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,const SlipCoefficientParameter,SlipCoefficientStatus*,double*);
-extern void smoothing_estimate(const rtklib_msgs::RtklibNav,const eagleye_msgs::VelocityScaleFactor,const SmoothingParameter,SmoothingStatus*,
+extern void smoothing_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs::TwistStamped,const SmoothingParameter,SmoothingStatus*,
   eagleye_msgs::Position*);
-extern void trajectory_estimate(const sensor_msgs::Imu,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::Heading,const eagleye_msgs::YawrateOffset,
+extern void trajectory_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*,geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,
   geometry_msgs::TwistStamped*);
-extern void heading_interpolate_estimate(const sensor_msgs::Imu,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::YawrateOffset,
+extern void heading_interpolate_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,const eagleye_msgs::SlipAngle,const HeadingInterpolateParameter,HeadingInterpolateStatus*,
   eagleye_msgs::Heading*);
 extern void position_interpolate_estimate(const eagleye_msgs::Position,const geometry_msgs::Vector3Stamped,const eagleye_msgs::Position,
   const eagleye_msgs::Height,const PositionInterpolateParameter,PositionInterpolateStatus*,eagleye_msgs::Position*,sensor_msgs::NavSatFix*);
-extern void pitching_estimate(const sensor_msgs::Imu,const nmea_msgs::Gpgga,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::Distance,
+extern void pitching_estimate(const sensor_msgs::Imu,const nmea_msgs::Gpgga,const geometry_msgs::TwistStamped,const eagleye_msgs::Distance,
   const HeightParameter,HeightStatus*,eagleye_msgs::Height*,eagleye_msgs::Pitching*,eagleye_msgs::AccXOffset*,eagleye_msgs::AccXScaleFactor*);
-extern void trajectory3d_estimate(const sensor_msgs::Imu,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::Heading,
-  const eagleye_msgs::YawrateOffset,const eagleye_msgs::YawrateOffset,const eagleye_msgs::Pitching,const TrajectoryParameter,TrajectoryStatus*,geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*);
+extern void trajectory3d_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading,
+  const eagleye_msgs::YawrateOffset,const eagleye_msgs::YawrateOffset,const eagleye_msgs::Pitching,const TrajectoryParameter,TrajectoryStatus*,
+  geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*);
 extern void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped, const sensor_msgs::Imu, const AngularVelocityOffsetStopParameter,
   AngularVelocityOffsetStopStatus*, eagleye_msgs::AngularVelocityOffset*);
 extern void rtk_deadreckoning_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs::Vector3Stamped,const nmea_msgs::Gpgga, const eagleye_msgs::Heading,
   const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::Position*,sensor_msgs::NavSatFix*);
 extern void rtk_deadreckoning_estimate(const geometry_msgs::Vector3Stamped,const nmea_msgs::Gpgga, const eagleye_msgs::Heading,
   const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::Position*,sensor_msgs::NavSatFix*);
-extern void rtk_heading_estimate(const nmea_msgs::Gpgga gga, const sensor_msgs::Imu, const eagleye_msgs::VelocityScaleFactor, const eagleye_msgs::Distance,
+extern void rtk_heading_estimate(const nmea_msgs::Gpgga gga, const sensor_msgs::Imu, const geometry_msgs::TwistStamped, const eagleye_msgs::Distance,
   const eagleye_msgs::YawrateOffset, const eagleye_msgs::YawrateOffset,  const eagleye_msgs::SlipAngle, const eagleye_msgs::Heading, const RtkHeadingParameter,
   RtkHeadingStatus*,eagleye_msgs::Heading*);
-extern void enable_additional_rolling_estimate(const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::YawrateOffset ,const eagleye_msgs::YawrateOffset,
+extern void enable_additional_rolling_estimate(const geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped,const eagleye_msgs::YawrateOffset ,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::Distance,const sensor_msgs::Imu,const geometry_msgs::PoseStamped,const eagleye_msgs::AngularVelocityOffset,
   const EnableAdditionalRollingParameter,EnableAdditionalRollingStatus*,eagleye_msgs::Rolling*,eagleye_msgs::AccYOffset*);
-extern void rolling_estimate(const sensor_msgs::Imu,const eagleye_msgs::VelocityScaleFactor,const eagleye_msgs::YawrateOffset,
+extern void rolling_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::YawrateOffset,
   const eagleye_msgs::YawrateOffset,const RollingParameter,RollingStatus*,eagleye_msgs::Rolling*);
 
+
+class VelocityEstimator
+{
+  public:
+    VelocityEstimator();
+
+    // Parameter setting
+    void setParam(std::string yaml_file);
+
+    // Main estimate function
+    void VelocityEstimate(const sensor_msgs::Imu, const rtklib_msgs::RtklibNav, const nmea_msgs::Gpgga, geometry_msgs::TwistStamped*);
+
+    eagleye_msgs::Status getStatus();
+
+  private:
+
+    class PitchrateOffsetStopEstimator
+    {
+      public:
+        PitchrateOffsetStopEstimator();
+
+        void setParam(std::string yaml_file);
+        bool PitchrateOffsetStopEstimate(double pitchrate, double stop_status);
+
+        double pitchrate_offset;
+        eagleye_msgs::Status pitchrate_offset_status;
+
+      private:
+        struct Param
+        {
+          int buffer_count_max;
+        };
+        PitchrateOffsetStopEstimator::Param param;
+
+        int stop_count;
+        std::vector<double> pitchrate_buffer;
+    };
+
+    class PitchingEstimator
+    {
+      public:
+        PitchingEstimator();
+
+        void setParam(std::string yaml_file);
+        bool PitchingEstimate(double imu_time_last, double doppler_velocity, double rtkfix_velocity,
+                              double pitchrate, double pitchrate_offset, double rtkfix_pitching,
+                              bool navsat_update_status, bool stop_status);
+
+        double pitching;
+        eagleye_msgs::Status pitching_status;
+
+      private:
+        struct Param
+        {
+          int buffer_max;
+          double outlier_threshold;
+          double estimated_velocity_threshold;
+          double estimated_gnss_coefficient;
+          double estimated_coefficient;
+        };
+        PitchingEstimator::Param param;
+
+        std::vector<double> time_buffer;
+        std::vector<double> corrected_pitchrate_buffer;
+        std::vector<double> rtkfix_pitching_buffer;
+        std::vector<bool> use_gnss_status_buffer;
+        std::vector<bool> navsat_update_status_buffer;
+    };
+
+    class AccelerationOffsetEstimator
+    {
+      public:
+        AccelerationOffsetEstimator();
+
+        void setParam(std::string yaml_file);
+        bool AccelerationOffsetEstimate(double imu_time_last, double rtkfix_velocity, double pitching,
+                              double acceleration, bool navsat_update_status);
+
+        double filtered_acceleration;
+        double acceleration_offset;
+        eagleye_msgs::Status acceleration_offset_status;
+
+      private:
+        struct Param
+        {
+          double buffer_min;
+          double buffer_max;
+          double filter_process_noise;
+          double filter_observation_noise;
+        };
+        AccelerationOffsetEstimator::Param param;
+
+        double acceleration_last;
+        double acceleration_variance_last;
+        std::vector<double> time_buffer;
+        std::vector<double> pitching_buffer; 
+        std::vector<double> filtered_acceleration_buffer;
+        std::vector<double> rtkfix_velocity_buffer; 
+        std::vector<double> navsat_update_status_buffer;
+    };
+
+    // Parameter
+    struct Param
+    {
+      double ecef_base_pos_x;
+      double ecef_base_pos_y;
+      double ecef_base_pos_z;
+      bool use_ecef_base_position;
+
+      double gga_downsample_time;
+      double stop_judgment_velocity_threshold;
+      double stop_judgment_buffer_maxnum;
+      double variance_threshold;
+
+      // doppler fusion parameter
+      double buffer_max;
+      double estimated_gnss_coefficient;
+      double estimated_coefficient;
+      double outlier_threshold;
+    };
+    VelocityEstimator::Param param;
+
+    // imu variables
+    double acceleration;
+    double pitchrate;
+    double imu_time_last;
+
+    // rtklib_nav variables
+    double doppler_velocity;
+    double rtklib_nav_time_last;
+    bool rtklib_update_status;
+
+    // gga variables
+    double ecef_base_position[3]; 
+    bool ecef_base_position_status;
+    double gga_time_last;
+    double gga_position_enu_last[3]; 
+    int gga_status_last;
+    double rtkfix_velocity; 
+    double rtkfix_pitching; 
+    bool navsat_update_status;
+
+    // stop judgment variables
+    bool stop_status;
+    std::vector<double> angular_velocity_x_buffer;
+    std::vector<double> angular_velocity_y_buffer;
+    std::vector<double> angular_velocity_z_buffer;
+
+    // // PitchrateOffsetStopEstimator variables
+    PitchrateOffsetStopEstimator pitchrate_offset_stop_estimator;
+    double pitchrate_offset;
+    eagleye_msgs::Status pitchrate_offset_status;
+
+    // PitchingEstimator variables
+    PitchingEstimator pitching_estimator;
+    double pitching;
+    eagleye_msgs::Status pitching_status;
+
+    // AccelerationOffsetEstimator
+    AccelerationOffsetEstimator acceleration_offset_estimator;    
+    double filtered_acceleration;
+    double acceleration_offset;
+    eagleye_msgs::Status acceleration_offset_status;
+
+    //DopplerFusion
+    double velocity;
+    std::vector<double> time_buffer;
+    std::vector<double> doppler_velocity_buffer;
+    std::vector<double> corrected_acceleration_buffer;
+    std::vector<double> rtklib_update_status_buffer;
+    eagleye_msgs::Status velocity_status;
+
+    bool updateImu(const sensor_msgs::Imu);
+    bool updateRtklibNav(const rtklib_msgs::RtklibNav);
+    bool updateGGA(const nmea_msgs::Gpgga);
+    bool StopJudgment(const sensor_msgs::Imu);
+    bool DopplerFusion();
+};
+
+
 #endif /*NAVIGATION_H */
diff --git a/eagleye_core/navigation/src/distance.cpp b/eagleye_core/navigation/src/distance.cpp
@@ -31,17 +31,17 @@
  #include "coordinate/coordinate.hpp"
  #include "navigation/navigation.hpp"
 
-void distance_estimate(const eagleye_msgs::VelocityScaleFactor velocity_scale_factor, DistanceStatus* distance_status,eagleye_msgs::Distance* distance)
+void distance_estimate(const geometry_msgs::TwistStamped velocity, DistanceStatus* distance_status,eagleye_msgs::Distance* distance)
 {
   if(distance_status->time_last != 0)
   {
-    distance->distance = distance->distance + velocity_scale_factor.correction_velocity.linear.x * std::abs((velocity_scale_factor.header.stamp.toSec() -
+    distance->distance = distance->distance + velocity.twist.linear.x * std::abs((velocity.header.stamp.toSec() -
       distance_status->time_last));
     distance->status.enabled_status = distance->status.estimate_status = true;
-    distance_status->time_last = velocity_scale_factor.header.stamp.toSec();
+    distance_status->time_last = velocity.header.stamp.toSec();
   }
   else
   {
-    distance_status->time_last = velocity_scale_factor.header.stamp.toSec();
+    distance_status->time_last = velocity.header.stamp.toSec();
   }
 }