feat(ndt_scan_matcher): added logging initial pose

[SakodaShintaro]

Description

Added code to log input and output Pose of initial position estimation in align_using_monte_carlo.

Tests performed

It was confirmed that it can be successfully executed against sample rosbag.

Effects on system behavior

Each time an initial position estimation is performed using NDT, the following three lines of log are output.

Each value is (x, y, z, qx, qy, qz, qw, roll, pitch, yaw(deg), cov(0,0), cov(1,1), ... cov(5, 5)).

1695192506.8527462 [ndt_scan_matcher-31] [INFO 1695192506.851505952] [localization.pose_estimator.ndt_scan_matcher]: align_using_monte_carlo_input,89570.098658,42300.198565,-3.188279,0.000363,-0.007508,0.018195,0.999806,0.025985,-0.860952,2.084941,1.000000,1.000000,0.010000,0.010000,0.010000,10.000000
...
1695192509.4600489 [ndt_scan_matcher-31] [INFO 1695192509.439114040] [localization.pose_estimator.ndt_scan_matcher]: align_using_monte_carlo_output,89571.156250,42301.187500,-3.157603,-0.008684,-0.001767,0.287582,0.957715,-1.011302,0.092282,33.427036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
1695192509.4605412 [ndt_scan_matcher-31] [INFO 1695192509.439304247] [localization.pose_estimator.ndt_scan_matcher]: best_score,5.89776

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.

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.

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

[codecov]

Codecov Report

Patch coverage has no change and project coverage change: -0.01% ⚠️

Comparison is base (909f12f) 15.71% compared to head (9b8936d) 15.71%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5045      +/-   ##
==========================================
- Coverage   15.71%   15.71%   -0.01%     
==========================================
  Files        1579     1579              
  Lines      109342   109346       +4     
  Branches    34106    34103       -3     
==========================================
  Hits        17188    17188              
- Misses      73239    73243       +4     
  Partials    18915    18915              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	15.72% <ø> (+<0.01%)	⬆️	Carriedforward from 909f12f

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

Files Changed	Coverage Δ
..._localizer/include/ekf_localizer/ekf_localizer.hpp	0.00% <ø> (ø)
...calizer/include/ekf_localizer/hyper_parameters.hpp	0.00% <ø> (ø)
localization/ekf_localizer/src/ekf_localizer.cpp	0.00% <ø> (ø)
...can_matcher/include/ndt_scan_matcher/util_func.hpp	0.00% <ø> (ø)
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	0.00% <0.00%> (ø)
localization/ndt_scan_matcher/src/util_func.cpp	0.00% <0.00%> (ø)
...lanner/scene_module/avoidance/avoidance_module.hpp	7.81% <ø> (ø)
...er/src/scene_module/avoidance/avoidance_module.cpp	12.77% <ø> (ø)

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

[KYabuuchi]

Thanks for creating the PR. It's cool 👍