Develop ros2 (#215)

* Stop parameterizing the gga and rmc topics (#186) * Stop parameterizing the gga and rmc topics * Update README.md * Feature/ros2/pubsub for autoware (#188) * Stop parameterizing the gga and rmc topics * Support for TwistWithCovariance input * Support GNSS twist input * Add dependency in gnss_converter package.xml * Fix tf2_geometry_msgs.hpp * Fix tf2_geometry_msgs.h in fix2pose.cpp * Fix bug in rtklib and fix2pose (#189) * Fix bug in rtklib and fix2pose * Remove galactic CI * Fix yaml error and namespace (#191) * Comment out cout in llsxy_mgrs.cpp * Fix tf (#194) Co-authored-by: Ryohei Sasaki <[email protected]> * Add fix-pose only punlish mode (#195) Co-authored-by: Ryohei Sasaki <[email protected]> * Add new geoid-model (#196) * Add new geoid-model * Delete including height_converter * Fix fix2kml * Delete geographic dependency in fix2pose * submodule tru in CI * Fix llh_converter Co-authored-by: Ryohei Sasaki <[email protected]> * Add TrajectoryParameter (#197) Co-authored-by: Ryohei Sasaki <[email protected]> * Add reverse_imu_wz parameter (#198) * Add autoware ekf trigger service client (#199) Co-authored-by: Ryohei Sasaki <[email protected]> * Update README.md * Update fix2pose.cpp (#203) * Add dual-antenna (#205) * Feature/covariance fix dr ros2 (#206) * fixing covariance output for twist * add variance/covariance to * update config file * update launch * add function to calculate covariance for pose * Add minimum covariance paeam of the pose to publish (#207) * refactor eagleye_rt.launch (#208) * Add fix-judgement-type in fix2pose (#212) * Sync log csv output and trajectory_plot with ROS1 ver (#213) * fix time bug in log csv * ROS1 Sync trajectory_plot * Update parameter description * Update parem description about covariance output --------- Co-authored-by: Ryohei Sasaki <[email protected]> Co-authored-by: Aoki-Takanose <[email protected]>
MapIV · Feb 3, 2023 · d62f0df · d62f0df
1 parent c66626f
commit d62f0df
Show file tree

Hide file tree

Showing 65 changed files with 1,434 additions and 1,046 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -16,7 +16,9 @@ jobs:
 
     steps:
       - name: checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v1
+        with:
+          submodules: true
 
       - name: rosdep update
         run: |
@@ -40,44 +42,6 @@ jobs:
           rosdep install --from-paths src --ignore-src -r -y
           colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests
 
-  galactic_build:
-    runs-on:  ubuntu-20.04
-
-    container: ros:galactic
-    services:
-      ros:
-        image: ros
-
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - name: checkout
-        uses: actions/checkout@v2
-
-      - 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
-        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/galactic/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
 
@@ -92,7 +56,9 @@ jobs:
 
     steps:
       - name: checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v1
+        with:
+          submodules: true
 
       - name: rosdep update
         run: |

diff --git a/.gitmodules b/.gitmodules
@@ -0,0 +1,4 @@
+[submodule "eagleye_util/llh_converter"]
+	path = eagleye_util/llh_converter
+	url = https://github.com/MapIV/llh_converter
+	branch = ros2
diff --git a/README.md b/README.md
@@ -46,7 +46,11 @@ Clone and build the necessary packages for Eagleye. ([rtklib_ros_bridge](https:/
 	git clone https://github.com/MapIV/eagleye.git -b main-ros2
 	git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
 	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
+	sudo geographiclib-get-geoids best
+	sudo mkdir /usr/share/GSIGEO
+	sudo cp eagleye/eagleye_util/llh_converter/data/gsigeo2011_ver2_1.asc /usr/share/GSIGEO/
 	cd ..
 	rosdep install --from-paths src --ignore-src -r -y
 	colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
@@ -98,7 +102,7 @@ The estimated results will be output about 100 seconds after playing the rosbag.
 1. Check if wheel speed (vehicle speed) is published in `/can_twist` topic.
 
 * Topic name: /can_twist
-* Message type: geometry_msgs/TwistStamped twist.liner.x
+* Message type: geometry_msgs/TwistStamped twist.liner.x or geometry_msgs/TwistWithCovarianceStamped twist.twist.liner.x
 
 
 2. Check if the IMU data is published in `/imu/data_raw` topic.
@@ -128,7 +132,7 @@ The estimated results will be output about 100 seconds after playing the rosbag.
 ### Subscribed Topics
  - /navsat/nmea_sentence (nmea_msgs/Sentence)
 
- - /can_twist (geometry_msgs/TwistStamped)
+ - /can_twist (geometry_msgs/TwistStamped or geometry_msgs/TwistWithCovarianceStamped)
 
  - /rtklib_nav (rtklib_msgs/RtklibNav)
 
@@ -138,7 +142,9 @@ The estimated results will be output about 100 seconds after playing the rosbag.
 
  - /eagleye/fix (sensor_msgs/NavSatFix) 
 
- - /eagleye/twist (ngeometry_msgs/TwistStamped)
+ - /eagleye/twist (geometry_msgs/TwistStamped)
+
+ - /eagleye/twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped)
 
 
 ### Note

diff --git a/eagleye_core/coordinate/src/ll2xy_mgrs.cpp b/eagleye_core/coordinate/src/ll2xy_mgrs.cpp
@@ -105,8 +105,8 @@ void ll2xy_mgrs(double llh[3], double xyz[3])
   //wgs_point.longitude = llh[1];
   wgs_point.altitude = llh[2];
 
-  std::cout << wgs_point.latitude << std::endl;
-  std::cout << wgs_point.longitude << std::endl;
+  // std::cout << wgs_point.latitude << std::endl;
+  // std::cout << wgs_point.longitude << std::endl;
 
 
   geodesy::UTMPoint utm_point;

diff --git a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp
@@ -24,6 +24,7 @@
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
 
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
@@ -144,12 +145,14 @@ struct HeadingParameter
   double outlier_threshold;
   double outlier_ratio_threshold;
   double curve_judgment_threshold;
+  double init_STD;
 };
 
 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;
@@ -202,6 +205,7 @@ struct HeadingInterpolateParameter
   double imu_rate;
   double stop_judgment_threshold;
   double sync_search_period;
+  double proc_noise;
 };
 
 struct HeadingInterpolateStatus
@@ -214,6 +218,7 @@ struct HeadingInterpolateStatus
   double provisional_heading_angle;
   std::vector<double> provisional_heading_angle_buffer;
   std::vector<double> imu_stamp_buffer;
+  double heading_variance_last;
 };
 
 struct PositionParameter
@@ -227,7 +232,7 @@ struct PositionParameter
   double tf_gnss_rotation_x;
   double tf_gnss_rotation_y;
   double tf_gnss_rotation_z;
-  double tf_gnss_rotation_w;
+  double tf_gnss_rotation_w = 1.0;
   std::string tf_gnss_parent_frame;
   std::string tf_gnss_child_frame;
 
@@ -330,6 +335,10 @@ struct TrajectoryParameter
 {
   double stop_judgment_threshold;
   double curve_judgment_threshold;
+  double sensor_noise_velocity;
+  double sensor_scale_noise_velocity;
+  double sensor_noise_yawrate;
+  double sensor_bias_noise_yawrate;
 };
 
 struct TrajectoryStatus
@@ -419,6 +428,8 @@ struct RtkDeadreckoningParameter
   double tf_gnss_rotation_w;
   std::string tf_gnss_parent_frame;
   std::string tf_gnss_child_frame;
+  double rtk_fix_STD;
+  double proc_noise;
 };
 
 struct RtkDeadreckoningStatus
@@ -436,6 +447,7 @@ struct RtkDeadreckoningStatus
   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 EnableAdditionalRollingParameter
@@ -499,6 +511,8 @@ extern void heading_estimate(const rtklib_msgs::msg::RtklibNav, const sensor_msg
 extern void heading_estimate(const nmea_msgs::msg::Gprmc, 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,
   const HeadingParameter, HeadingStatus*,eagleye_msgs::msg::Heading*);
+extern void heading_estimate(const eagleye_msgs::msg::Heading, 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, const HeadingParameter, HeadingStatus*,eagleye_msgs::msg::Heading*);
 extern void position_estimate(const rtklib_msgs::msg::RtklibNav, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped,
   const eagleye_msgs::msg::Distance, const eagleye_msgs::msg::Heading, const geometry_msgs::msg::Vector3Stamped, const PositionParameter, PositionStatus*,
   eagleye_msgs::msg::Position*);
@@ -513,7 +527,7 @@ extern void smoothing_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_
   eagleye_msgs::msg::Position*);
 extern void trajectory_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped,
   const eagleye_msgs::msg::Heading, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*,
-  geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*);
+  geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*,geometry_msgs::msg::TwistWithCovarianceStamped*);
 extern void heading_interpolate_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped,
   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*);
@@ -524,7 +538,7 @@ extern void pitching_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg:
   eagleye_msgs::msg::AccXScaleFactor*);
 extern void trajectory3d_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, const eagleye_msgs::msg::Heading,
   const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Pitching,const TrajectoryParameter,TrajectoryStatus*,
-  geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*);
+  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,

diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp
@@ -150,10 +150,10 @@ void heading_estimate_(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamp
       std::vector<double> inversion_up_index;
       std::vector<double> inversion_down_index;
 
-     if(heading_interpolate.status.enabled_status == false)
-     {
-       heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]];
-     }
+      if(heading_interpolate.status.enabled_status == false)
+      {
+        heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]];
+      }
 
       int ref_cnt;
       std::vector<double> heading_angle_buffer2;
@@ -238,6 +238,9 @@ void heading_estimate_(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamp
             provisional_heading_angle_buffer[index[index_length-1]]);
         }
         heading->status.estimate_status = true;
+
+        double heading_STD = heading_parameter.init_STD;
+        heading->variance = heading_STD*heading_STD;
       }
     }
   }
@@ -323,4 +326,36 @@ void heading_estimate(const nmea_msgs::msg::Gprmc nmea_rmc,sensor_msgs::msg::Imu
   heading_status->gnss_status_buffer.push_back(gnss_status);
 
   heading_estimate_(imu, velocity, yawrate_offset_stop, yawrate_offset, slip_angle, heading_interpolate, heading_parameter, heading_status, heading);
+}
+
+void heading_estimate(const eagleye_msgs::msg::Heading multi_antenna_heading,sensor_msgs::msg::Imu imu,geometry_msgs::msg::TwistStamped velocity,
+  eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,eagleye_msgs::msg::YawrateOffset yawrate_offset,eagleye_msgs::msg::SlipAngle slip_angle,
+  eagleye_msgs::msg::Heading heading_interpolate,HeadingParameter heading_parameter, HeadingStatus* heading_status,eagleye_msgs::msg::Heading* heading)
+{
+  bool gnss_status;
+  double heading_angle = 0.0;
+
+  rclcpp::Time multi_anttena_clock(multi_antenna_heading.header.stamp);
+  double multi_anttena_time = multi_anttena_clock.seconds();
+  if (heading_status->ros_time_last ==  multi_anttena_time || multi_anttena_time == 0)
+  {
+    gnss_status = false;
+    heading_angle = 0;
+    heading_status->ros_time_last = multi_anttena_time;
+  }
+  else
+  {
+    gnss_status = true;
+    heading_angle = multi_antenna_heading.heading_angle;
+    heading_status->ros_time_last = multi_anttena_time;
+  }
+
+  heading_status->heading_angle_buffer .push_back(heading_angle);
+  heading_status->gnss_status_buffer .push_back(gnss_status);
+
+  heading_estimate_(imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading);
+
+  if(!heading->status.estimate_status) heading->heading_angle = heading_angle;
+  if(gnss_status) heading->status.estimate_status = true;
+
 }
diff --git a/eagleye_core/navigation/src/heading_interpolate.cpp b/eagleye_core/navigation/src/heading_interpolate.cpp
@@ -43,6 +43,8 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr
   bool heading_estimate_status;
   std::size_t imu_stamp_buffer_length;
 
+  double proc_noise = heading_interpolate_parameter.proc_noise;
+
   double search_buffer_number = heading_interpolate_parameter.sync_search_period * heading_interpolate_parameter.imu_rate;
 
   rclcpp::Time ros_clock(heading.header.stamp);
@@ -85,6 +87,7 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr
     heading_interpolate_parameter.stop_judgment_threshold)
   {
     heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle + (yawrate * (imu_time - heading_interpolate_status->time_last));
+    heading_interpolate_status->heading_variance_last += proc_noise*proc_noise;
   }
 
   // data buffer generate
@@ -114,23 +117,27 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr
     if (heading_estimate_status == true && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index &&
       heading_interpolate_status->heading_estimate_status_count > 1)
     {
+      double heading_variance = heading.variance;
       diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle);
       for (i = estimate_index; i <= heading_interpolate_status->number_buffer; i++)
       {
         heading_interpolate_status->provisional_heading_angle_buffer[i-1] = heading_interpolate_status->provisional_heading_angle_buffer[i-1] -
           diff_estimate_heading_angle;
+        heading_variance += proc_noise*proc_noise;
       }
       heading_interpolate_status->provisional_heading_angle =
         heading_interpolate_status->provisional_heading_angle_buffer[heading_interpolate_status->number_buffer-1];
 
       heading_interpolate->status.enabled_status = true;
       heading_interpolate->status.estimate_status = true;
+      heading_interpolate_status->heading_variance_last = heading_variance;
     }
     else if (heading_interpolate_status->heading_estimate_status_count == 1)
     {
       heading_interpolate_status->provisional_heading_angle = heading.heading_angle;
       heading_interpolate->status.enabled_status = true;
       heading_interpolate->status.estimate_status = false;
+      heading_interpolate_status->heading_variance_last = heading.variance;
     }
     else
     {
@@ -141,6 +148,7 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr
   if (heading_interpolate_status->heading_estimate_start_status == true)
   {
     heading_interpolate->heading_angle = heading_interpolate_status->provisional_heading_angle + slip_angle.slip_angle;
+    heading_interpolate->variance = heading_interpolate_status->heading_variance_last;
   }
   else
   {

diff --git a/eagleye_core/navigation/src/position.cpp b/eagleye_core/navigation/src/position.cpp
@@ -77,15 +77,16 @@ void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::
     q.setRPY(0, 0, (90* M_PI / 180)-heading_interpolate_3rd.heading_angle);
     transform.setRotation(q);
 
-    tf2::Transform transform2;
+    tf2::Transform transform2, transform3;
     tf2::Quaternion q2(position_parameter.tf_gnss_rotation_x,position_parameter.tf_gnss_rotation_y,
       position_parameter.tf_gnss_rotation_z,position_parameter.tf_gnss_rotation_w);
-    transform2.setOrigin(transform*tf2::Vector3(-position_parameter.tf_gnss_translation_x,
-      -position_parameter.tf_gnss_translation_y, -position_parameter.tf_gnss_translation_z));
-    transform2.setRotation(transform*q2);
+    transform2.setOrigin(tf2::Vector3(position_parameter.tf_gnss_translation_x, position_parameter.tf_gnss_translation_y,
+      position_parameter.tf_gnss_translation_z));
+    transform2.setRotation(q2);
+    transform3 = transform * transform2.inverse();
 
     tf2::Vector3 tmp_pos;
-    tmp_pos = transform2.getOrigin();
+    tmp_pos = transform3.getOrigin();
 
     enu_pos[0] = tmp_pos.getX();
     enu_pos[1] = tmp_pos.getY();