Localization not working with custom sensor kit

[danielvadranapuUB]

Code of Conduct

• I have read CODE OF CONDUCT and Support Guidelines before creating this Discussion post.

Contents

Is there anyway we can preprocess the pointcloud data for a single lidar? Currently, we have a velodyne 32 channel lidar and novatel gps. I am directly passing velodyne_points into the iter4_localization.xml. Is it mandatory to pass preprocessed lidar data?

I have attached the tf_tree, sensorkit package, vehicle description and log file
launch.log

frames_2024-06-25_14.24.48.pdf
mkz_vehicle_launch.zip
mkz_sensor_kit_launch.zip
mkz_sensor_kit.zip

[danielvadranapuUB]

After selecting the 2d pose estimate the getting the error NDT align server failed

[rviz2-80] [INFO] [1719342484.890685593] [rviz2]: Setting estimate pose: Frame:map, Position(1013.35, 1000.85, 0), Orientation(0, 0, 0.999761, 0.0218605) = Angle: 3.09787
[initial_pose_adaptor_node-77] [INFO] [1719342484.890893419] [default_ad_api.helpers.initial_pose_adaptor]: fit_target: pointcloud_map, frame: map
[pose_initializer_node-36] [INFO] [1719342486.673294670] [localization.util.pose_initializer_node]: EKF triggering service is available!
[pose_initializer_node-36] [INFO] [1719342486.673598143] [localization.util.pose_initializer_node]: EKF Deactivation succeeded
[pose_initializer_node-36] [INFO] [1719342486.673673874] [localization.util.pose_initializer_node]: NDT triggering service is available!
[pose_initializer_node-36] [INFO] [1719342486.673985052] [localization.util.pose_initializer_node]: NDT Deactivation succeeded
[pose_initializer_node-36] [INFO] [1719342486.674063006] [localization.util.pose_initializer_node]: Call NDT align server.
[aggregator_node-20] [WARN] [1719342486.674009507] [Aggregator]: No timestamp set for diagnostic message. Message names: ndt_scan_matcher: trigger_node_service_status
[aggregator_node-20] [WARN] [1719342488.535230771] [Aggregator]: No timestamp set for diagnostic message. Message names: ndt_scan_matcher: ndt_align_service_status
[service_log_checker_node-3] [ERROR] [1719342488.535646593] [system.service_log_checker]: /localization/initialize: status code 4 'NDT align server failed.' (/localization/util/pose_initializer_node)
[service_log_checker_node-3] [ERROR] [1719342488.535902779] [system.service_log_checker]: /localization/initialize: status code 4 'NDT align server failed.' (/default_ad_api/node/localization)
[service_log_checker_node-3] [ERROR] [1719342488.535962730] [system.service_log_checker]: /api/localization/initialize: status code 4 'NDT align server failed.' (/default_ad_api/node/localization)
[service_log_checker_node-3] [ERROR] [1719342488.536058247] [system.service_log_checker]: /api/localization/initialize: status code 4 'NDT align server failed.' (/default_ad_api/helpers/initial_pose_adaptor)

[SakodaShintaro]

(1) I don't think Autoware expects raw point clouds to be passed directly to tier4_localizatoin.xml. Basically, it is assumed that the data will be preprocessed using the sensing module before being passed to localization. The following document may be helpful: https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-and-sensor-model/creating-sensor-model/#sensor-launch

(2) One thing to note is that concatenate doesn't seem to work well with single LiDAR yet.
https://github.com/orgs/autowarefoundation/discussions/4700

I think that currently the simple solution is to relay the topic.

(3) I'm not sure why the ndt_align service is failing.

The following warning seems a bit suspicious as it is unusual.

[aggregator_node-20] [WARN] [1719342486.674009507] [Aggregator]: No timestamp set for diagnostic message. Message names: ndt_scan_matcher: trigger_node_service_status
[aggregator_node-20] [WARN] [1719342488.535230771] [Aggregator]: No timestamp set for diagnostic message. Message names: ndt_scan_matcher: ndt_align_service_status

If you use the latest code, the timestamp is set with the following code:

• https://github.com/autowarefoundation/autoware.universe/blob/1d1ee46f55f8c494f03bd0c1b2779e8bd0182bec/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L944
• https://github.com/autowarefoundation/autoware.universe/blob/1d1ee46f55f8c494f03bd0c1b2779e8bd0182bec/localization/ndt_scan_matcher/src/diagnostics_module.cpp#L88-L105

I hope these information helps you.

[danielvadranapuUB]

Thanks for the reply are there any tutorials explicitly on Localization for custom sensor kit. I tried lot more possibilities but I am unable to resolve the issue the Closest I could I come is the point data is being displayed. Please look into the image attached

[SakodaShintaro]

Sorry, I don't know if there is a clear tutorial.

But it looks like sensor_kit is already prepared.

In that case, it's probably a good idea to try to see if tier4_sensing_launch can be launched first

https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_sensing_launch

If you want to properly use localization from sensor, I think you will likely need to launch

• Vehicle
• System
• Map
• Sensing
• Localization

https://github.com/autowarefoundation/autoware_launch/blob/6cd9a9db0cf4fe79adb861c51590a7b2470c37ed/autoware_launch/launch/autoware.launch.xml#L66-L96

And when you look at /diagnostics with the following command, is there anything you can see?

ros2 run rqt_runtime_monitor rqt_runtime_monitor

[armaganarsln]

Did you share the .bag file?

[danielvadranapuUB]

Hello, Thanks for your response. I resolved the issue everything was I run the bag, but it doesn’t work in real time. Also, while playing the bag suddenly car disappears and goes to some random position in rviz. Thanks, Daniel. Get Outlook for iOS<https://aka.ms/o0ukef>

…

________________________________ From: Armağan Arslan ***@***.***> Sent: Friday, September 20, 2024 12:56:46 PM To: autowarefoundation/autoware ***@***.***> Cc: Daniel Vadranapu ***@***.***>; Author ***@***.***> Subject: Re: [autowarefoundation/autoware] Localization not working with custom sensor kit (Discussion #4916) Did you share the .bag file? — Reply to this email directly, view it on GitHub<#4916 (reply in thread)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/BCVPTHCVLXSYPTQNTBL5RYLZXRHU5AVCNFSM6AAAAABJ4M3OJGVHI2DSMVQWIX3LMV43URDJONRXK43TNFXW4Q3PNVWWK3TUHMYTANZQG42TGNI>. You are receiving this because you authored the thread.Message ID: ***@***.***>

[meliketanrikulu]

Hello @danielvadranapuUB Could you share bag file to reproduce the problem.

[liuXinGangChina]

hi, since you code works well with rosbag (it proves that the entire localziation pipeline works), code should also works well for localization real time.

bag and tf are needed for issue analysis.

@danielvadranapuUB

happy hacking!