Skip to content

autoware_pose_initializer#

Purpose#

The autoware_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. This node depends on the map height fitter library. See here for more details.

Interfaces#

Parameters#

Name Type Description Default Range
user_defined_initial_pose.enable string If true, user_defined_initial_pose.pose is set as the initial position. [boolean] false N/A
user_defined_initial_pose.pose string initial pose (x, y, z, quat_x, quat_y, quat_z, quat_w). [array] [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] N/A
gnss_pose_timeout float The duration that the GNSS pose is valid. [sec] 3.0 ≥0.0
pose_error_check_enabled boolean If true, check error between initial pose result and GNSS pose. false N/A
pose_error_threshold float The error threshold between GNSS and initial pose 5.0 ≥0.0
stop_check_enabled string If true, initialization is accepted only when the vehicle is stopped. true N/A
stop_check_duration float The duration used for the stop check above. [sec] 3.0 ≥0.0
ekf_enabled string If true, EKF localizer is activated. true N/A
gnss_enabled string If true, use the GNSS pose when no pose is specified. true N/A
yabloc_enabled string If true, YabLocModule is used. true N/A
ndt_enabled string If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. true N/A
gnss_particle_covariance array gnss particle covariance [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0] N/A
output_pose_covariance array output pose covariance [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2] N/A

Services#

Name Type Description
/localization/initialize tier4_localization_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_adapi_v1_msgs::msg::LocalizationInitializationState pose initialization state
/initialpose3d geometry_msgs::msg::PoseWithCovarianceStamped calculated initial ego pose
/diagnostics diagnostic_msgs::msg::DiagnosticArray diagnostics

Diagnostics#

pose_initializer_status#

If the score of initial pose estimation result is lower than score threshold, ERROR message is output to the /diagnostics topic.

drawing

Connection with Default AD API#

This autoware_pose_initializer is used via default AD API. For detailed description of the API description, please refer to the description of autoware_default_adapi.

drawing

Initialize pose via CLI#

Using the GNSS estimated position#

ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization

The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.

Using the input position#

ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
pose_with_covariance:
  - header:
      frame_id: map
    pose:
      pose:
        position:
          x: 89571.1640625
          y: 42301.1875
          z: -3.1565165519714355
        orientation:
          x: 0.0
          y: 0.0
          z: 0.28072773940524687
          w: 0.9597874433062874
      covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
method: 0
"

The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.

Direct initial position set#

ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
pose_with_covariance:
  - header:
      frame_id: map
    pose:
      pose:
        position:
          x: 89571.1640625
          y: 42301.1875
          z: -3.1565165519714355
        orientation:
          x: 0.0
          y: 0.0
          z: 0.28072773940524687
          w: 0.9597874433062874
      covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
method: 1
"

The initial position is set directly by the input position without going through localization algorithm.

Via ros2 topic pub#

ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
header:
  frame_id: map
pose:
  pose:
    position:
      x: 89571.1640625
      y: 42301.1875
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.28072773940524687
      w: 0.9597874433062874
"

It behaves the same as "initialpose (from rviz)". The position.z and the covariance will be overwritten by ad_api_adaptors, so there is no need to input them.