Guardrail deemed as an unknown obstacle makes the vehicle stop.

[Kim-mins]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

Hi team,

I'm currently running Autoware, and I found the situation that a guardrail deemed as an unknown obstacle makes the vehicle stop.
Here are the videos on RViz and frontview: [rviz] [frontview]

According to videos, even though there's no obstacle at front, an unknown obstacle keeps the vehicle from driving.

Expected behavior

I hope the vehicle successfully drive through the corner.

Actual behavior

But the vehicle cannot move at all.

Steps to reproduce

Here's a ros2bag file to reproduce: [ros2bag]

Versions

• Autoware: 20240115, prebuilt docker
• Carla: 0.9.13
• OS (host): Ubuntu 20.04
• OS (docker): Ubuntu 22.04

Possible causes

According to the rviz video, the pointcloud map and sensed pointcloud data are not synced. So the localization could be the cause.

Additional context

No response

[badai-nguyen]

@Kim-mins thank you raising the issue.
Could you make sure 2 points:

1. The map pointcloud include the guardrail
2. The localization is work correctly.

I think if above are satisfied, there shouldn't be unknown objects and vehicle could drive through the corner.

[Kim-mins]

Thank you for the response @badai-nguyen!

I checked the two points:

1. The map pointcloud include the guardrail
   Yes. I checked it with Vector Map Builder, and as you can see at the image below, the map includes the pointcloud for the guardrail(I checked with red pen).
   

2. The localization is work correctly.
   I checked the localization with ros2bag file, and here's the video: [video]
   The white and green dots in the video are aligned well at the start of the video, but they grow apart while cornering.

So, according to your points, it seems the localization does not work correctly, and it makes the perception bad, which eventually occurs the immobility.

I also checked that this issue occurs when the vehicle starts right behind the corner, not while just going through the corner from some other points.

Do you have any idea to resolve this issue? In my experience, the cornering always makes the localization bad..

Thank you!

[badai-nguyen]

@Kim-mins Thank you for your analysis.

• Since the guardrail pointcloud are included in map poincloud, if the localization works precisely as expected, then the guardrail should be filtered by compare_map_filter.
• Could you check the result of compare_map_filter more detail, for example what kind of guardrail pointcloud is remained in the topic: /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud

[Kim-mins]

Thank you for the response @badai-nguyen!

I echoed to the topic /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud, and I could get the message like below:

message

header:
  stamp:
    sec: 93
    nanosec: 544579655
  frame_id: base_link
height: 1
width: 6125
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
is_bigendian: false
point_step: 16
row_step: 98000
data:
- 204
- 81
- 28
- 194
- 0
- 134
- 251
- 63
- 9
- 130
- 143
- 191
- 0
- 0
- 128
- 63
- 200
- 79
- 153
- 65
- 64
- 2
- 175
- 192
- 60
- 36
- 111
- 191
- 0
- 0
- 128
- 63
- 24
- 69
- 250
- 65
- 192
- 121
- 18
- 193
- 214
- 5
- 99
- 191
- 0
- 0
- 128
- 63
- 168
- 110
- 68
- 66
- 0
- 59
- 96
- 193
- 88
- 31
- 2
- 191
- 0
- 0
- 128
- 63
- 44
- 18
- 7
- 66
- 128
- 71
- 22
- 193
- 125
- 51
- 182
- 190
- 0
- 0
- 128
- 63
- 80
- 68
- 215
- 65
- 128
- 246
- 220
- 192
- 17
- 109
- 200
- 190
- 0
- 0
- 128
- 63
- 176
- 232
- 211
- 65
- 128
- 115
- 205
- 192
- 93
- 204
- 172
- 190
- 0
- 0
- 128
- 63
- 96
- 107
- 211
- 65
- 192
- 253
- 198
- 192
- 197
- 49
- 167
- 190
- 0
- 0
- 128
- 63
- '...'
is_dense: true

But I cannot know the meaning of those numbers. Could you please tell me how to investigate more?

Thank you!

[badai-nguyen]

@Kim-mins I think you can add that topic in Rviz and compare with /perceptioin/obstacle_segmentation/pointcloud to see how the compare_map_filter works.

[Kim-mins]

Sorry for the late reply @badai-nguyen.

I tried to compare results of the two topics following your suggestion(thanks!), and I found that some points from the topic /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud are alive.

rviz2.mp4

Localization is not working well, and at 0:07 of the video, some of the points from the guardrail are alive.
Perhaps those points are deemed as an obstacle.

[meliketanrikulu]

Thanks you @Kim-mins . Have you been able to find the reason why you are having problems with localization? Are you still working on this issue? Could you share the latest situation?

[Kim-mins]

Sure @meliketanrikulu!

Actually, I'm in the same situation and still with the issue..

I guess this could be the cause: localization get bad while turning(at least in my running environment).
As in this video, before turning to the right, the localization was nice, but during and after turning to the right, the localization gets bad.
So perhaps this issue also has something to do with the issue on cornering, since this issue also accompanies turning.

For the possible reason of bad localization at the corner, I heard that this could be from my low clock frequency(as the answer here).

Could you please share your opinion for this?

[meliketanrikulu]

@Kim-mins Do you experience problems in all turns or is it a problem you encounter only in this turn? Could you share pcd map file , map_projector_info.yaml and rosbag file for testing localization

[Kim-mins]

Well, in my observation, it is ok when the vehicle starts from another point and passing that corner with guardrail.
However, when the starting point is near to the corner(as in this issue), then the vehicle does not move at all.

You can download files and ros2bag file here, and here

[meliketanrikulu]

Hello @Kim-mins . I tested your data. I think it is purely a localization problem.
Planning uses non-ground points to detect whether there are objects in front of it. I displayed the non-ground points in the screenshot below. White point cloud is non-groun points

The reason why the non-ground points are on the road is that the localization has shifted. I could see it easily when I zoomed in and looked. According to the map, the entire point cloud is shifted to the left.
You said that this is a problem you encounter when you initialize close to this region. Based on this, I assume that the pose initializer does not work well in this region.
Pose initializer uses the reference pose (usually gnss pose) and finds the most suitable pose by experimenting with NDT (particle filter) and localizes it at that point.
There is no initialization section in this bag file, but things that may cause errors are as follows.

1. When I visualized the gnss poses, I saw that it was quite far from where it should be. Since the reference to the pose initializer is pose gnss, this may be one of the reasons why it cannot be initialized properly.
   
   Blue Line : Current Vehicle Pose History
   Red Line : GNSS Pose History
   (The actual location of the vehicle is close to the blue line, but the yaw angle should be different. We can see that looking at the map - point cloud matching results)
2. NDT may not work well enough here. Because there is an empty space on the right side. For this you can increase the maximum distance of the point cloud.

Additionally : when I ran your data, I saw that ground segmentation did not work well at the beginning. I don't think this is what's causing the error here. Because there is this problem only at the beginning of the bag file. But it will cause problems elsewhere. I recommend you to review your ground segmentation parameters.

White point cloud: non-ground points
Colorful point cloud: Concatenated point cloud.

[Kim-mins]

Thank you for the detailed investigation @meliketanrikulu!

I also investigated with my tool, and as you concluded, I agree that GNSS pose of the vehicle seems quite far from where it should be.
But I'm little bit confused if the bad GNSS pose is the cause or not.

Here's the video of the situation: [video]
At first, the pointcloud matches with the .pcd map, and after the routing request, it seems there's no problem since the trajectory color is not red.
Also, as you can see, the GNSS pose of the ego vehicle at the initial point has small offset.
When the vehicle starts to drive, the vehicle stops at the corner due to the issue here.

Additionally, I also tested the situation that the vehicle starts at the point far from the corner, but the GNSS pose is bad either.
Here's the video of the situation: [video]
As you can see, the GNSS pose is bad at the initial point, but the vehicle could pass the corner and drive well.
And after passing the corner, the GNSS pose seems better than before.
Could you please share your opinion on this?

For the ground segmentation issue, this seems my .pcd map is bad, or just Carla-specific issue.
I also reported this several months before, and I got the answer to check my ground segmentation parameters, just like you suggested. I'll check it too.

Thanks a lot!

[meliketanrikulu]

Hello @Kim-mins. Sorry for the late reply. In your bag file there is not initialization part. Could you provide data with initialization part. Because I cant see moment of disruption it was already disrupted in the bag file.
Looks like an interesting bug. NDT probably doesn't work well here because the adjacent area is empty, and EKF may not tolerate it because the action was started recently. Its just an idea if you share data with initialization part I can test again

[Kim-mins]

Sorry for late reply @meliketanrikulu.. I was busy for other tasks..

I recorded the driving again. Here's the ros2bag file with initialization: [ros2bag]

Thank you!

[meliketanrikulu]

Hello @Kim-mins . Sorry for late reply. I was working on another issue.
I tested your data and I saw NDT is working well but EKF output is shift after initialization.
The problem start when vehicle is turning so first of all I checked yaw-bias estimation . yaw-bias estimationis open as a default. I changed this parameter as false the problem is solved. As I understand the problem related to yaw bias estimation.

Solution:
1.) The reason for this may be related to your calibration angles. Can you check the base_link-lidar calibration? You can check base_link - lidar calibration looking to the /localization/pose_twist_fusion_filter/estimated_yaw_bias this topic which is output of the ekf_localizer node. This value should be as close to 0 as possible. You can check this comment. It seems they are similar problems
2.)If it does not affect your system, you can continue your tests by turning off yaw bias estimation for now.

[xmfcx]

Closing this issue since there was no response by the author.

[Kim-mins]

Sorry reaching you too late @meliketanrikulu.. I was busy for other tasks..

Thank you for the investigation and possible solutions.

First, I checked the data from the topic /localization/pose_twist_fusion_filter/estimated_yaw_bias, I could get the data as below

...
---
stamp:
  sec: 123
  nanosec: 508968209
data: -0.0024257719105246073
---
stamp:
  sec: 123
  nanosec: 558968210
data: -0.0024339113732222404
---
stamp:
  sec: 123
  nanosec: 608968211
data: -0.002439166237103367
---
...

I cannot sure, but seems the data is really close to 0 now.

I also tried your second solution too, and seems it works for me.
Here's the rviz2 video: [video]
As the video shows, there's no shift now. So it seems the issue is gone with the solution.

I'm really sorry for the late reply.. and thank you so much for the solution!