diff --git a/docs/how-to-guides/index.md b/docs/how-to-guides/index.md index 1efda48eeb7..78ad8ce5560 100644 --- a/docs/how-to-guides/index.md +++ b/docs/how-to-guides/index.md @@ -13,7 +13,6 @@ - [Determining component dependencies](others/determining-component-dependencies.md) - [Advanced usage of colcon](others/advanced-usage-of-colcon.md) - [Applying Clang-Tidy to ROS packages](others/applying-clang-tidy-to-ros-packages.md) -- [Using Eagleye in Autoware](others/eagleye-integration-guide.md) - [Defining temporal performance metrics on components](others/defining-temporal-performance-metrics.md) TODO: Write the following contents. diff --git a/docs/how-to-guides/integrating-autoware/.pages b/docs/how-to-guides/integrating-autoware/.pages index 7b7f62f439f..3d7f51c569c 100644 --- a/docs/how-to-guides/integrating-autoware/.pages +++ b/docs/how-to-guides/integrating-autoware/.pages @@ -4,5 +4,5 @@ nav: - 2. Creating vehicle and sensor description: creating-vehicle-and-sensor-description - 3. Creating vehicle interface: creating-vehicle-interface-package - 4. Creating maps: creating-maps - - 5. Launch Autoware: launch-autoware.md + - 5. Launch Autoware: launch-autoware - 6. Tuning parameters and performance: tuning-parameters-and-performance diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware.md b/docs/how-to-guides/integrating-autoware/launch-autoware.md deleted file mode 100644 index 7c3aab759c8..00000000000 --- a/docs/how-to-guides/integrating-autoware/launch-autoware.md +++ /dev/null @@ -1,5 +0,0 @@ -# Launch Autoware - -!!! warning - - Under Construction diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/.pages b/docs/how-to-guides/integrating-autoware/launch-autoware/.pages new file mode 100644 index 00000000000..25f992ebd0d --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/.pages @@ -0,0 +1,4 @@ +nav: + - index.md + - Localization mode: localization-mode + - Perception mode: perception.md diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_goal_pose.png b/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_goal_pose.png new file mode 100644 index 00000000000..5f394ce73ce Binary files /dev/null and b/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_goal_pose.png differ diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_pose_estimate.png b/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_pose_estimate.png new file mode 100644 index 00000000000..e6ebf3197ec Binary files /dev/null and b/docs/how-to-guides/integrating-autoware/launch-autoware/images/2d_pose_estimate.png differ diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_after.png b/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_after.png new file mode 100644 index 00000000000..687cd150d2e Binary files /dev/null and b/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_after.png differ diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_before.png b/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_before.png new file mode 100644 index 00000000000..7d9147521dd Binary files /dev/null and b/docs/how-to-guides/integrating-autoware/launch-autoware/images/autoware_state_panel_before.png differ diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/images/route_planning_is_complete.png b/docs/how-to-guides/integrating-autoware/launch-autoware/images/route_planning_is_complete.png new file mode 100644 index 00000000000..59557f912c3 Binary files /dev/null and b/docs/how-to-guides/integrating-autoware/launch-autoware/images/route_planning_is_complete.png differ diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/index.md b/docs/how-to-guides/integrating-autoware/launch-autoware/index.md new file mode 100644 index 00000000000..3195fb2bc3d --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/index.md @@ -0,0 +1,87 @@ +# Launch Autoware + +!!! warning + + Under Construction + +This section explains how to run your vehicle with Autoware. + +## Install Autoware + +Follow the [installation steps of Autoware](../../../installation/). + +## Launch Autoware + +Launch Autoware with the following command: + +```bash +ros2 launch autoware_launch autoware.launch.xml vehicle_model:=YOUR_VEHICLE sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP +``` + +It is possible to specify which components to launch using command-line arguments. +For example, if you don't need to launch perception, planning, and control for localization debug, you can launch the following: + +```bash +ros2 launch autoware_launch autoware.launch.xml vehicle_model:=YOUR_VEHICLE sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP \ + launch_perception:=false \ + launch_planning:=false \ + launch_control:=false +``` + +The basic command-line options are documented in [autoware.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/autoware.launch.xml). + +There are options available to switch between different methods for some component. +For example, by specifying `localization_mode` or `perception_mode`, you can switch localization and perception methods, respectively. +These options allow you to choose the desired algorithms or sensor configurations for the respective functionalities. + +For options on eagleye component, please refer to the sub-pages. + +- [localization-mode](localization-mode/index.md) +- [perception-mode](perception.md) + +## Set initial pose + +If GNSS is available, Autoware automatically initializes the vehicle's pose. + +If not or if the automatic initialization returns an incorrect position, you need to set the initial pose using the RViz GUI. + +1. Click the 2D Pose estimate button in the toolbar, or hit the P key + + ![2D Pose estimate](images/2d_pose_estimate.png) + +2. In the 3D View pane, click and hold the left mouse button, and then drag to set the direction for the initial pose. + +## Set goal pose + +Set a goal pose for the ego vehicle. + +1. Click the 2D Nav Goal button in the toolbar, or hit the G key + + ![2D Pose estimate](images/2d_goal_pose.png) + +2. In the 3D View pane, click and hold the left mouse button, and then drag to set the direction for the goal pose. + If successful, you will see the calculated planning path on RViz. + + ![route planning](images/route_planning_is_complete.png){width="800"} + +## Engage + +In your terminal, execute the following command. + +```bash +source ~/autoware.YOURS/install/setup.bash +ros2 topic pub /autoware.YOURS/engage autoware_auto_vehicle_msgs/msg/Engage "engage: true" -1 +``` + +You can also engage via RViz with "AutowareStatePanel". +The panel can be found in `Panels > Add New Panel > tier4_state_rviz_plugin > AutowareStatePanel`. + +Once the route is computed, the "AUTO" button becomes active. Pressing the AUTO button engages the autonomous driving mode. + +![autoware state panel](images/autoware_state_panel_before.png) + +Now the vehicle should drive along the calculated path! + +During the autonomous driving, the StatePanel appears as shown in the image below. Pressing the "STOP" button allows you to stop the vehicle. + +![autoware state panel](images/autoware_state_panel_after.png) diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/.pages b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/.pages new file mode 100644 index 00000000000..24441b7323f --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/.pages @@ -0,0 +1,3 @@ +nav: + - index.md + - Eagleye: eagleye-guide.md diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/eagleye-guide.md b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/eagleye-guide.md new file mode 100644 index 00000000000..722f99b1830 --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/eagleye-guide.md @@ -0,0 +1,120 @@ +# Using Eagleye with Autoware + +This page will show you how to set up [Eagleye](https://github.com/MapIV/eagleye) in order to use it with Autoware. +For the details of the integration proposal, please refer to [this discussion](https://github.com/orgs/autowarefoundation/discussions/3257). + +## What is Eagleye? + +Eagleye is an open-source GNSS/IMU-based localizer initially developed by [MAP IV. Inc](https://map4.jp/). It provides a cost-effective alternative to LiDAR and point cloud-based localization by using low-cost GNSS and IMU sensors to provide vehicle position, orientation, and altitude information. + +### Dependencies + +The below packages are automatically installed during the setup of Autoware as they are listed in [autoware.repos](https://github.com/autowarefoundation/autoware/blob/main/autoware.repos). + +1. [Eagleye](https://github.com/MapIV/eagleye.git) (autoware-main branch) +2. [RTKLIB ROS Bridge](https://github.com/MapIV/rtklib_ros_bridge.git) (ros2-v0.1.0 branch) +3. [LLH Converter](https://github.com/MapIV/llh_converter.git) (ros2 branch) + +## Architecture + +Eagleye can be utilized in the Autoware localization stack in two ways: + +1. Feed only twist into the EKF localizer. + + ![Eagleye twist integration](images/eagleye-integration-guide/eagleye_twist.drawio.svg) + +2. Feed both twist and pose from Eagleye into the EKF localizer (twist can also be used with regular `gyro_odometry`). + + ![Eagleye pose twist integration](images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg) + +**Note: RTK positioning is required when using Eagleye as the pose estimator.** +On the other hand, it is not mandatory when using it as the twist estimator. + +## Requirements + +Eagleye requires GNSS, IMU and vehicle speed as inputs. + +### IMU topic + +`sensor_msgs/msg/Imu` is supported for Eagleye IMU input. + +### Vehicle speed topic + +`geometry_msgs/msg/TwistStamped` and `geometry_msgs/msg/TwistWithCovarianceStamped` are supported for the input vehicle speed. + +### GNSS topic + +Eagleye requires latitude/longitude height and doppler velocity generated by the GNSS receiver. +Your GNSS ROS driver must publish the following messages: + +- `sensor_msgs/msg/NavSatFix`: This message contains latitude, longitude, and height information. +- `geometry_msgs/msg/TwistWithCovarianceStamped`: This message contains gnss doppler velocity information. + +Eagleye has been tested with the following example GNSS ROS drivers: ublox_gps and septentrio_gnss_driver. The settings needed for each of these drivers are as follows: + +| GNSS ROS drivers | modification | +| --------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| [ublox_gps](https://github.com/KumarRobotics/ublox/tree/ros2/ublox_gps) | No additional settings are required. It publishes `sensor_msgs/msg/NavSatFix` and `geometry_msgs/msg/TwistWithCovarianceStamped` required by Eagleye with default settings. | +| [septentrio_gnss_driver](https://github.com/septentrio-gnss/septentrio_gnss_driver/tree/ros2) | Set `publish.navsatfix` and `publish.twist` in the config file [`gnss.yaml`](https://github.com/septentrio-gnss/septentrio_gnss_driver/blob/ros2/config/gnss.yaml#L90) to `true` | + +## Parameter Modifications for Integration into Your Vehicle + +### topic name & topic type + +The users must correctly specify input topics for GNSS latitude, longitude, and height , GNSS doppler speed , IMU , and vehicle speed in the [`eagleye_config.yaml`](https://github.com/MapIV/autoware_launch/blob/3f04a9dd7bc4a4c49d4ec790e3f6b9958ab822da/autoware_launch/config/localization/eagleye_config.param.yaml#L7-L16). + +```yaml +# Topic +twist: + twist_type: 1 # TwistStamped : 0, TwistWithCovarianceStamped: 1 + twist_topic: /sensing/vehicle_velocity_converter/twist_with_covariance +imu_topic: /sensing/imu/tamagawa/imu_raw +gnss: + velocity_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3 + velocity_source_topic: /sensing/gnss/ublox/navpvt + llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2 + llh_source_topic: /sensing/gnss/ublox/nav_sat_fix +``` + +### sensor frequency + +Also, the frequency of GNSS and IMU must be set in [`eagleye_config.yaml`](https://github.com/MapIV/autoware_launch/blob/3f04a9dd7bc4a4c49d4ec790e3f6b9958ab822da/autoware_launch/config/localization/eagleye_config.param.yaml#L36) + +```yaml +common: + imu_rate: 50 + gnss_rate: 5 +``` + +### Conversion from fix to pose + +The parameters for converting `sensor_msgs/msg/NavSatFix` to `geometry_msgs/msg/PoseWithCovarianceStamped` is listed in [`fix2pose.yaml`](https://github.com/MapIV/eagleye/blob/autoware-main/eagleye_util/fix2pose/launch/fix2pose.xml). +If you use a different geoid or projection type, change these parameters. + +### Other parameters + +The other parameters are described [here](https://github.com/MapIV/eagleye/tree/autoware-main/eagleye_rt/config). +Basically, these do not need to be changed . + +## Notes on initialization + +Eagleye requires an initialization process for proper operation. **Without initialization, the output for twist will be in the raw value, and the pose data will not be available.** + +### 1. Static Initialization + +The first step is static initialization, which involves allowing the Eagleye to remain stationary for approximately 5 seconds after startup to estimate the yaw-rate offset. + +### 2. Dynamic initialization + +The next step is dynamic initialization, which involves running the Eagleye in a straight line for approximately 30 seconds. This process estimates the scale factor of wheel speed and azimuth angle. + +Once dynamic initialization is complete, the Eagleye will be able to provide corrected twist and pose data. + +### How to check the progress of initialization + +- **TODO** + +## Note on georeferenced maps + +Note that the output position might not appear to be in the point cloud maps if you are using maps that are not properly georeferenced. +In the case of a single GNSS antenna, initial position estimation (dynamic initialization) can take several seconds to complete after starting to run in an environment where GNSS positioning is available. diff --git a/docs/how-to-guides/others/images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg similarity index 100% rename from docs/how-to-guides/others/images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg rename to docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg diff --git a/docs/how-to-guides/others/images/eagleye-integration-guide/eagleye_twist.drawio.svg b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/images/eagleye-integration-guide/eagleye_twist.drawio.svg similarity index 100% rename from docs/how-to-guides/others/images/eagleye-integration-guide/eagleye_twist.drawio.svg rename to docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/images/eagleye-integration-guide/eagleye_twist.drawio.svg diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/index.md b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/index.md new file mode 100644 index 00000000000..4846b766ffe --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/localization-mode/index.md @@ -0,0 +1,129 @@ +# Localization mode + +The Autoware provides multiple localization methods that work with multiple different sensor configurations. +The table below shows the supported sensor configurations and their corresponding algorithms. + +| localization mode | algorithm | map type | +| ----------------- | --------- | --------------- | +| LiDAR-based | NDT | point cloud map | +| Camera-based | YabLoc | vector map | +| GNSS/IMU-based | Eagleye | - | + +## LiDAR-based localizer (default) + +By default, Autoware launches [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) for localization. + +## Camera-based localizer + +You can use YabLoc as a camera-based localization method. +For more details on YabLoc, please refer to the [README of YabLoc](https://github.com/autowarefoundation/autoware.universe/blob/main/localization/yabloc/README.md) in autoware.universe. + +To use YabLoc as a pose_estimator, add `localization_mode:=camera` when launching Autoware. +By default, the `localization_mode` is set to `lidar`. +By specifying this command-line argument, YabLoc nodes will be automatically launched while the NDT nodes will not be started. + +Here is an example of a launch command: + +```bash +ros2 launch autoware_launch autoware.launch.xml \ + vehicle_model:=YOUR_VEHICLE \ + sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP \ + localization_mode:=camera # Add this argument +``` + +## GNSS/IMU-based localizer + +You can use Eagleye as a GNSS/IMU-based localization method. For more details on Eagleye, please refer to the [Eagleye](eagleye-guide.md). + +Eagleye has a function for position estimation and twist estimation, namely `pose_estimator` and `twist_estimator`, respectively. +When running Eagleye in twist_estimator mode, ndt_scan_matcher is used as the pose_estimator. +Eagleye will improve scan matching by providing accurate twists using GNSS doppler. + +To use Eagleye, it requires both specifying the command-line arguments and modifying the launch file. + +Firstly, please modify the commented-out line and change it to launch `map4_localization_component.launch.xml` instead of `tier4_localization_component.launch.xml` in [`autoware.launch.xml`](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/autoware.launch.xml). +Please refer to the following snippet for the modification details: + +```xml + + + + + +``` + +NOTE: Please refer to [`map4_localization_launch`](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/map4_localization_launch) in the `autoware.universe` package and [`map4_localization_component.launch.xml`](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/map4_localization_component.launch.xml) in `autoware_launch` package for information on how to modify the localization launch. + +Once you have modified the launch file, you can use Eagleye by specifying the `pose_estimator_mode` through command-line arguments. + +**Example of using Eagleye as the pose twist estimator:** + +```bash +ros2 launch autoware_launch autoware.launch.xml \ + vehicle_model:=YOUR_VEHICLE \ + sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP \ + pose_estimator_mode:=gnss # Add this argument +``` + +**Example of using Eagleye as the twist estimator:** + +```bash +ros2 launch autoware_launch autoware.launch.xml \ + vehicle_model:=YOUR_VEHICLE \ + sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP \ + pose_estimator_mode:=lidar # Not mandatory, as it is the default. +``` + +## Comprehensive Options Table + +The table below indicates which pose_estimator and twist_estimator are called based on the invoked launch file and provided arguments. + + + + + + + + + + + + + + + + + + + + + + + + +
tier4_localization_component.launch.xml
localization_modepose_estimatortwist_estimator
lidar (default)ndt_scan_matchergyro_odometer
camerayablocgyro_odometer
+ + + + + + + + + + + + + + + + + + + + + + + + +
map4_localization_component.launch.xml
pose_estimator_modepose_estimatortwist_estimator
lidar (default)ndt_scan_matchereagleye
gnsseagleyeeagleye
diff --git a/docs/how-to-guides/integrating-autoware/launch-autoware/perception.md b/docs/how-to-guides/integrating-autoware/launch-autoware/perception.md new file mode 100644 index 00000000000..708715e6b43 --- /dev/null +++ b/docs/how-to-guides/integrating-autoware/launch-autoware/perception.md @@ -0,0 +1,33 @@ +# Perception mode + +!!! warning + + Under Construction + +By specifying the `perception_mode`, users can switch between different sensor configurations for perception. +This allows you to choose the specific sensor setup that you want to use for the perception tasks. + +```bash +ros2 launch autoware_launch autoware.launch.xml vehicle_model:=YOUR_VEHICLE sensor_kit:=YOUR_SENSOR_KIT map_path:=/PATH/TO/YOUR/MAP \ + perception_mode:=lidar +``` + +## LiDAR + +`perception_mode:=lidar` + +## Radar + +`perception_mode:=radar` + +## Camera LiDAR fusion + +`perception_mode:=camera_lidar_fusion` + +## Camera LiDAR Radar fusion + +`perception_mode:=camera_lidar_radar_fusion` + +## LiDAR Radar fusion + +`perception_mode:=lidar_radar_fusion` diff --git a/docs/how-to-guides/others/eagleye-integration-guide.md b/docs/how-to-guides/others/eagleye-integration-guide.md deleted file mode 100644 index 8a4a29b0bae..00000000000 --- a/docs/how-to-guides/others/eagleye-integration-guide.md +++ /dev/null @@ -1,128 +0,0 @@ -# Using Eagleye with Autoware - -This page will show you how to set up [Eagleye](https://github.com/MapIV/eagleye) in order to use it with Autoware. -For the details of the integration proposal, please refer to [this](https://github.com/orgs/autowarefoundation/discussions/3257) Discussion. - -## What is Eagleye? - -Eagleye is an open-source GNSS/IMU-based localizer initially developed by [MAP IV. Inc](https://map4.jp/). It provides a cost-effective alternative to LiDAR and point cloud-based localization by using low-cost GNSS and IMU sensors to provide vehicle position, orientation, and altitude information. By integrating Eagleye into Autoware, users can choose between LiDAR and point cloud-based localization stacks or GNSS/IMU-based Eagleye localizer, depending on their specific needs and operating environment. - -## Architecture - -Eagleye can be utilized in the Autoware localization stack in two ways: - -1. Feed only twist into the EKF localizer. - - ![Eagleye twist integration](images/eagleye-integration-guide/eagleye_twist.drawio.svg) - -2. Feed both twist and pose from Eagleye into the EKF localizer (twist can also be used with regular `gyro_odometry`). - - ![Eagleye pose twist integration](images/eagleye-integration-guide/eagleye_pose_twist.drawio.svg) - -Note that RTK positioning is only required for localization using the Eagleye pose. RTK positioning is not required for twist. - -## Requirements - -GNSS/IMU/vehicle speed is required for Eagleye input. - -### IMU topic - -`sensor_msgs/msg/Imu` are supported for IMU. - -### Vehicle speed topic - -`geometry_msgs/msg/TwistStamped` and `geometry_msgs/msg/TwistWithCovarianceStamped` are supported for the input vehicle speed. - -### GNSS topic - -Eagleye requires latitude/longitude height information and velocity information generated by the GNSS receiver. -Your GNSS ROS driver must publish the following messages: - -- `sensor_msgs/msg/NavSatFix`: This message contains latitude, longitude, and height information. -- `geometry_msgs/msg/TwistWithCovarianceStamped`: This message contains gnss doppler velocity information. - -Eagleye has been tested with the following example GNSS ROS drivers: ublox_gps and septentrio_gnss_driver. The settings needed for each of these drivers are as follows: - -- [ublox_gps](https://github.com/KumarRobotics/ublox/tree/ros2/ublox_gps): This ROS driver publishes `sensor_msgs/msg/NavSatFix` and `geometry_msgs/msg/TwistWithCovarianceStamped` required by Eagleye with default settings. Therefore, no additional settings are required. -- [septentrio_gnss_driver](https://github.com/septentrio-gnss/septentrio_gnss_driver/tree/ros2): Set `publish.navsatfix` and `publish.twist` in the config file [`gnss.yaml`](https://github.com/septentrio-gnss/septentrio_gnss_driver/blob/ros2/config/gnss.yaml#L90) to `true` - -## Eagleye Setup - -### Install dependencies - -Clone the following three packages for Eagleye: - -1. [Eagleye](https://github.com/MapIV/eagleye.git) (autoware-main branch) -2. [RTKLIB ROS Bridge](https://github.com/MapIV/rtklib_ros_bridge.git) (ros2-v0.1.0 branch) -3. [LLH Converter](https://github.com/MapIV/llh_converter.git) (ros2 branch) - -### Modifying Autoware Launch files - -You need to install Eagleye-related packages and change Autoware's launcher. Four files are required in the Autoware localization launcher to run Eagleye: `eagleye_rt.launch.xml`, `eagleye_config.yaml`, `gnss_converter.launch.xml`, and `fix2pose.launch.xml`. - -You must correctly specify input topics for GNSS latitude, longitude, and height information, GNSS speed information, IMU information, and vehicle speed information in the [`eagleye_config.yaml`](https://github.com/MapIV/autoware_launch/blob/3f04a9dd7bc4a4c49d4ec790e3f6b9958ab822da/autoware_launch/config/localization/eagleye_config.param.yaml#L7-L16). - -```yaml -# Topic -twist: - twist_type: 1 # TwistStamped : 0, TwistWithCovarianceStamped: 1 - twist_topic: /sensing/vehicle_velocity_converter/twist_with_covariance -imu_topic: /sensing/imu/tamagawa/imu_raw -gnss: - velocity_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3 - velocity_source_topic: /sensing/gnss/ublox/navpvt - llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2 - llh_source_topic: /sensing/gnss/ublox/nav_sat_fix -``` - -Also, the frequency of GNSS and IMU must be set in [`eagleye_config.yaml`](https://github.com/MapIV/autoware_launch/blob/3f04a9dd7bc4a4c49d4ec790e3f6b9958ab822da/autoware_launch/config/localization/eagleye_config.param.yaml#L36) - -```yaml -common: - imu_rate: 50 - gnss_rate: 5 -``` - -The basic parameters that do not need to be changed except those mentioned above, i.e., topic names and sensors' frequency, are described below [here](https://github.com/MapIV/eagleye/tree/autoware-main/eagleye_rt/config). -Additionally, the parameters for converting `sensor_msgs/msg/NavSatFix` to `geometry_msgs/msg/PoseWithCovarianceStamped` is listed in [`fix2pose.yaml`](https://github.com/MapIV/eagleye/blob/autoware-main/eagleye_util/fix2pose/launch/fix2pose.xml). - -Please refer to `map4_localization_launch` in the `autoware.universe` package and `map4_localization_component.launch.xml` in `autoware_launch` package for information on how to modify the localization launch. - -Eagleye has a function for position estimation and a function for twist estimation, namely `pose_estimator` and `twist_estimator`, respectively. - -| 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) | - -In Autoware, you can set the pose estimator to GNSS by setting `pose_estimator_mode:=gnss` in `map4_localization_component.launch.xml` in `autoware_launch` package. -Note that the output position might not appear to be in the point cloud maps if you are using maps that are not properly georeferenced. -In the case of a single GNSS antenna, initial position estimation (dynamic initialization) can take several seconds to complete after starting to run in an environment where GNSS positioning is available. - -Alternatively, the twist estimator can be set to Eagleye and the pose estimator to NDT by specifying `pose_estimator_mode:=lidar` in the same launch file. -Unlike Eagleye position estimation, Eagleye twist estimation first outputs uncorrected raw values when activated, and then outputs corrected twists as soon as static initialization is complete. - -## Executing Launch files - -### Execution command - -Enable Eagleye in Autoware by switching the localization module in autoware.launch.xml and the `pose_estimator_mode` parameter in `map4_localization_component.launch.xml` in `autoware.launch.xml`. - -When using Eagleye, comment out `tier4_localization_component.launch.xml` and start `map4_localization_component.launch.xml` in `autoware.launch.xml`. - -```xml - - - - - -``` - -#### Notes with initialization - -Eagleye requires an initialization process for proper operation. Without initialization, the output for twist will be in the raw value, and the pose data will not be available. - -The first step is static initialization, which involves allowing the Eagleye to remain stationary for approximately 5 seconds after startup to estimate the yaw-rate offset. - -The next step is dynamic initialization, which involves running the Eagleye in a straight line for approximately 30 seconds. This process estimates the scale factor of wheel speed and azimuth angle. Once dynamic initialization is complete, the Eagleye will be able to provide corrected twist and pose data.