v1.7.2-ros2 (#327)

* Add static initilization skip mode (#271)

* Add covariance in nmea2fix (#272)

* Galactit build (#276)

* Add velocity subscriver in slip_angle_node (#278)

* fixed stop judgment bug (#290)

* Change of coordinate system for Eagleye's standard input IMU (#296)

* Revert "Change of coordinate system for Eagleye's standard input IMU (#296)" (#297)

This reverts commit 03f9e1f.

* Change of coordinate system for Eagleye's standard input IMU (#299)

* Change of coordinate system for Eagleye's standard input IMU

* Correction of Response

* fix compile warning (#305)

* add missing dependencies in package.xml (#306)

* Fix height in eagleye_fix (#307)

* Add architecture svg (#308)

* S^Cit fix2pose into two separate nodes (#316)

* [ROS2]Fixed heading estimation bug in multi-antenna mode at standstill. (#322)

* Fixed heading estimation bug in multi-antenna mode at standstill.

* Add comment about is_first_correction_velocity in multi-antenna mode

---------

Co-authored-by: Aoki-Takanose <49303804+Aoki-Takanose@users.noreply.github.com>

Author: rsasaki0109

README.md

@@ -12,6 +12,10 @@ Eagleye is an open-source software for vehicle localization utilizing GNSS and I
 
 ![Flowchart of Eagleye](docs/flowchart.png)
 
+## Architecture
+
+![Architecture of Eagleye](docs/eagleye_architecture.drawio.svg)
+
 ## Recommended Sensors
 **GNSS receiver**
 * [Septentrio Mosaic development kit with GNSS antenna](https://shop.septentrio.com/en/shop/mosaic-x5-devkit) 
@@ -76,6 +80,16 @@ Access mosaic's web ui and upload the following file in Admin/Configuration.
 
 https://www.dropbox.com/s/uckt9
 
+### IMU
+
+1. IMU settings.
+
+* Output rate 50Hz
+
+2. Please be careful with the coordinate system when using the [tamagawa ros driver](https://github.com/MapIV/tamagawa_imu_driver) created by MAP IV. If the x-direction indicated on the IMU is set to the vehicle's direction and the y-direction to the right side of the vehicle, please set the `roll` in the `eagleye/eagleye_util/tf/config/sensors_tf.yaml file` to `3.14159`.
+
+		 roll: 3.14159
+
 ### Eagleye parameters
 
 The parameters of eagleye can be set in the [eagleye_config.yaml](https://github.com/MapIV/eagleye/tree/ros2-galactic-v1.1.5/eagleye_rt/config/eagleye_config.yaml). The default settings are 5Hz for GNSS and 50Hz for IMU.
@@ -157,7 +171,8 @@ To visualize the eagleye output location /eagleye/fix, for example, use the foll
 
 To convert from eagleye/fix to eagleye/pose, use the following command　
 
-	ros2 launch eagleye_fix2pose fix2pose.xml
+	ros2 launch eagleye_geo_pose_fusion geo_pose_fusion.launch.xml
+	ros2 launch eagleye_geo_pose_converter geo_pose_converter.launch.xml
 
 ## Sample data
 ### ROSBAG(ROS1)

docs/eagleye_architecture.drawio.svg

@@ -15,6 +15,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   # add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+if($ENV{ROS_DISTRO} STREQUAL "galactic")
+  add_definitions(-DROS_DISTRO_GALACTIC)
+endif()
+
 # find dependencies
 find_package(ament_cmake_auto REQUIRED)
 find_package(eagleye_coordinate REQUIRED)

eagleye_core/navigation/CMakeLists.txt

@@ -70,7 +70,7 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStampe
     angular_velocity_stop_status->yaw_rate_buffer.erase(angular_velocity_stop_status->yaw_rate_buffer.begin());
   }
 
-  if (velocity.twist.linear.x < angular_velocity_stop_parameter.stop_judgment_threshold)
+  if (std::abs(velocity.twist.linear.x) < angular_velocity_stop_parameter.stop_judgment_threshold)
   {
     ++angular_velocity_stop_status->stop_count;
   }

eagleye_core/navigation/src/angular_velocity_offset_stop.cpp

@@ -60,42 +60,7 @@ void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::
   enu_pos[1] = position_status->enu_pos[1];
   enu_pos[2] = position_status->enu_pos[2];
 
-  if(!position_status->gnss_update_failure)
-  {
-    gnss_status = true;
-
-    geometry_msgs::msg::PoseStamped pose;
-
-    pose.pose.position.x = enu_pos[0];
-    pose.pose.position.y = enu_pos[1];
-    pose.pose.position.z = enu_pos[2];
-
-    heading_interpolate_3rd.heading_angle = fmod(heading_interpolate_3rd.heading_angle,2*M_PI);
-    tf2::Transform transform;
-    tf2::Quaternion q;
-    transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z));
-    q.setRPY(0, 0, (90* M_PI / 180)-heading_interpolate_3rd.heading_angle);
-    transform.setRotation(q);
-
-    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(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 = transform3.getOrigin();
-
-    enu_pos[0] = tmp_pos.getX();
-    enu_pos[1] = tmp_pos.getY();
-    enu_pos[2] = tmp_pos.getZ();
-  }
-  else
-  {
-    gnss_status = false;
-  }
+  gnss_status = !position_status->gnss_update_failure;
 
   if (heading_interpolate_3rd.status.estimate_status == true && velocity_status.status.enabled_status == true)
   {

eagleye_core/navigation/src/position.cpp

@@ -64,31 +64,6 @@ void rtk_dead_reckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nm
     llh2xyz(llh_rtk,ecef_rtk);
     xyz2enu(ecef_rtk,ecef_base_pos,enu_rtk);
 
-    geometry_msgs::msg::PoseStamped pose;
-
-    pose.pose.position.x = enu_rtk[0];
-    pose.pose.position.y = enu_rtk[1];
-    pose.pose.position.z = enu_rtk[2];
-
-    heading.heading_angle = fmod(heading.heading_angle,2*M_PI);
-    tf2::Transform transform;
-    tf2::Quaternion q;
-    transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z));
-    q.setRPY(0, 0, (90* M_PI / 180)-heading.heading_angle);
-    transform.setRotation(q);
-
-    tf2::Transform transform2;
-    tf2::Quaternion q2(rtk_dead_reckoning_parameter.tf_gnss_rotation_x,rtk_dead_reckoning_parameter.tf_gnss_rotation_y,rtk_dead_reckoning_parameter.tf_gnss_rotation_z,rtk_dead_reckoning_parameter.tf_gnss_rotation_w);
-    transform2.setOrigin(transform*tf2::Vector3(-rtk_dead_reckoning_parameter.tf_gnss_translation_x, -rtk_dead_reckoning_parameter.tf_gnss_translation_y,-rtk_dead_reckoning_parameter.tf_gnss_translation_z));
-    transform2.setRotation(transform*q2);
-
-    tf2::Vector3 tmp_pos;
-    tmp_pos = transform2.getOrigin();
-
-    enu_rtk[0] = tmp_pos.getX();
-    enu_rtk[1] = tmp_pos.getY();
-    enu_rtk[2] = tmp_pos.getZ();
-
     if (rtk_dead_reckoning_status->position_stamp_last != gga_time && gga.gps_qual == 4)
     {
       rtk_dead_reckoning_status->provisional_enu_pos_x = enu_rtk[0];

eagleye_core/navigation/src/rtk_dead_reckoning.cpp

@@ -59,7 +59,7 @@ void yaw_rate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped veloci
     yaw_rate_offset_stop_status->yaw_rate_buffer.erase(yaw_rate_offset_stop_status->yaw_rate_buffer.begin());
   }
 
-  if (velocity.twist.linear.x < yaw_rate_offset_stop_parameter.stop_judgment_threshold)
+  if (std::abs(velocity.twist.linear.x) < yaw_rate_offset_stop_parameter.stop_judgment_threshold)
   {
     ++yaw_rate_offset_stop_status->stop_count;
   }

eagleye_core/navigation/src/yaw_rate_offset_stop.cpp

@@ -10,6 +10,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+if($ENV{ROS_DISTRO} STREQUAL "galactic")
+  add_definitions(-DROS_DISTRO_GALACTIC)
+endif()
+
 # find dependencies
 find_package(ament_cmake_auto REQUIRED)
 find_package(PkgConfig REQUIRED)

eagleye_rt/CMakeLists.txt

@@ -111,6 +111,8 @@ Figure shows the relationship between these parameters.
 | outlier_ratio_threshold             | double | Ratio of allowable outliers in the interval (Value from 0~1)                    | 0.5                  |
 | curve_judgment_threshold            | double | Yaw rate threshold for curve determination [rad/s]                              | 0.0873 (5 deg/s)     |
 | init_STD                   | double | Standard deviation of Doppler azimuth angle [rad] | 0.0035 (0.2 deg)          |
+| skip_static_initialization                   | bool | Whether to skip initial static estimation of yaw rate offset | false          |
+| yaw_rate_offset_stop_in_skip_mode                   | double | Initial yaw rate offset stop in skip mode [rad/sec] | 0.0           |
 
 ### heading_interpolate
 

eagleye_rt/config/README.md

@@ -31,8 +31,6 @@
       z : 0.0
       use_ecef_base_position : false
 
-    reverse_imu_wz: false
-
     # Eagleye Navigation Parameters
     # Basic Navigation Functions
     common:
@@ -72,6 +70,8 @@
       outlier_ratio_threshold: 0.5
       curve_judgment_threshold: 0.0873
       init_STD: 0.0035 #[rad] (= 0.2 [deg])
+      skip_static_initialization: false
+      yaw_rate_offset_stop_in_skip_mode: 0.0
 
     heading_interpolate:
       sync_search_period: 2