Develop ros2 (#270)

* Take a geoid model with relative paths (#218) Co-authored-by: Ryohei Sasaki <[email protected]> * Change in submodules installation method (#219) * Change in submodules installation method * Delete foxy CI * Param rename (#220) Co-authored-by: Ryohei Sasaki <[email protected]> * Update README.md * Fix canless launch (#225) * Fix canless launch * Remove submodule install in CI --------- Co-authored-by: Ryohei Sasaki <[email protected]> * Add Input gnss speed thereshold (#231) * Add Input gnss speed thereshold * Update twist_covariance_thresh description --------- Co-authored-by: Ryohei Sasaki <[email protected]> * Fix/ros2/multi antenna error (#232) * Add multi antenaa(WIP) * Simplyfy tf packages * Fix tf packages build error * Delete ekf_trigger * Add sub antennal in nmea_conveter * Fix param loading in gnss_conveter * Fix navsatfix input in multi antenna mode * Fixed a bug that caused yawrate_offset_stop to be incorrectly estimated. (#235) * Add meridional-aberration-angle correction (#236) * Bug fix in fis2pose (#239) * covariance output (#241) * Add checkout in CI(ROS2) (#243) * Add checkout in CI * remove unnecessary line break * Rename xml (#249) * Fix path error(ROS2) (#254) * Allow warnings to be output when fixes are inaccurate (#258) * Correcting incorrect warnings (#259) * fix tf2_geometry header (#265) * Add CONTRIBUTING.md --------- Co-authored-by: Aoki-Takanose <[email protected]>
MapIV · May 23, 2023 · 75e8fc2 · 75e8fc2
1 parent 898830f
commit 75e8fc2
Show file tree

Hide file tree

Showing 72 changed files with 1,792 additions and 1,722 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -17,9 +17,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v1
-        with:
-          submodules: true
-
       - name: rosdep update
         run: |
           apt-get update
@@ -32,52 +29,16 @@ jobs:
         run: |
           cd ~/eagleye/src/
           git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
-      - name: Install GeographicLib
-        run: |
-          apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc
-      - name: Build
-        run: |
-          cd ~/eagleye/
-          source /opt/ros/humble/setup.bash
-          rosdep install --from-paths src --ignore-src -r -y
-          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests
-
-  foxy_build:
-    runs-on:  ubuntu-20.04
-
-    container: ros:foxy
-    services:
-      ros:
-        image: ros
-
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - name: checkout
-        uses: actions/checkout@v1
-        with:
-          submodules: true
-
-      - name: rosdep update
-        run: |
-          apt-get update
-          rosdep update
-      - name: Create Workspace
-        run: |
-          mkdir -p ~/eagleye/src/
-          cp -r `pwd` ~/eagleye/src/
-      - name: Clone rtklib_msgs
+      - name: Clone llh_converter
         run: |
           cd ~/eagleye/src/
-          git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
+          git clone https://github.com/MapIV/llh_converter.git -b ros2
       - name: Install GeographicLib
         run: |
           apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc
       - name: Build
         run: |
           cd ~/eagleye/
-          source /opt/ros/foxy/setup.bash
+          source /opt/ros/humble/setup.bash
           rosdep install --from-paths src --ignore-src -r -y
           colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests
diff --git a/.gitmodules b/.gitmodules
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
@@ -1,15 +1,14 @@
 # Contributing to Eagleye
 ## Repository branch structure
-- main-ros1  
-This is the latest stable version of Eagleye ROS1. It is updated every three months.
-- **develop-ros1**  
-If contributors wish to add functionality to eagleye, they should create a new branch from this develop branch and submit a pull request.
+
 - main-ros2  
 This is the latest stable version of Eagleye ROS2. It is updated every three months. 
 - develop-ros2  
 This is a branch to reflect changes in develop-ros1 to ros2 as well.  
-MapIV develops eagleye functions mainly in ROS1, but if you are using eagleye in ROS2 and want to submit a PR,  
-please submit a PR in this branch.
+If contributors wish to add functionality to eagleye, they should create a new branch from this develop branch and submit a pull request.
+- main-ros1  
+This is the latest stable version of Eagleye ROS1. Public development at ros1 ended in May 2023.
+
 - feature/<feature_name>  
 Branches for new features and improvements
 - fix/<bug_name>  

diff --git a/README.md b/README.md
@@ -45,6 +45,7 @@ Clone and build the necessary packages for Eagleye. ([rtklib_ros_bridge](https:/
 	cd $HOME/catkin_ws/src
 	git clone https://github.com/MapIV/eagleye.git -b main-ros2 --recursive
 	git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
+	git clone https://github.com/MapIV/llh_converter.git -b ros2
 	git clone https://github.com/MapIV/nmea_ros_bridge.git -b ros2-v0.1.0
 	git clone https://github.com/MapIV/gnss_compass_ros.git -b main-ros2
 	sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc

diff --git a/eagleye_core/coordinate/CMakeLists.txt b/eagleye_core/coordinate/CMakeLists.txt
@@ -16,7 +16,7 @@ ament_auto_find_build_dependencies()
 find_package(PkgConfig)
 find_package(geodesy)
 find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
-  PAHT_SUFFIXES GeographicLib
+  PATH_SUFFIXES GeographicLib
 )
 set(GeographicLib_LIBRARIES
   NAMES Geographic

diff --git a/eagleye_core/coordinate/package.xml b/eagleye_core/coordinate/package.xml
@@ -14,6 +14,7 @@
   <build_export_depend>rclcpp</build_export_depend>
   <build_export_depend>geodesy</build_export_depend>
   <depend>geographic_info</depend>
+  <depend>geographic_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt
@@ -39,9 +39,9 @@ ament_auto_add_library(eagleye_navigation SHARED
   src/smoothing.cpp
   src/trajectory.cpp
   src/velocity_scale_factor.cpp
-  src/yawrate_offset_stop.cpp
-  src/yawrate_offset.cpp
-  src/rtk_deadreckoning.cpp
+  src/yaw_rate_offset_stop.cpp
+  src/yaw_rate_offset.cpp
+  src/rtk_dead_reckoning.cpp
   src/rtk_heading.cpp
   src/velocity_estimator.cpp
   include/eagleye_navigation/eagleye_navigation.hpp

diff --git a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp
@@ -55,7 +55,11 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
+#ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
 #include <math.h>
 #include <numeric>
 
@@ -102,9 +106,9 @@ struct YawrateOffsetStopParameter
 struct YawrateOffsetStopStatus
 {
   int stop_count;
-  double yawrate_offset_stop_last;
+  double yaw_rate_offset_stop_last;
   bool estimate_start_status;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> yaw_rate_buffer;
 };
 
 struct YawrateOffsetParameter
@@ -126,11 +130,11 @@ struct YawrateOffsetStatus
   int heading_estimate_status_count;
   int estimated_number;
   std::vector<double> time_buffer;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> yaw_rate_buffer;
   std::vector<double> heading_angle_buffer;
   std::vector<double> correction_velocity_buffer;
   std::vector<bool> heading_estimate_status_buffer;
-  std::vector<double> yawrate_offset_stop_buffer;
+  std::vector<double> yaw_rate_offset_stop_buffer;
 };
 
 struct HeadingParameter
@@ -156,10 +160,10 @@ struct HeadingStatus
   int estimated_number;
   std::vector<double> time_buffer;
   std::vector<double> heading_angle_buffer;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> yaw_rate_buffer;
   std::vector<double> correction_velocity_buffer;
-  std::vector<double> yawrate_offset_stop_buffer;
-  std::vector<double> yawrate_offset_buffer;
+  std::vector<double> yaw_rate_offset_stop_buffer;
+  std::vector<double> yaw_rate_offset_buffer;
   std::vector<double> slip_angle_buffer;
   std::vector<double> gnss_status_buffer;
 };
@@ -187,10 +191,10 @@ struct RtkHeadingStatus
   double last_rtk_heading_angle;
   std::vector<double> time_buffer;
   std::vector<double> heading_angle_buffer;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> yaw_rate_buffer;
   std::vector<double> correction_velocity_buffer;
-  std::vector<double> yawrate_offset_stop_buffer;
-  std::vector<double> yawrate_offset_buffer;
+  std::vector<double> yaw_rate_offset_stop_buffer;
+  std::vector<double> yaw_rate_offset_buffer;
   std::vector<double> slip_angle_buffer;
   std::vector<double> gnss_status_buffer;
   std::vector<double> distance_buffer;
@@ -244,6 +248,7 @@ struct PositionParameter
   double gnss_receiving_threshold;
   double outlier_threshold;
   double outlier_ratio_threshold;
+  double gnss_error_covariance;
 };
 
 struct PositionStatus
@@ -268,6 +273,7 @@ struct PositionInterpolateParameter
   double imu_rate;
   double stop_judgment_threshold;
   double sync_search_period;
+  double proc_noise;
 };
 
 
@@ -276,6 +282,7 @@ struct PositionInterpolateStatus
   int position_estimate_status_count;
   int number_buffer;
   bool position_estimate_start_status;
+  bool is_estimate_start{false};
   double position_stamp_last;
   double time_last;
   double provisional_enu_pos_x;
@@ -285,6 +292,7 @@ struct PositionInterpolateStatus
   std::vector<double> provisional_enu_pos_y_buffer;
   std::vector<double> provisional_enu_pos_z_buffer;
   std::vector<double> imu_stamp_buffer;
+  Eigen::MatrixXd position_covariance_last;
 };
 
 struct SlipangleParameter
@@ -337,8 +345,8 @@ struct TrajectoryParameter
   double curve_judgment_threshold;
   double sensor_noise_velocity;
   double sensor_scale_noise_velocity;
-  double sensor_noise_yawrate;
-  double sensor_bias_noise_yawrate;
+  double sensor_noise_yaw_rate;
+  double sensor_bias_noise_yaw_rate;
 };
 
 struct TrajectoryStatus
@@ -404,12 +412,12 @@ struct AngularVelocityOffsetStopStatus
 {
   int stop_count;
   double rollrate_offset_stop_last;
-  double pitchrate_offset_stop_last;
-  double yawrate_offset_stop_last;
+  double pitch_rate_offset_stop_last;
+  double yaw_rate_offset_stop_last;
   bool estimate_start_status;
   std::vector<double> rollrate_buffer;
-  std::vector<double> pitchrate_buffer;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> pitch_rate_buffer;
+  std::vector<double> yaw_rate_buffer;
 };
 
 struct RtkDeadreckoningParameter
@@ -464,7 +472,7 @@ struct EnableAdditionalRollingStatus
 {
   double distance_last;
   double acc_offset_sum;
-  double yawrate;
+  double yaw_rate;
   double rollrate;
   double imu_acceleration_y;
   double rollrate_offset_stop;
@@ -474,9 +482,9 @@ struct EnableAdditionalRollingStatus
   std::vector<double> roll_rate_interpolate_buffer;
   std::vector<double> rolling_estimated_buffer;
   std::vector<double> imu_time_buffer;
-  std::vector<double> yawrate_buffer;
+  std::vector<double> yaw_rate_buffer;
   std::vector<double> velocity_buffer;
-  std::vector<double> yawrate_offset_buffer;
+  std::vector<double> yaw_rate_offset_buffer;
   std::vector<double> acceleration_y_buffer;
   std::vector<double> distance_buffer;
 };
@@ -501,9 +509,9 @@ extern void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav, co
 extern void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter,
   VelocityScaleFactorStatus*, geometry_msgs::msg::TwistStamped*, eagleye_msgs::msg::VelocityScaleFactor*);
 extern void distance_estimate(const geometry_msgs::msg::TwistStamped, DistanceStatus*, eagleye_msgs::msg::Distance*);
-extern void yawrate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, const YawrateOffsetStopParameter,
+extern void yaw_rate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, const YawrateOffsetStopParameter,
   YawrateOffsetStopStatus*, eagleye_msgs::msg::YawrateOffset*);
-extern void yawrate_offset_estimate(const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Heading,
+extern void yaw_rate_offset_estimate(const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Heading,
   const sensor_msgs::msg::Imu, const YawrateOffsetParameter, YawrateOffsetStatus*, eagleye_msgs::msg::YawrateOffset*);
 extern void heading_estimate(const rtklib_msgs::msg::RtklibNav, const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped,
   const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::YawrateOffset,  const eagleye_msgs::msg::SlipAngle, const eagleye_msgs::msg::Heading,
@@ -532,7 +540,7 @@ extern void heading_interpolate_estimate(const sensor_msgs::msg::Imu, const geom
   const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::Heading, const eagleye_msgs::msg::SlipAngle,
   const HeadingInterpolateParameter,HeadingInterpolateStatus*, eagleye_msgs::msg::Heading*);
 extern void position_interpolate_estimate(const eagleye_msgs::msg::Position, const geometry_msgs::msg::Vector3Stamped, const eagleye_msgs::msg::Position,
-  const eagleye_msgs::msg::Height,const PositionInterpolateParameter, PositionInterpolateStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*);
+  const eagleye_msgs::msg::Height,const eagleye_msgs::msg::Heading, const PositionInterpolateParameter, PositionInterpolateStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*);
 extern void pitching_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg::Gpgga, const geometry_msgs::msg::TwistStamped,
   const eagleye_msgs::msg::Distance,const HeightParameter,HeightStatus*,eagleye_msgs::msg::Height*,eagleye_msgs::msg::Pitching*,eagleye_msgs::msg::AccXOffset*,
   eagleye_msgs::msg::AccXScaleFactor*);
@@ -541,9 +549,9 @@ extern void trajectory3d_estimate(const sensor_msgs::msg::Imu, const geometry_ms
   geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*,geometry_msgs::msg::TwistWithCovarianceStamped*);
 extern void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu,
   const AngularVelocityOffsetStopParameter, AngularVelocityOffsetStopStatus*, eagleye_msgs::msg::AngularVelocityOffset*);
-extern void rtk_deadreckoning_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga,
+extern void rtk_dead_reckoning_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga,
   const eagleye_msgs::msg::Heading,const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*);
-extern void rtk_deadreckoning_estimate(const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga, const eagleye_msgs::msg::Heading,
+extern void rtk_dead_reckoning_estimate(const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga, const eagleye_msgs::msg::Heading,
   const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*);
 extern void rtk_heading_estimate(const nmea_msgs::msg::Gpgga, const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped,
   const eagleye_msgs::msg::Distance,const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::YawrateOffset,  const eagleye_msgs::msg::SlipAngle,
@@ -576,10 +584,10 @@ class VelocityEstimator
         PitchrateOffsetStopEstimator();
 
         void setParam(std::string yaml_file);
-        bool PitchrateOffsetStopEstimate(double pitchrate, double stop_status);
+        bool PitchrateOffsetStopEstimate(double pitch_rate, double stop_status);
 
-        double pitchrate_offset;
-        eagleye_msgs::msg::Status pitchrate_offset_status;
+        double pitch_rate_offset;
+        eagleye_msgs::msg::Status pitch_rate_offset_status;
 
       private:
         struct Param
@@ -591,7 +599,7 @@ class VelocityEstimator
         PitchrateOffsetStopEstimator::Param param;
 
         int stop_count;
-        std::vector<double> pitchrate_buffer;
+        std::vector<double> pitch_rate_buffer;
     };
 
     class PitchingEstimator
@@ -601,7 +609,7 @@ class VelocityEstimator
 
         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,
+                              double pitch_rate, double pitch_rate_offset, double rtkfix_pitching,
                               bool navsat_update_status, bool stop_status);
 
         double pitching;
@@ -625,7 +633,7 @@ class VelocityEstimator
         PitchingEstimator::Param param;
 
         std::vector<double> time_buffer;
-        std::vector<double> corrected_pitchrate_buffer;
+        std::vector<double> corrected_pitch_rate_buffer;
         std::vector<double> rtkfix_pitching_buffer;
         std::vector<bool> use_gnss_status_buffer;
         std::vector<bool> navsat_update_status_buffer;
@@ -697,7 +705,7 @@ class VelocityEstimator
 
     // imu variables
     double acceleration;
-    double pitchrate;
+    double pitch_rate;
     double imu_time_last;
 
     // rtklib_nav variables
@@ -722,9 +730,9 @@ class VelocityEstimator
     std::vector<double> angular_velocity_z_buffer;
 
     // // PitchrateOffsetStopEstimator variables
-    PitchrateOffsetStopEstimator pitchrate_offset_stop_estimator;
-    double pitchrate_offset;
-    eagleye_msgs::msg::Status pitchrate_offset_status;
+    PitchrateOffsetStopEstimator pitch_rate_offset_stop_estimator;
+    double pitch_rate_offset;
+    eagleye_msgs::msg::Status pitch_rate_offset_status;
 
     // PitchingEstimator variables
     PitchingEstimator pitching_estimator;