feat(pose_initializer): add new parameter for check error between initial pose and GNSS pose

[RyuYamamoto]

Description

• Add new parameter for check initial pose error with gnss(feat(pose_initializer): check error initial pose and gnss pose, output diagnostics autoware.universe#8947).
• Check 2D(x-y) Error between Initial Pose result and GNSS Pose, if pose error is large, output an message in the /diagnostics topic.
• This feature is off by default, but can be enabled by setting pose_error_check_enabled to true when the GNSS is reliable, such as RTK-GNSS.

Related links

autowarefoundation/autoware.universe#8947

Tests performed

• checkout autoware.universe to feat/initial_pose_check_with_gnss
• Check that localization works correctly in logging_simulator.
• Set pose_error_check_enabled to True and check that diagnostics (gnss_pose_error_2d, is_gnss_pose_error) are output. If set to False, gnss_pose_error_2d and is_gnss_pose_error are not output.

Notes for reviewers

Interface changes

ROS Topic Changes

ROS Parameter Changes

Add new parameters.

Parameter Name	Default Value	Update Description
pose_error_threshold	5.0	The error threshold between GNSS and initial pose
pose_error_check_enabled	false	If true, check error between initial pose result and GNSS pose

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[SakodaShintaro]

LGTM