feat: add gnss/imu localizer

launch/map4_localization_launch/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.14)
+project(map4_localization_launch)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_package(INSTALL_TO_SHARE
+  eagleye_pose_twist_localization_launch
+  eagleye_twist_localization_launch
+)

launch/map4_localization_launch/README.md

@@ -0,0 +1,44 @@
+# map4_localization_launch
+
+This localization launch package is a sample package to introduce eagleye into autoware.
+
+Eagleye provides a cost-effective alternative to LiDAR and point cloud-based localization by using low-cost GNSS and IMU sensors to provide vehicle position and orientation.
+
+Autoware users will be able to choose between their existing LiDAR and point cloud-based localization stacks or GNSS/IMU-based eagleye localizer, depending on their specific needs and operating environment.
+
+In addition to the LiDAR-based solution provided by `tier4_localization_launch` (shown in the first row of the following table), there are two ways to utilize eagleye in `map4_localization_launch` with the autoware localization stack.
+
+localization launch  | twist estimator | pose estimator
+-- | -- | --
+tier4_localization_launch| gyro_odometry | ndt_scan_matcher
+map4_localization_launch/eagleye_twist_localization_launch| eagleye_rt(gyro/odom/gnss fusion) | ndt_scan_matcher
+map4_localization_launch/eagleye_pose_twist_localization_launch| eagleye_rt(gyro/odom/gnss fusion) | eagleye_rt(gyro/odom/gnss fusion)
+
+## Structure
+
+- eagleye_twist_localization_launch
+
+![map4_localization_launch/eagleye_twist_localization_launch](./eagleye_twist_localization_launch/localization_launch.drawio.svg)
+
+- eagleye_pose_twist_localization_launch
+
+![map4_localization_launch/eagleye_pose_twist_localization_launch](./eagleye_pose_twist_localization_launch/localization_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+Include `localization.launch.xml` in other launch files as follows.
+
+Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `localization.launch.xml`.
+
+```xml
+  <include file="$(find-pkg-share map4_localization_launch)/eagleye_twist_localization_launch/localization.launch.xml">
+    <!-- Parameter files -->
+    <arg name="FOO_param_path" value="..."/>
+    <arg name="BAR_param_path" value="..."/>
+    ...
+  </include>
+```

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/localization.launch.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Parameter files -->
+  <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
+  <arg name="pose_initializer_param_path"/>
+  <arg name="pose_initializer_common_param_path"/>
+  <arg name="eagleye_param_path"/>
+
+  <!-- localization module -->
+  <group>
+    <push-ros-namespace namespace="localization"/>
+    <!-- util module -->
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/util/util.launch.xml"/>
+    </group>
+    <!-- pose_twist_estimator module -->
+    <group>
+      <push-ros-namespace namespace="pose_twist_estimator"/>
+      <include file="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/pose_twist_estimator/pose_twist_estimator.launch.xml"/>
+    </group>
+    <!-- pose_twist_fusion_filter module -->
+    <group>
+      <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
+    </group>
+    <!-- localization_error_monitor_module -->
+    <group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml"/>
+    </group>
+  </group>
+</launch>

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml

@@ -0,0 +1,154 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="config_yaml" default="eagleye_config.param.yaml"/>
+  <arg name="config_path" default="$(find-pkg-share autoware_launch)/config/localization/$(var config_yaml)"/>
+  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
+  <arg name="output_twist_with_cov_name" default="/localization/twist_estimator/twist_with_covariance"/>
+  <arg name="directory_with_related_launches" default="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye"/>
+  <arg name="use_eagleye_pose" default="true"/>
+
+  <arg name="enable_additional_rolling" default="false"/>
+  <arg name="use_multi_antenna_mode" default="false"/>
+
+  <group>
+    <push-ros-namespace namespace="eagleye"/>
+
+    <node pkg="topic_tools" exec="relay" name="eagleye_output_twist_relay" args="twist_with_covariance $(var output_twist_with_cov_name)"/>
+
+    <arg name="use_rtk_dead_reckoning" default="false"/>
+    <arg name="use_rtk_heading" default="false"/>
+
+    <node pkg="eagleye_rt" name="twist_relay" exec="twist_relay">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+
+    <node pkg="eagleye_rt" name="tf_converted_imu" exec="tf_converted_imu">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="velocity_scale_factor_node" exec="velocity_scale_factor">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+      <param name="velocity_scale_factor_save_str" value="$(find-pkg-share eagleye_rt)/config/velocity_scale_factor.txt"/>
+    </node>
+    <node pkg="eagleye_rt" name="yaw_rate_offset_stop_node" exec="yaw_rate_offset_stop">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="yaw_rate_offset_node_1st" exec="yaw_rate_offset" args="1st">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="yaw_rate_offset_node_2nd" exec="yaw_rate_offset" args="2nd">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_node_1st" exec="heading" args="1st" if="$(eval &quot;'$(var use_rtk_heading)'=='false'&quot;)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+      <param name="use_multi_antenna_mode" value="$(var use_multi_antenna_mode)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_node_2nd" exec="heading" args="2nd" if="$(eval &quot;'$(var use_rtk_heading)'=='false'&quot;)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+      <param name="use_multi_antenna_mode" value="$(var use_multi_antenna_mode)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_node_3rd" exec="heading" args="3rd" if="$(eval &quot;'$(var use_rtk_heading)'=='false'&quot;)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+      <param name="use_multi_antenna_mode" value="$(var use_multi_antenna_mode)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_interpolate_node_1st" exec="heading_interpolate" args="1st">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_interpolate_node_2nd" exec="heading_interpolate" args="2nd">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="heading_interpolate_node_3rd" exec="heading_interpolate" args="3rd">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="slip_angle_node" exec="slip_angle">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="distance_node" exec="distance">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="trajectory_node" exec="trajectory">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="rtk_dead_reckoning" exec="rtk_dead_reckoning" if="$(var use_rtk_dead_reckoning)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="rtk_heading_node_1st" exec="rtk_heading" args="1st" if="$(var use_rtk_heading)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <node pkg="eagleye_rt" name="rtk_heading_node_2nd" exec="rtk_heading" args="2nd" if="$(var use_rtk_heading)">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <group if="$(var use_eagleye_pose)">
+      <node pkg="eagleye_rt" name="position_node" exec="position" if="$(eval &quot;'$(var use_rtk_dead_reckoning)'=='false'&quot;)">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="position_interpolate_node" exec="position_interpolate" if="$(eval &quot;'$(var use_rtk_dead_reckoning)'=='false'&quot;)">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="smoothing_node" exec="smoothing">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="height_node" exec="height">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="angular_velocity_offset_stop_node" exec="angular_velocity_offset_stop">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="correction_imu" exec="correction_imu">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="rolling" exec="rolling">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="enable_additional_rolling" exec="enable_additional_rolling" if="$(eval &quot;'$(var enable_additional_rolling)'=='true'&quot;)">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+      <node pkg="eagleye_rt" name="rtk_heading_node_3rd" exec="rtk_heading" args="3rd" if="$(var use_rtk_heading)">
+        <param from="$(var config_path)"/>
+        <param name="yaml_file" value="$(var config_path)"/>
+      </node>
+    </group>
+
+    <node pkg="eagleye_rt" name="monitor" exec="monitor">
+      <param from="$(var config_path)"/>
+      <param name="yaml_file" value="$(var config_path)"/>
+    </node>
+    <include file="$(var directory_with_related_launches)/gnss_converter.launch.xml">
+      <arg name="config_path" value="$(var config_path)"/>
+    </include>
+
+  </group>
+
+  <group if="$(var use_eagleye_pose)">
+    <include file="$(var directory_with_related_launches)/fix2pose.launch.xml">
+      <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
+    </include>
+  </group>
+
+</launch>

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/fix2pose.launch.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
+  <node pkg="eagleye_fix2pose" name="fix2pose_node" exec="fix2pose">
+    <!-- /eagleye/fix or  /GNSS/fix(GNSS pose only mode)-->
+    <remap from="eagleye/fix" to="eagleye/fix" />
+    <remap from="eagleye/pose_with_covariance" to="$(var output_pose_with_cov_name)"/>
+    <!-- plane rectangular coordinate number -->
+    <param name="plane" value="7"/>
+    <!-- 1 : plane rectangular coordinate  2 : MGRS -->
+    <param name="tf_num" value="2"/>
+    <!-- 0 : No convert  1 : ellipsoid -> altitude  2 : altitude -> ellipsoid -->
+    <param name="convert_height_num" value="0"/>
+    <!-- 0 : EGM2008-1  1 : Japanese Geoid 2011 Ver2.1 -->
+    <param name="geoid_type" value="1"/>
+
+    <param name="parent_frame_id" value="map"/>
+    <param name="child_frame_id" value="eagleye_base_link"/>
+    <param name="base_link_frame_id" value="base_link"/>
+    <param name="gnss_frame_id" value="gnss_link"/>
+
+    <param name="fix_only_publish" value="false"/>
+    <!-- 0:fix.status 1:fix.position_covariance[0] (if fix_only_publish==true)-->
+    <param name="fix_judgement_type" value="0"/>
+    <!--Fix status(if fix_judgement_type is 0)-->
+    <param name="fix_gnss_status" value="0"/>
+    <!-- Minimum covariance of the pose topics to be published(if fix_judgement_type is 0)-->
+    <param name="fix_std_pos_thres" value="0.1"/>
+
+
+  </node>
+
+</launch>

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/gnss_converter.launch.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+  <arg name="config_path"/>
+
+  <group>
+    <push-ros-namespace namespace="gnss"/>
+    <node pkg="eagleye_gnss_converter" name="gnss_converter_node" exec="gnss_converter">
+          <param from="$(var config_path)"/>
+    </node>
+  </group>
+
+</launch>

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+  <group>
+    <include file="$(find-pkg-share map4_localization_launch)/eagleye_pose_twist_localization_launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml">
+      <arg name="output_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="output_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+    </include>
+  </group>
+</launch>

launch/map4_localization_launch/eagleye_pose_twist_localization_launch/util/util.launch.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- pose_initializer -->
+  <group>
+    <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+      <arg name="common_config_file" value="$(var pose_initializer_common_param_path)"/>
+      <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+      <arg name="sub_gnss_pose_cov" value="/localization/pose_estimator/pose_with_covariance"/>
+    </include>
+  </group>
+
+  <!-- pose_initializer (Automatic call from AD API) -->
+  <group>
+    <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
+  </group>
+
+</launch>

launch/map4_localization_launch/eagleye_twist_localization_launch/localization.launch.xml

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Parameter files -->
+  <arg name="crop_box_filter_measurement_range_param_path"/>
+  <arg name="voxel_grid_downsample_filter_param_path"/>
+  <arg name="random_downsample_filter_param_path"/>
+  <arg name="ndt_scan_matcher_param_path"/>
+  <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
+  <arg name="pose_initializer_param_path"/>
+  <arg name="pose_initializer_common_param_path"/>
+  <arg name="eagleye_param_path"/>
+
+  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
+  <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+
+  <!-- localization module -->
+  <group>
+    <push-ros-namespace namespace="localization"/>
+    <!-- util module -->
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.xml">
+        <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+      </include>
+    </group>
+    <!-- pose_estimator module -->
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_estimator/pose_estimator.launch.xml"/>
+    </group>
+    <!-- twist_estimator module -->
+    <group>
+      <push-ros-namespace namespace="twist_estimator"/>
+      <include file="$(find-pkg-share map4_localization_launch)/eagleye_twist_localization_launch/twist_estimator/twist_estimator.launch.xml"/>
+    </group>
+    <!-- pose_twist_fusion_filter module -->
+    <group>
+      <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
+    </group>
+    <!-- localization_error_monitor_module -->
+    <group>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml"/>
+    </group>
+  </group>
+</launch>