Proposal for Simplifying Pose Initialization in Autoware

[xmfcx]

Proposal for Simplifying Pose Initialization in Autoware

The current pose initialization architecture in Autoware is complex and contains redundant parameters. Specifically, the use of an explicit method parameter in the InitializeLocalization.srv service leads to unnecessary complexity.

This proposal aims to simplify the initialization process by inferring the initialization method from the covariance values of the provided pose, thereby eliminating the need for the method parameter.

Related PRs and Issues:
These PRs/Issues were made before this proposal, we'll close them. Let's discuss and come to a consensus first.

• fix(pose_initializer): add topic subscription to direct initialization autoware.universe#8941
• fix(pose_initializer): switch direct init with gnss cov check autoware.universe#8948
• Add Direct Initialization Option with GNSS pose autoware.universe#8940

Initialization service

We currently use tier4_localization_msgs/srv/InitializeLocalization.srv

# constants for specifying initialization method
uint8 AUTO = 0
uint8 DIRECT = 1

# input pose for initialization
geometry_msgs/PoseWithCovarianceStamped[<=1] pose_with_covariance

# Variable to set initialization method
# AUTO: The initial position is automatically estimated with localization algorithm.
#       The input pose will be used as an initial guess if provided.
# DIRECT: The initial position is set directly by the input pose without going through localization algorithm.
uint8 method
---
# ERROR CODES used in response status in case of failure
uint16 ERROR_UNSAFE = 1
uint16 ERROR_GNSS_SUPPORT = 2
uint16 ERROR_GNSS = 3
uint16 ERROR_ESTIMATION = 4

autoware_common_msgs/ResponseStatus status

There is no need for the method parameter. We can infer the method from the covariance values of the pose_with_covariance parameter.

This makes sense because the covariance values can define the search space for the pose. If it's very small, it means that the pose is very accurate and we can use it directly.

• Zero Covariance: Indicates high confidence in the pose. The system will use the pose directly for initialization.
• Non-Zero Covariance: Indicates uncertainty in the pose. The system will use localization algorithms (e.g., NDT alignment) to refine the pose.

We can use autoware_adapi_v1_msgs/localization/srv/InitializeLocalization.srv as it is.

Utilize the existing AD API service definition to maintain consistency and reduce redundancy.

Maybe we can just add 2 comment lines to make it clear that the covariance values are used to infer the initialization method.

geometry_msgs/PoseWithCovarianceStamped[<=1] pose

# If the covariance values are zero, the pose will be used directly for initialization.
# If the covariance values are non-zero, the GNSS covariance values will be used for initialization.
---
uint16 ERROR_UNSAFE = 1
uint16 ERROR_GNSS_SUPPORT = 2
uint16 ERROR_GNSS = 3
uint16 ERROR_ESTIMATION = 4
autoware_adapi_v1_msgs/ResponseStatus status

Pose initialization architecture changes

Existing architecture

flowchart TD
  subgraph pose_initializer["Pose Initializer"]
    init_method{{"service parameter:<br/>initialization method"}}
    Auto
    Direct
    trigger_ekf_and_ndt["trigger ekf and ndt"]
  end


  subgraph Auto["AUTO"]
    init_pose_is_filled{{"service call contains a pose"}}
    get_gnss_pose["get gnss pose"]
    use_service_pose2["use the service pose"]
    override_gnss_cov(["override gnss cov<br/>from config"])
    ndt_align_pose["ndt->align_pose()"]
  end
  subgraph Direct["DIRECT"]
    use_service_pose["use the service pose"]

  end

  gnss_poser -- /sensing/gnss/pose_with_covariance --> get_gnss_pose["get gnss pose"]

  pose_via_cli["Pose via CLI"] -- /localization/initialize--> init_method
  ad_api["AD API"] -- /localization/initialize --> init_method
  init_method -- AUTO --> Auto
  init_method -- DIRECT --> Direct
  init_pose_is_filled -- yes --> use_service_pose2
  init_pose_is_filled -- no --> get_gnss_pose
  get_gnss_pose --> override_gnss_cov
  override_gnss_cov --> ndt_align_pose
  use_service_pose2 --> ndt_align_pose
  ndt_align_pose --> trigger_ekf_and_ndt
  use_service_pose --> trigger_ekf_and_ndt
  ndt_align_pose <-- /localization/pose_estimator/ndt_align_srv --> ndt_scan_matcher["ndt_scan_matcher"]
  trigger_ekf_and_ndt -->|/localization/pose_estimator/trigger_node| ndt_scan_matcher
  trigger_ekf_and_ndt --> |/localization/pose_twist_fusion_filter/trigger_node| ekf_localizer["ekf_localizer"]
  pose_initializer -->|/initialpose3D| ekf_localizer
  
  init_method:::cl_conditional
  use_service_pose:::cl_purple
  Auto:::cl_output
  Direct:::cl_output
  trigger_ekf_and_ndt:::cl_purple
  init_pose_is_filled:::cl_conditional
  get_gnss_pose:::cl_purple
  use_service_pose2:::cl_purple
  override_gnss_cov:::cl_rose
  ndt_align_pose:::cl_purple
  pose_via_cli:::cl_node
  ad_api:::cl_node
  ndt_scan_matcher:::cl_node
  ekf_localizer:::cl_node
  pose_initializer:::cl_node
  gnss_poser:::cl_node
  
  classDef cl_node fill:#FFF2CC, stroke:#D6B656, stroke-width:3px
  classDef cl_conditional fill:#FFE6CC, stroke:#D79B00, stroke-width:3px
  classDef cl_output fill:#D5E8D4, stroke:#82B366, stroke-width:3px
  classDef cl_purple fill:#E1D5E7, stroke:#9673A6, stroke-width:3px
  classDef cl_rose fill:#FFDFE5, stroke:#FF5978, stroke-width:3px

Loading

Proposal

flowchart TD
  subgraph pose_initializer["Pose Initializer"]

  init_pose_is_filled{{"service call contains a pose"}}
  get_gnss_pose["get gnss pose"]
  load_cov_param{{"load covariance from param"}}
  use_gnss_cov(["use gnss cov"])
  use_service_pose2["use the service pose"]
  cov_zero{{"covariance is zero"}}
  override_gnss_cov(["override gnss cov<br/>from config"])
  ndt_align_pose["ndt->align_pose()"]

  trigger_ekf_and_ndt["trigger ekf and ndt"]

  initialize_directly["initialize directly"]
  end


  gnss_poser -- /sensing/gnss/pose_with_covariance --> get_gnss_pose["get gnss pose"]

  pose_via_cli["Pose via CLI"] -- /localization/initialize--> init_pose_is_filled
  ad_api["AD API"] -- /localization/initialize --> init_pose_is_filled
  init_pose_is_filled -- yes --> use_service_pose2
  init_pose_is_filled -- no --> get_gnss_pose
  get_gnss_pose --> load_cov_param
  load_cov_param -- yes --> override_gnss_cov
  load_cov_param -- no --> use_gnss_cov

  use_gnss_cov --> cov_zero
  override_gnss_cov --> cov_zero
  use_service_pose2 --> cov_zero

  cov_zero -- no --> ndt_align_pose
  cov_zero -- yes --> initialize_directly

  ndt_align_pose --> trigger_ekf_and_ndt
  initialize_directly --> trigger_ekf_and_ndt

  ndt_scan_matcher["ndt_scan_matcher"]

  ndt_align_pose <-- /localization/pose_estimator/ndt_align_srv --> ndt_scan_matcher
  trigger_ekf_and_ndt -->|/localization/pose_estimator/trigger_node| ndt_scan_matcher
  trigger_ekf_and_ndt --> |/localization/pose_twist_fusion_filter/trigger_node| ekf_localizer["ekf_localizer"]

  pose_initializer -->|/initialpose3D| ekf_localizer

  initialize_directly:::cl_purple
  trigger_ekf_and_ndt:::cl_purple
  init_pose_is_filled:::cl_conditional
  get_gnss_pose:::cl_purple
  use_service_pose2:::cl_purple
  override_gnss_cov:::cl_rose
  ndt_align_pose:::cl_purple
  pose_via_cli:::cl_node
  ad_api:::cl_node
  ndt_scan_matcher:::cl_node
  ekf_localizer:::cl_node
  pose_initializer:::cl_node
  gnss_poser:::cl_node
  load_cov_param:::cl_conditional
  cov_zero:::cl_conditional
  use_gnss_cov:::cl_rose
  
  classDef cl_node fill:#FFF2CC, stroke:#D6B656, stroke-width:3px
  classDef cl_conditional fill:#FFE6CC, stroke:#D79B00, stroke-width:3px
  classDef cl_output fill:#D5E8D4, stroke:#82B366, stroke-width:3px
  classDef cl_purple fill:#E1D5E7, stroke:#9673A6, stroke-width:3px
  classDef cl_rose fill:#FFDFE5, stroke:#FF5978, stroke-width:3px

Loading

Comparison of Existing vs. Proposed Architecture:

Existing Architecture

• Method-based decision-making: Explicitly checks the service method (AUTO or DIRECT).
• Branches based on method: Different paths for GNSS-based or direct initialization.
• Separate processing for pose and GNSS data: Complex conditionals based on method.

Proposed Architecture

• Covariance-based decision-making: Infers initialization method from pose covariance values.
• Simplified flow: Directly checks if covariance is zero to initialize directly or use NDT alignment.
• Unified processing: Fewer conditionals, streamlining initialization logic by focusing on covariance.

Benefits of the Proposed Changes

• Simplification: Removing the method parameter reduces redundancy and simplifies the service interface.
• Intuitive Initialization: Inferring the initialization method from covariance values aligns with standard practices in robotics.
• Consistency: Using the existing AD API service definition promotes consistency across the system.
• Maintainability: Simplified code paths make the system easier to maintain and extend.

Considerations

• Robustness: Ensure that the system handles cases where covariance values are near zero but not exactly zero due to floating-point inaccuracies.
• Documentation: Provide clear documentation and examples to help users understand how to set covariance values appropriately.
• Backward Compatibility: Communicate changes clearly to developers and users to facilitate a smooth transition.

We've prepared this with @meliketanrikulu and I'm happy to hear your thoughts.

cc. @YamatoAndo @isamu-takagi @yukkysaito @mitsudome-r @SakodaShintaro