Develop ros1 (#185)

* Fix canless mode flag * FIx heading parameter in eagleye_pp_config.yaml * Fix rmc config (#155) * Add canless usage in README.md (#156) * Add rtk_deadreckoning emergency (#157) * Feature/only fixed pose publish (#158) * Add fix onu publish mode in fix2pose * Fix nullptr access * Add TF base_link2gnss to eagleye_pp (#162) * Fixed problem with callback function not defined in main statement (#164) * Add esitimated twist outlier response (#161) * Revert "Add esitimated twist outlier response (#161)" (#167) This reverts commit ae0b78e. * [WIP]Retain previous value if VSF/YO is abnormal (#165) * Retain previous value if VSF is abnormal * Add is_abnormal in eagleye_msgs/status * Add is_abnormal in eagleye_msgs * Introduction of error codes * Remove th_velocity_scale_factor_percent in monitor_node * If yawrate_offset_stop is unestimated, no heading estimation (#169) * If yawrate_offset_stop is unestimated, no heading estimation * Add warning of uncompleted stop calibration in heading_node.cpp * Feature/parameter rename (#171) * parameter rename * add param description * add parameter description * fix parameter * fix param * modification for CI * Fix problem of not starting up * Feature/double antenna (#166) * Refactor heading_interporate.cpp * Add heading_estimate of multiantenna in heading.cpp * Add eagleye_rt_dualantenna.launch * Refactor heading.cpp and yawrate_offset.cpp * Settings for autoware * Add fix onu publish mode in fix2pose * Fix tf in fix2pose * Fix tf settiong in fix2pose * Fix tf-conversion * Output heading without storing buffer in multi-antenna mode. * Undo parameters from for autoware * Make it easier to set input GNSS topics * Fix setting of input GNSS topics * Fix namespace * Fix conflict * Initialization of heading_estimate_status * Change email address in Contacts in README.md * Fix/yaml cpp for colcon (#180) * fix CMakeLists about yaml-cpp for colcon * Update submodule Co-authored-by: autoware <[email protected]> * Add yaml_file param * Fixed bug with wrong variables Co-authored-by: YutaHoda <[email protected]> Co-authored-by: YutaHoda <[email protected]> Co-authored-by: Aoki-Takanose <[email protected]>
MapIV · Nov 11, 2022 · 78bc8aa · 78bc8aa
1 parent 1e7b304
commit 78bc8aa
Show file tree

Hide file tree

Showing 66 changed files with 2,450 additions and 1,032 deletions.
diff --git a/README.md b/README.md
@@ -123,6 +123,21 @@ Clone and build the necessary packages for Eagleye.
 
 - [eagleye_pp](eagleye_pp) : post-processing version
 
+## Optional Features
+
+### canless mode
+
+While normal eagleye requires CAN (wheel speed) information, this option allows localization estimation with GNSS/IMU alone, without vehicle speed.
+
+However, in this mode, note the following
+- Accuracy degrades in non-open sky environments.
+- RTK positioning is required.
+- Localization estimating is not possible when driving backward.
+
+To use this mode
+- Use launch/eagleye_rt_canless.launch for eagleye_rt instead of launch/eagleye_rt.launch.
+- Set use_canless_mode in eagleye_pp_config.yaml to true for eagleye_pp.
+
 ## Sample data
 ### ROSBAG
 
@@ -155,4 +170,4 @@ Eagleye is provided under the [BSD 3-Clause](https://github.com/MapIV/eagleye/bl
 
 ## Contacts
 
-If you have further question, email to map4@tier4.jp.
+If you have further question, email to support@map4.jp.
diff --git a/docs/parameter_overview.png b/docs/parameter_overview.png
diff --git a/eagleye_core/navigation/include/navigation/navigation.hpp b/eagleye_core/navigation/include/navigation/navigation.hpp
@@ -58,10 +58,12 @@
 
 struct VelocityScaleFactorParameter
 {
-  double estimated_number_min;
-  double estimated_number_max;
-  double estimated_velocity_threshold;
-  double estimated_coefficient;
+  double imu_rate;
+  double gnss_rate;
+  double moving_judgment_threshold;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double gnss_receiving_threshold;
   bool save_velocity_scale_factor{false};
 };
 
@@ -83,8 +85,9 @@ struct DistanceStatus
 
 struct YawrateOffsetStopParameter
 {
-  double stop_judgment_velocity_threshold;
-  double estimated_number;
+  double imu_rate;
+  double estimated_interval;
+  double stop_judgment_threshold;
   double outlier_threshold;
 };
 
@@ -98,10 +101,12 @@ struct YawrateOffsetStopStatus
 
 struct YawrateOffsetParameter
 {
-  double estimated_number_min;
-  double estimated_number_max;
-  double estimated_coefficient;
-  double estimated_velocity_threshold;
+  double imu_rate;
+  double gnss_rate;
+  double moving_judgment_threshold;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double gnss_receiving_threshold;
   double outlier_threshold;
 };
 
@@ -121,21 +126,23 @@ struct YawrateOffsetStatus
 
 struct HeadingParameter
 {
-  double estimated_number_min;
-  double estimated_number_max;
-  double estimated_gnss_coefficient;
-  double estimated_heading_coefficient;
+  double imu_rate;
+  double gnss_rate;
+  double stop_judgment_threshold;
+  double moving_judgment_threshold;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double gnss_receiving_threshold;
   double outlier_threshold;
-  double estimated_velocity_threshold;
-  double stop_judgment_velocity_threshold;
-  double estimated_yawrate_threshold;
-
+  double outlier_ratio_threshold;
+  double curve_judgment_threshold;
 };
 
 struct HeadingStatus
 {
   int tow_last;
   double rmc_time_last;
+  double ros_time_last;
   int estimated_number;
   std::vector<double> time_buffer;
   std::vector<double> heading_angle_buffer;
@@ -149,17 +156,17 @@ struct HeadingStatus
 
 struct RtkHeadingParameter
 {
-  double estimated_distance;
-  double estimated_heading_buffer_min;
-  double estimated_number_min;
-  double estimated_number_max;
-  double estimated_gnss_coefficient;
-  double estimated_heading_coefficient;
+  double imu_rate;
+  double gnss_rate;
+  double stop_judgment_threshold;
+  double slow_judgment_threshold;
+  double update_distance;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double gnss_receiving_threshold;
   double outlier_threshold;
-  double estimated_velocity_threshold;
-  double stop_judgment_velocity_threshold;
-  double estimated_yawrate_threshold;
-
+  double outlier_ratio_threshold;
+  double curve_judgment_threshold;
 };
 
 struct RtkHeadingStatus
@@ -184,8 +191,9 @@ struct RtkHeadingStatus
 
 struct HeadingInterpolateParameter
 {
-  double stop_judgment_velocity_threshold;
-  double number_buffer_max;
+  double imu_rate;
+  double stop_judgment_threshold;
+  double sync_search_period;
 };
 
 struct HeadingInterpolateStatus
@@ -202,12 +210,6 @@ struct HeadingInterpolateStatus
 
 struct PositionParameter
 {
-  double estimated_distance;
-  double separation_distance;
-  double estimated_velocity_threshold;
-  double outlier_threshold;
-  double estimated_enu_vel_coefficient;
-  double estimated_position_coefficient;
   double ecef_base_pos_x;
   double ecef_base_pos_y;
   double ecef_base_pos_z;
@@ -220,6 +222,15 @@ struct PositionParameter
   double tf_gnss_rotation_w;
   std::string tf_gnss_parent_frame;
   std::string tf_gnss_child_frame;
+
+  double imu_rate;
+  double gnss_rate;
+  double moving_judgment_threshold;
+  double estimated_interval;
+  double update_distance;
+  double gnss_receiving_threshold;
+  double outlier_threshold;
+  double outlier_ratio_threshold;
 };
 
 struct PositionStatus
@@ -241,8 +252,9 @@ struct PositionStatus
 
 struct PositionInterpolateParameter
 {
-  double number_buffer_max;
-  double stop_judgment_velocity_threshold;
+  double imu_rate;
+  double stop_judgment_threshold;
+  double sync_search_period;
 };
 
 struct PositionInterpolateStatus
@@ -263,18 +275,19 @@ struct PositionInterpolateStatus
 
 struct SlipangleParameter
 {
-  double stop_judgment_velocity_threshold;
   double manual_coefficient;
+  double stop_judgment_threshold;
 };
 
 struct SlipCoefficientParameter
 {
-  double estimated_number_min;
-  double estimated_number_max;
-  double estimated_velocity_threshold;
-  double estimated_yawrate_threshold;
+  double imu_rate;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double stop_judgment_threshold;
+  double moving_judgment_threshold;
+  double curve_judgment_threshold;
   double lever_arm;
-  double stop_judgment_velocity_threshold;
 };
 
 struct SlipCoefficientStatus
@@ -286,12 +299,13 @@ struct SlipCoefficientStatus
 
 struct SmoothingParameter
 {
+  double gnss_rate;
+  double moving_judgment_threshold;
+  double moving_average_time;
+  double moving_ratio_threshold;
   double ecef_base_pos_x;
   double ecef_base_pos_y;
   double ecef_base_pos_z;
-  int estimated_number_max;
-  double estimated_velocity_threshold;
-  double estimated_threshold;
 };
 
 struct SmoothingStatus
@@ -305,8 +319,8 @@ struct SmoothingStatus
 
 struct TrajectoryParameter
 {
-  double stop_judgment_velocity_threshold;
-  double stop_judgment_yawrate_threshold;
+  double stop_judgment_threshold;
+  double curve_judgment_threshold;
 };
 
 struct TrajectoryStatus
@@ -318,14 +332,16 @@ struct TrajectoryStatus
 
 struct HeightParameter
 {
-  double estimated_distance;
-  double estimated_distance_max;
-  double separation_distance;
-  double estimated_velocity_threshold;
-  double estimated_velocity_coefficient;
-  double estimated_height_coefficient;
+  double imu_rate;
+  double gnss_rate;
+  double moving_judgment_threshold;
+  double estimated_minimum_interval;
+  double estimated_maximum_interval;
+  double update_distance;
+  double gnss_receiving_threshold;
   double outlier_threshold;
-  int average_num;
+  double outlier_ratio_threshold;
+  double moving_average_time;
 };
 
 struct HeightStatus
@@ -360,9 +376,11 @@ struct HeightStatus
 
 struct AngularVelocityOffsetStopParameter
 {
-  double stop_judgment_velocity_threshold;
-  double estimated_number;
+  double imu_rate;
+  double estimated_interval;
+  double stop_judgment_threshold;
   double outlier_threshold;
+
 };
 
 struct AngularVelocityOffsetStopStatus
@@ -379,7 +397,7 @@ struct AngularVelocityOffsetStopStatus
 
 struct RtkDeadreckoningParameter
 {
-  double stop_judgment_velocity_threshold;
+  double stop_judgment_threshold;
   double ecef_base_pos_x;
   double ecef_base_pos_y;
   double ecef_base_pos_z;
@@ -414,11 +432,12 @@ struct RtkDeadreckoningStatus
 
 struct EnableAdditionalRollingParameter
 {
-  double matching_update_distance;
-  double stop_judgment_velocity_threshold;
-  double rolling_buffer_num;
-  double link_Time_stamp_parameter;
-  double imu_buffer_num;
+  double imu_rate;
+  double stop_judgment_threshold;
+  double update_distance;
+  double moving_average_time;
+  double sync_judgment_threshold;
+  double sync_search_period;
 };
 
 struct EnableAdditionalRollingStatus
@@ -444,7 +463,7 @@ struct EnableAdditionalRollingStatus
 
 struct RollingParameter
 {
-  double stop_judgment_velocity_threshold;
+  double stop_judgment_threshold;
   double filter_process_noise;
   double filter_observation_noise;
 };
@@ -471,6 +490,8 @@ extern void heading_estimate(const rtklib_msgs::RtklibNav, const sensor_msgs::Im
   HeadingStatus*,eagleye_msgs::Heading*);
 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 heading_estimate(const eagleye_msgs::Heading, 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 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 geometry_msgs::TwistStamped, const eagleye_msgs::StatusStamped, const eagleye_msgs::Distance, const eagleye_msgs::Heading,
@@ -539,6 +560,8 @@ class VelocityEstimator
       private:
         struct Param
         {
+          int imu_rate;
+          int estimated_interval;
           int buffer_count_max;
         };
         PitchrateOffsetStopEstimator::Param param;
@@ -563,10 +586,16 @@ class VelocityEstimator
       private:
         struct Param
         {
-          int buffer_max;
+          double imu_rate;
+          double gnss_rate;
+          double estimated_interval;
+          double buffer_max;
           double outlier_threshold;
           double estimated_velocity_threshold;
+          double slow_judgment_threshold;
+          double gnss_receiving_threshold;
           double estimated_gnss_coefficient;
+          double outlier_ratio_threshold;
           double estimated_coefficient;
         };
         PitchingEstimator::Param param;
@@ -594,6 +623,10 @@ class VelocityEstimator
       private:
         struct Param
         {
+          double imu_rate;
+          double gnss_rate;
+          double estimated_minimum_interval;
+          double estimated_maximum_interval;
           double buffer_min;
           double buffer_max;
           double filter_process_noise;
@@ -618,14 +651,21 @@ class VelocityEstimator
       double ecef_base_pos_z;
       bool use_ecef_base_position;
 
+      double imu_rate;
+      double gnss_rate;
+
       double gga_downsample_time;
+      double stop_judgment_interval;
       double stop_judgment_velocity_threshold;
       double stop_judgment_buffer_maxnum;
       double variance_threshold;
 
       // doppler fusion parameter
+      double estimated_interval;
       double buffer_max;
+      double gnss_receiving_threshold;
       double estimated_gnss_coefficient;
+      double outlier_ratio_threshold;
       double estimated_coefficient;
       double outlier_threshold;
     };
@@ -637,6 +677,7 @@ class VelocityEstimator
     double imu_time_last;
 
     // rtklib_nav variables
+    double gnss_rate;
     double doppler_velocity;
     double rtklib_nav_time_last;
     bool rtklib_update_status;

diff --git a/eagleye_core/navigation/package.xml b/eagleye_core/navigation/package.xml
@@ -30,6 +30,7 @@
   <exec_depend>rtklib_msgs</exec_depend>
   <exec_depend>eagleye_msgs</exec_depend>
   <exec_depend>eagleye_coordinate</exec_depend>
+  <depend>yaml-cpp</depend>
 
   <export>
   </export>