-
Notifications
You must be signed in to change notification settings - Fork 683
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
- Loading branch information
1 parent
2f18a95
commit 9a6e85f
Showing
3 changed files
with
70 additions
and
45 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,47 +1,4 @@ | ||
| # pose_initializer | ||
|
|
||
| ## Purpose | ||
|
|
||
| `pose_initializer` is the package to send an initial pose to `ekf_localizer`. | ||
| It receives roughly estimated initial pose from GNSS/user. | ||
| Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service. | ||
| Finally, it publishes the initial pose to `ekf_localizer`. | ||
|
|
||
| ## Interfaces | ||
|
|
||
| ### Parameters | ||
|
|
||
| | Name | Type | Description | | ||
| | --------------------- | ---- | ---------------------------------------------------------------------------------------- | | ||
| | `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | | ||
| | `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | | ||
| | `stop_check_duration` | bool | The duration used for the stop check above. | | ||
| | `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | | ||
| | `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | | ||
|
|
||
| ### Services | ||
|
|
||
| | Name | Type | Description | | ||
| | -------------------------- | ------------------------------------------------- | --------------------- | | ||
| | `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api | | ||
|
|
||
| ### Clients | ||
|
|
||
| | Name | Type | Description | | ||
| | -------------------------------------------- | ------------------------------------------------------- | ----------------------- | | ||
| | `/localization/pose_estimator/ndt_align_srv` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose estimation service | | ||
|
|
||
| ### Subscriptions | ||
|
|
||
| | Name | Type | Description | | ||
| | ----------------------------------------------------------- | --------------------------------------------- | -------------------- | | ||
| | `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss | | ||
| | `/sensing/vehicle_velocity_converter/twist_with_covariance` | geometry_msgs::msg::TwistStamped | twist for stop check | | ||
| | `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map | | ||
|
|
||
| ### Publications | ||
|
|
||
| | Name | Type | Description | | ||
| | ------------------------------------ | ---------------------------------------------------------- | --------------------------- | | ||
| | `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state | | ||
| | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | | ||
| - [pose_initializer](./docs/pose_initializer.md) | ||
| - [map_height_fitter](./docs/map_height_fitter.md) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,22 @@ | ||
| # map_height_fitter | ||
|
|
||
| ## Purpose | ||
|
|
||
| This node provides a service to fit a pose height to a map. | ||
| Use this service as preprocessing for `pose_initializer` when using a initial poses with inaccurate height such as RViz and GNSS. | ||
| This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius. | ||
| If no point is found in this range, returns the input pose without changes. | ||
|
|
||
| ## Interfaces | ||
|
|
||
| ### Services | ||
|
|
||
| | Name | Type | Description | | ||
| | ----------------------------------- | ------------------------------------------------------- | -------------------- | | ||
| | `/localization/util/fit_map_height` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose fitting service | | ||
|
|
||
| ### Subscriptions | ||
|
|
||
| | Name | Type | Description | | ||
| | --------------------- | ----------------------------- | -------------- | | ||
| | `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map | |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| # pose_initializer | ||
|
|
||
| ## Purpose | ||
|
|
||
| `pose_initializer` is the package to send an initial pose to `ekf_localizer`. | ||
| It receives roughly estimated initial pose from GNSS/user. | ||
| Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service. | ||
| Finally, it publishes the initial pose to `ekf_localizer`. | ||
|
|
||
| ## Interfaces | ||
|
|
||
| ### Parameters | ||
|
|
||
| | Name | Type | Description | | ||
| | --------------------- | ---- | ---------------------------------------------------------------------------------------- | | ||
| | `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | | ||
| | `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | | ||
| | `stop_check_duration` | bool | The duration used for the stop check above. | | ||
| | `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | | ||
| | `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | | ||
|
|
||
| ### Services | ||
|
|
||
| | Name | Type | Description | | ||
| | -------------------------- | ------------------------------------------------- | --------------------- | | ||
| | `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api | | ||
|
|
||
| ### Clients | ||
|
|
||
| | Name | Type | Description | | ||
| | -------------------------------------------- | ------------------------------------------------------- | ----------------------- | | ||
| | `/localization/pose_estimator/ndt_align_srv` | tier4_localization_msgs::srv::PoseWithCovarianceStamped | pose estimation service | | ||
|
|
||
| ### Subscriptions | ||
|
|
||
| | Name | Type | Description | | ||
| | ----------------------------------------------------------- | --------------------------------------------- | -------------------- | | ||
| | `/sensing/gnss/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | pose from gnss | | ||
| | `/sensing/vehicle_velocity_converter/twist_with_covariance` | geometry_msgs::msg::TwistStamped | twist for stop check | | ||
|
|
||
| ### Publications | ||
|
|
||
| | Name | Type | Description | | ||
| | ------------------------------------ | ---------------------------------------------------------- | --------------------------- | | ||
| | `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state | | ||
| | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | |