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 <[email protected]>

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)

eagleye_core/navigation/CMakeLists.txt

@@ -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/src/angular_velocity_offset_stop.cpp

@@ -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/position.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/rtk_dead_reckoning.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/yaw_rate_offset_stop.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_rt/CMakeLists.txt

@@ -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/config/README.md

@@ -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/eagleye_config.yaml

@@ -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

eagleye_rt/launch/eagleye_rt.launch.xml

@@ -145,7 +145,9 @@
         </include>
         <include file="$(find-pkg-share eagleye_tf)/launch/tf.launch.xml" if="$(var use_tf)"/>
 
-        <include file="$(find-pkg-share eagleye_fix2pose)/launch/fix2pose.launch.xml">
+        <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml">
+        </include>
+        <include file="$(find-pkg-share eagleye_geo_pose_converter)/launch/geo_pose_converter.launch.xml">
         </include>
   </group>
 

eagleye_rt/src/heading_node.cpp

@@ -55,6 +55,9 @@ static bool use_multi_antenna_mode;
 
 bool is_first_correction_velocity = false;
 
+bool skip_static_initialization = false;
+double yaw_rate_offset_stop_in_skip_mode = 0.0;
+
 std::string node_name = "eagleye_heading";
 
 void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg)
@@ -65,6 +68,8 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg)
 void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
 {
   velocity = *msg;
+  // To avoid unnecessary buffering when it's just sitting there right after start-up, we're making it so it doesn't buffer.
+  // Multi-antenna mode is an exception.
   if (is_first_correction_velocity == false && msg->twist.linear.x > heading_parameter.moving_judgment_threshold)
   {
     is_first_correction_velocity = true;
@@ -115,13 +120,28 @@ void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg)
 
 void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 {
-  if (!is_first_correction_velocity) return;
-  if(use_can_less_mode && !velocity_status.status.enabled_status) return;
-  if(!yaw_rate_offset_stop.status.enabled_status)
+  if (!is_first_correction_velocity)
+  {
+    RCLCPP_WARN(rclcpp::get_logger(node_name), "is_first_correction_velocity is false.");
+    return;
+  }
+  if(use_can_less_mode && !velocity_status.status.enabled_status)
   {
-    RCLCPP_WARN(rclcpp::get_logger(node_name), "Heading estimation is not started because the stop calibration is not yet completed.");
+    RCLCPP_WARN(rclcpp::get_logger(node_name), "velocity_status is not enabled.");
     return;
   }
+  if(!yaw_rate_offset_stop.status.enabled_status)
+  {
+    if(skip_static_initialization)
+    {
+      yaw_rate_offset_stop.yaw_rate_offset = yaw_rate_offset_stop_in_skip_mode;
+    }
+    else
+    {
+      RCLCPP_WARN(rclcpp::get_logger(node_name), "Heading estimation is not started because the stop calibration is not yet completed.");
+      return;
+    }
+  }
 
   imu = *msg;
   heading.header = msg->header;
@@ -176,6 +196,8 @@ int main(int argc, char** argv)
     heading_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_ratio_threshold"].as<double>();
     heading_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["heading"]["curve_judgment_threshold"].as<double>();
     heading_parameter.init_STD = conf["/**"]["ros__parameters"]["heading"]["init_STD"].as<double>();
+    skip_static_initialization = conf["/**"]["ros__parameters"]["heading"]["skip_static_initialization"].as<bool>();
+    yaw_rate_offset_stop_in_skip_mode = conf["/**"]["ros__parameters"]["heading"]["yaw_rate_offset_stop_in_skip_mode"].as<double>();
 
     std::cout<< "use_gnss_mode " << use_gnss_mode << std::endl;
 
@@ -194,6 +216,8 @@ int main(int argc, char** argv)
     std::cout << "outlier_ratio_threshold " << heading_parameter.outlier_ratio_threshold << std::endl;
     std::cout << "curve_judgment_threshold " << heading_parameter.curve_judgment_threshold << std::endl;
     std::cout << "init_STD " << heading_parameter.init_STD << std::endl;
+    std::cout << "skip_static_initialization " << skip_static_initialization << std::endl;
+    std::cout << "yaw_rate_offset_stop_in_skip_mode " << yaw_rate_offset_stop_in_skip_mode << std::endl;
   }
   catch (YAML::Exception& e)
   {
@@ -237,6 +261,12 @@ int main(int argc, char** argv)
     rclcpp::shutdown();
   }
 
+  if(use_multi_antenna_mode)
+  {
+    // When using multi-antenna mode, set is_first_correction_velocity to true even when stationary for the first time.
+    is_first_correction_velocity = true;
+  }
+
   auto sub1 = node->create_subscription<sensor_msgs::msg::Imu>("imu/data_tf_converted", 1000, imu_callback);
   auto sub2 = node->create_subscription<rtklib_msgs::msg::RtklibNav>(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback);
   auto sub3 = node->create_subscription<nmea_msgs::msg::Gprmc>(subscribe_rmc_topic_name, 1000, rmc_callback);

eagleye_rt/src/rolling_node.cpp

@@ -46,27 +46,27 @@ struct RollingStatus _rolling_status;
 
 static bool _use_can_less_mode;
 
-void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg)
+void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
 {
   _velocity_msg = *msg;
 }
 
-void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg)
+void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg)
 {
   _velocity_status_msg = *msg;
 }
 
-void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr msg)
+void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg)
 {
   _yaw_rate_offset_stop_msg = *msg;
 }
 
-void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr msg)
+void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg)
 {
   _yaw_rate_offset_2nd_msg = *msg;
 }
 
-void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg)
+void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 {
   if(_use_can_less_mode && !_velocity_status_msg.status.enabled_status) return;
   _imu_msg = *msg;

eagleye_rt/src/rtk_heading_node.cpp

@@ -50,7 +50,7 @@ struct RtkHeadingStatus heading_status;
 
 static bool use_can_less_mode;
 
-void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg)
+void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
 {
   velocity = *msg;
 }
@@ -60,7 +60,7 @@ void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg)
   gga = *msg;
 }
 
-void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg)
+void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg)
 {
   velocity_status = *msg;
 }

eagleye_rt/src/slip_angle_node.cpp

@@ -46,12 +46,12 @@ struct SlipangleParameter slip_angle_parameter;
 
 static bool use_can_less_mode;
 
-void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg)
+void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
 {
   velocity = *msg;
 }
 
-void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg)
+void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg)
 {
   velocity_status = *msg;
 }
@@ -124,6 +124,7 @@ int main(int argc, char** argv)
   auto sub2 = node->create_subscription<eagleye_msgs::msg::VelocityScaleFactor>("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback);  //ros::TransportHints().tcpNoDelay()
   auto sub3 = node->create_subscription<eagleye_msgs::msg::YawrateOffset>("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback);  //ros::TransportHints().tcpNoDelay()
   auto sub4 = node->create_subscription<eagleye_msgs::msg::YawrateOffset>("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback);  //ros::TransportHints().tcpNoDelay()
+  auto sub5 = node->create_subscription<geometry_msgs::msg::TwistStamped>("velocity", rclcpp::QoS(10), velocity_callback);  //ros::TransportHints().tcpNoDelay()
   pub = node->create_publisher<eagleye_msgs::msg::SlipAngle>("slip_angle", rclcpp::QoS(10));
 
   rclcpp::spin(node);

eagleye_rt/src/slip_coefficient_node.cpp

@@ -66,7 +66,7 @@ void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr ms
   }
 }
 
-void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg)
+void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg)
 {
   velocity_status = *msg;
 }