Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(pose_initializer, ndt_scan_matcher): check initial pose result and publish diag #8275

Merged
merged 7 commits into from
Aug 14, 2024
Merged

feat(pose_initializer, ndt_scan_matcher): check initial pose result and publish diag #8275

merged 7 commits into from
Aug 14, 2024

Conversation

RyuYamamoto
Copy link
Contributor

@RyuYamamoto RyuYamamoto commented Jul 31, 2024 •
edited
Loading

Description

  • Check the score of the initial pose estimation result in ndt_scan_matcher and if it is below the threshold, set false to the reliability in PoseWithCovarianceStamped.srv and return it.
  • Receive the initial pose estimation result in pose_initializer and if reliability is false, output an ERROR message in the /diagnostics topic.

diagnostic_pose_reliability

Related links

depend tier4/tier4_autoware_msgs#143

Parent Issue:

  • Link

How was this PR tested?

  • Check that localization works correctly in logging_simulator.
  • check that if initial pose estimation is successful, initial_pose_reliability set to True .
  • check that if initial pose estimation is failed, initial_pose_reliability set to False.

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

@RyuYamamoto
feat(localization): check initial pose result and publish diag
8d2cd5c 
Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
@github-actions github-actions bot added the component:localization Vehicle's position determination in its environment. (auto-assigned) label Jul 31, 2024
@github-actions GitHub Actions
Copy link

github-actions bot commented Jul 31, 2024 •
edited
Loading

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

@RyuYamamoto RyuYamamoto mentioned this pull request Jul 31, 2024
Merged
7 tasks
@RyuYamamoto
feat: update README
e4d3f5f 
Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
@github-actions github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Aug 1, 2024
@RyuYamamoto RyuYamamoto marked this pull request as ready for review August 1, 2024 09:49
@RyuYamamoto RyuYamamoto added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Aug 1, 2024
@RyuYamamoto
fix: rename reliability to reliable
02f90ce 
Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
@YamatoAndo YamatoAndo changed the title feat(localization): check initial pose result and publish diag feat(pose_initializer, ndt_scan_matcher): check initial pose result and publish diag Aug 5, 2024
SakodaShintaro
SakodaShintaro reviewed Aug 5, 2024
View reviewed changes
Copy link
Contributor

@SakodaShintaro SakodaShintaro left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think, since there is no change about yabloc's initializer service, reliable will be always false when using yabloc. Is it correct?

If so, in my opinion

(1) Best: judge by yabloc's score (If there is such a thing)
(2) Better: always returns true

@RyuYamamoto
Copy link
Contributor Author

@SakodaShintaro

I think that's right, for now, I will fix to always returns true.

if implemented reliability judge logic in yabloc in the future, and I want to use it for judge reliability.

@RyuYamamoto
feat: always return true in yabloc module
232f6e1 
Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
@codecov Codecov
Copy link

codecov bot commented Aug 7, 2024 •
edited
Loading

Codecov Report

Attention: Patch coverage is 22.72727% with 17 lines in your changes missing coverage. Please review.

Project coverage is 29.23%. Comparing base (847e2c9) to head (5723d57).
Report is 17 commits behind head on main.

Files Patch % Lines
...zer/src/pose_initializer/pose_initializer_core.cpp 0.00% 12 Missing ⚠️
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp 71.42% 1 Missing and 1 partial ⚠️
...lizer/src/pose_initializer/localization_module.cpp 0.00% 2 Missing ⚠️
...alizer/src/camera/camera_pose_initializer_core.cpp 0.00% 1 Missing ⚠️
Additional details and impacted files 
@@            Coverage Diff             @@
##             main    #8275      +/-   ##
==========================================
+ Coverage   23.77%   29.23%   +5.46%     
==========================================
  Files        1379     1603     +224     
  Lines      101362   116818   +15456     
  Branches    38527    50285   +11758     
==========================================
+ Hits        24101    34157   +10056     
+ Misses      74818    73475    -1343     
- Partials     2443     9186    +6743
Flag Coverage Δ *Carryforward flag
differential 12.49% <22.72%> (?)
total 29.25% <ø> (+5.47%) ⬆️ Carriedforward from 232f6e1

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

SakodaShintaro
SakodaShintaro approved these changes Aug 9, 2024
View reviewed changes
Copy link
Contributor

@SakodaShintaro SakodaShintaro left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks Good To Me

localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp
@@ -199,6 +199,8 @@ void CameraPoseInitializer::on_service(
// Estimate orientation
const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w);
const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad);
// TODO(localization/mapping team): judge reliability of estimate pose
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for adding TODO comment 👍

@SakodaShintaro SakodaShintaro merged commit 2c19f1e into autowarefoundation:main Aug 14, 2024
29 of 30 checks passed
xtk8532704 pushed a commit to tier4/autoware.universe that referenced this pull request Aug 15, 2024
@RyuYamamoto @xtk8532704
feat(pose_initializer, ndt_scan_matcher): check initial pose result a…
84b284d 
…nd publish diag (autowarefoundation#8275)

* feat(localization): check initial pose result and publish diag

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* fix: refactor

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* feat: update README

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* fix: rename reliability to reliable

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

* feat: always return true in yabloc module

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>

---------

Signed-off-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
Signed-off-by: xtk8532704 <1041084556@qq.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@SakodaShintaro SakodaShintaro SakodaShintaro approved these changes

@anhnv3991 anhnv3991 Awaiting requested review from anhnv3991 anhnv3991 is a code owner

@isamu-takagi isamu-takagi Awaiting requested review from isamu-takagi

@KYabuuchi KYabuuchi Awaiting requested review from KYabuuchi KYabuuchi is a code owner

@Motsu-san Motsu-san Awaiting requested review from Motsu-san Motsu-san is a code owner

@TaikiYamada4 TaikiYamada4 Awaiting requested review from TaikiYamada4 TaikiYamada4 is a code owner

@YamatoAndo YamatoAndo Awaiting requested review from YamatoAndo YamatoAndo is a code owner

Assignees
No one assigned
Labels
component:localization Vehicle's position determination in its environment. (auto-assigned) tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@RyuYamamoto @SakodaShintaro