ndt_scan_matcher crashes when using init by GNSS

[miku54]

Hi everyone! I am currently using autoware built from source code to debug the GNSS positioning function. When the sensor is working properly, vehicle_model and sensor_model have been set in autoware.launch.xml. Use "ros2 launch autoware_launch autoware.launch.xml map_path:=~/tues_test" to open autoware. When ready, click "init by GNSS" in rviz2. The terminal prompts that the ndt_scan_matcher node has crashed. I uploaded the rosbag containing the sensor data and the launch log. I don't know how to solve the problem. In addition, during my debugging process, sometimes the value of maps_to_add.size() becomes 0. Is this normal?
rosbag : https://drive.google.com/file/d/1gjRj7QPY3Llu_-Xkg4RPr-N0FkMnIh5b/view?usp=drive_link
launch log:
launch.txt
my map files: https://drive.google.com/file/d/1bC3CsG6D_GCR5CnfGnyxfBGQ-gUmoJjB/view?usp=sharing
debug rviz2:

Version information(?) :

Sometimes this happens:

And the situation where rviz2 does not respond after click "init by GNSS":

[SakodaShintaro]

• maps_to_add.size()=0 is normal if the vehicle is not moving. (It means the number of differential maps for dynamic map loading.)
• Sorry, I couldn't open launch.log due to encoding issues.
• The vehicle is not stopped is often printed when the twist information (/localization/twist_estimator/twist_with_covariance) is not publish
• Transform Probability is below the threshold is an important log. If the score (around 2) is sufficient to localize, you should change the threshold in https://github.com/autowarefoundation/autoware_launch/blob/ea30ec025d1123ffcb09eb4a48214c69ff2b6a3e/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml#L81
• What happens if you use the latest version of Autoware?

[SakodaShintaro]

• I still can't open the updated launch.log.

• I could build the latest main of autoware.

vcs import src < autoware.repos
vcs import src < tools.repos
rosdep update
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
MAKEFLAGS="-j2" colcon build \
  --symlink-install \
  --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF \
  --continue-on-error

An critical PR was merged on Jun 4, it is important whether you use autoware_auto_***_msgs or autoware_***_msgs.

autowarefoundation/autoware.universe#7251

[miku54]

• I still can't open the updated launch.log.
• I could build the latest main of autoware.

vcs import src < autoware.repos
vcs import src < tools.repos
rosdep update
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
MAKEFLAGS="-j2" colcon build \
  --symlink-install \
  --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF \
  --continue-on-error

An critical PR was merged on Jun 4, it is important whether you use autoware_auto_***_msgs or autoware_***_msgs.

autowarefoundation/autoware.universe#7251

I am really sorry. I made sure that the launch.log file I uploaded this time is normal and valid. Updating autoware may be a solution. I will update and debug at the same time. Is there any solution without updating the software?

[SakodaShintaro]

I was able to open the latest launch.log, thank you.

However, it contains little information.

If you add a glog to your autoware src, it may dump more information.

cd ./autoware/src
git clone https://github.com/tier4/glog.git -b v0.6.0_t4-ros
<build>

Plus, I notice that the following messages doesn't appear in launch.txt.

maps_to_add.size()=0
and

The vehicle is not stopped

Does this mean that Autoware sometimes works and sometimes doesn't?

[miku54]

The vehicle is not stopped

I was able to open the latest launch.log, thank you.

However, it contains little information.

If you add a glog to your autoware src, it may dump more information.

cd ./autoware/src
git clone https://github.com/tier4/glog.git -b v0.6.0_t4-ros
<build>

Plus, I notice that the following messages doesn't appear in launch.txt.

maps_to_add.size()=0 and

The vehicle is not stopped

Does this mean that Autoware sometimes works and sometimes doesn't?

maps_to_add.size()=0
Usually it will be printed in the log "Update map (Add: 0, Remove: 1)". I did not get the "The vehicle is not stopped" message during normal use. Autoware uses local positioning normally. ndt_scan_matcher does not die suddenly. What other information do I need to provide? I don't know where to start to solve this problem.

[miku54]

If you add a glog to your autoware src, it may dump more information.

cd ./autoware/src
git clone https://github.com/tier4/glog.git -b v0.6.0_t4-ros
<build>

The latest source code git clone is completed, but the autoware_universe_utils build fails:

/usr/local/include/boost/geometry/strategies/expand/services.hpp:40:5: error: static assertion failed: Not implemented for this Strategy.
40 | BOOST_GEOMETRY_STATIC_ASSERT_FALSE(

Does Boost.Geometry need to specify a version? The libboost-all-dev version in my system is 1.74

[wayne-turing]

Have you tried using GDB to debug the NDT process crash?
1732175835.1428473 [pose_initializer_node-32] Failed to find match for field 'x'
1732175835.1429963 [pose_initializer_node-32] Failed to find match for field 'y'.
1732175835.1431136 [pose_initializer_node-32] Failed to find match for field 'z'.
1732175835.1432176 [pose_initializer_node-32] �[0m[INFO] [1732175835.141565973] [localization.util.pose_initializer_node]: Call NDT align server.�[0m 1732175835.4350610 [ERROR] [ndt_scan_matcher-31]: process has died [pid 7133, exit code -11, cmd

This part maybe check lidar data (map / sensor ) is correct.

[miku54]

Have you tried using GDB to debug the NDT process crash? 1732175835.1428473 [pose_initializer_node-32] Failed to find match for field 'x' 1732175835.1429963 [pose_initializer_node-32] Failed to find match for field 'y'. 1732175835.1431136 [pose_initializer_node-32] Failed to find match for field 'z'. 1732175835.1432176 [pose_initializer_node-32] �[0m[INFO] [1732175835.141565973] [localization.util.pose_initializer_node]: Call NDT align server.�[0m 1732175835.4350610 [ERROR] [ndt_scan_matcher-31]: process has died [pid 7133, exit code -11, cmd

This part maybe check lidar data (map / sensor ) is correct.

Haven't tried using GDB yet, I'll report the output here when I'm done testing.

[miku54]

Have you tried using GDB to debug the NDT process crash? 1732175835.1428473 [pose_initializer_node-32] Failed to find match for field 'x' 1732175835.1429963 [pose_initializer_node-32] Failed to find match for field 'y'. 1732175835.1431136 [pose_initializer_node-32] Failed to find match for field 'z'. 1732175835.1432176 [pose_initializer_node-32] �[0m[INFO] [1732175835.141565973] [localization.util.pose_initializer_node]: Call NDT align server.�[0m 1732175835.4350610 [ERROR] [ndt_scan_matcher-31]: process has died [pid 7133, exit code -11, cmd

This part maybe check lidar data (map / sensor ) is correct.

Use gdb to intercept the display:

 (gdb) run Starting program: ~/autoware/build/ndt_scan_matcher/ndt_scan_matcher
 [Thread debugging using libthread_db enabled] Using host libthread_db library "/lib/aarch64-linux-gnu/libthread_db.so.1".
 [New Thread 0xfffff4e67160 (LWP 12174)] 
[New Thread 0xffffff4657160 (LWP 12175)]
 [New Thread 0xffffff3e47160 (LWP 12176)]
 [New Thread 0xffffff3637160 (LWP 12177)]
 [New Thread 0xfffff2e27160 (LWP 12178)]
 [New Thread 0xfffff2516160 (LWP 12179)]
 [New Thread 0xffffff1c05160 (LWP 12180)]
 [New Thread 0xffffff12f4160 (LWP 12181)]
 [New Thread 0xfffff0ae4160 (LWP 12182)]
 [New Thread 0xffffd3ff7160 (LWP 12183)] terminate called after throwing an instance of 'rclcpp::exceptions::UninitializedStaticallyTypedParameterException' what(): Statically typed parameter 'frame.base_frame' must be initialized. Thread 1 "ndt_scan_matche" received signal SIGABRT, Aborted. __pthread_kill_implementation (threadid=281474841796640, signo=signo@entry=6, no_tid=no_tid@entry=0) at ./nptl/pthread_kill.c:44
44 ./nptl/pthread_kill.c: No such file or directory.
(gdb)

But 'frame.base_frame' has been set to 'base_link'.

[wayne-turing]

@miku54 have you try to print gnss_module.cpp output pose info?
This function geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose()

• `  PoseWithCovarianceStamped pose = *pose_;`
  

• const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id);
  To print all data. Maybe will find some information.

[liuXinGangChina]

hi ， @miku54
Still trying to download the bag and map，and what you faced seem not to be a big problem （I have run autoware in china many times using ndt since 2018）

For now ， you can try manually draw a init pose in rviz， is that work for you？

Once i got some clue， i will update here

happy hacking

Xingang

[miku54]

manually draw a init pose in rviz

manually draw a init pose in rviz Is working fine and the node will not crash, but manually setting the initial position is not my original intention, I want to complete the init by gnss normally.

[liuXinGangChina]

All right，I get your point。checking fellow items，if all matched ， sure AW will localize itself

1. pointcloud map is geo-referenced （i did not find the map in your zip folder）
2. your gnss driver works fine and output the info that AW need （i’m checking your bag now）
3. switch gnss_init to enable in the launch file

Happy hacking!

Xingang

[miku54]

All right，I get your point。checking fellow items，if all matched ， sure AW will localize itself

1. pointcloud map is geo-referenced （i did not find the map in your zip folder）
2. your gnss driver works fine and output the info that AW need （i’m checking your bag now）
3. switch gnss_init to enable in the launch file

Happy hacking!

Xingang

Oh, I'm so sorry! I have now updated the map file I used for testing.

[cyn-liu]

Hi @miku54
I used the latest Autoware and your map and bag for logging_simulator.launch.xml simulation, and found that the root cause of your bag's inability to initialize GPS positioning is the significant deviation between GPS location and point cloud map location.

The initial position for manual initialization is (x=2.71, y=3.57, z=0).
The initial GPS position is (x=97850.44, y=43133.23, z=20.72).

gnss_failure.mp4

When I manually subtracted this deviation before initializing the positioning and ran the logging_simulator.launch.xml simulation again, I found that GPS initialization positioning could be performed normally.

gnss_success.mp4

I think the reason for such a large deviation between GPS location and map location is that you did not use the geo reference when you mapping.

[miku54]

It looks like issue has been solved， i will mark it answered @miku54

Hi, Xingang@liuXinGangChina
After a few days of debugging, I found that the problem was not solved. Although the node no longer crashes unexpectedly, the init by gnss does not seem to work: I did not put the test car at the origin of the map, but at other locations, because I thought it could be located at every place on the map through longitude and latitude to achieve secondary repositioning. But in fact, it is not like this. Although the button turns green, the positioning shows that I am still at the origin of the map. What is the effect of init by gnss? In addition, if the positioning is based on gnss data, is the odometer and imu data introduced? Because after the positioning is successful, the pcd point cloud will keep drifting.

[liuXinGangChina]

Hi @miku54

I can't find more detail from your description , would you record your screen , especially the rviz, and post here?
It will help a lot

Happy hacking

Xingang

[miku54]

Hi @miku54

I can't find more detail from your description , would you record your screen , especially the rviz, and post here? It will help a lot

Thank you very much for your patient reply! The following is the rviz interface when I debug:

screenshot_20241205093005.mp4

[miku54]

Even though init by gnss turns green, the positioning point still does not switch to my current position.

[liuXinGangChina]

hi @miku54

1. It looks like you are using nvidia orin for test ，please set device at maxn mode for better performance
2. can you provide this bag to me for test， i can not see more clue from the video

happy hacking

心刚