`Predicted Trajectory` generates the path ignoring the NPC vehicle stopping at the downhill

[Kim-mins]

Checklist

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

Description

Hi team, I'm currently running Autoware on Carla 0.9.13, and I found the situation that, predicted trajectory generates the path ignoring the NPC vehicle stopping on the downhill.

Here are the videos for more details: [video: rviz]

In the video, I turned off viewing predicted trajectory at 0:25, and I turn it on at 0:34. At 0:34, there's a message obstacle on the path, but the predicted trajectory generates the path(white polygon) ignoring the NPC vehicle and the speed does not decreases enough.

Expected behavior

The car to stop behind the vehicle properly.

Actual behavior

The car does not consider the vehicle in front of it, and eventually collides.

Steps to reproduce

From docker image [ghcr.io/autowarefoundation/autoware-universe:humble-20230715-prebuilt-cuda-amd64], I ran Autoware and replayed the situation with the [rosbag file]

Versions

• Autoware from docker image [ghcr.io/autowarefoundation/autoware-universe:humble-20230715-prebuilt-cuda-amd64]
• Carla: 0.9.13

Possible causes

No response

Additional context

No response

[VRichardJP]

Is it only for downhill vehicles? From the video it looks like your machine/setup is struggling to run the simulation, maybe it is just that everything is so slow that autoware just can't brake properly?

[Kim-mins]

Thank you for the response @VRichardJP.

I intentionally slowed down(↓ while replaying) or stopped(space bar while replaying) the rosbag replay, just for the video recording. I'm currently running Autoware and simulator(carla) synchronously(fps=20), so I think there's no sync problem. You can check the video below, showing a normal behavior of the car. https://github.com/autowarefoundation/autoware.universe/assets/50267797/105c1cb5-f74e-455f-b3a2-9d5415393966

[VRichardJP]

In that case I can think of 2 possible causes for your issue:

• Autoware wants to brake for the obstacle (we see the obstacle "wall" in Rviz), but fails to because the slope is too strong. For example, you could try to create a goal point in the downward slope (withuot any obstacle) and see if Autoware manages to stop there or not. Normally, slopes are handled by the "slope compensation" algorithm in the longitudinal controller. Could you check whether it is enabled in the parameters? It is also possible your lanelet map does not follow the slope (e.g. is flat), in which case Autoware thinks there is no slope.
• otherwise you found a strange bug. Congrats! :-)

[Kim-mins]

Thank you for the response @VRichardJP! I'll try your suggestion soon. I checked the value for the parameter you told is true(enabled), but I have no idea to check if there's any slope in my lanelet map. Could you please provide me the way to check it?(any parameters to check on lanelet map file?)

[VRichardJP]

If you change the camera view in Rviz to orbit or 3d follower, you should be able to check visually whether the lanelet points follow the PCD map slope or not.

[Kim-mins]

Thank you for your help @VRichardJP!

I tried to run the scenario you suggested, and I could observe that the car does not stops well at the goal point in the middle of the slope. Here are the videos: [carla: front view] [rviz] (the blue box in the video is the goal point)

I cannot know the reason, so I checked 3d view on rviz too. Here's the video: [rviz: 3d] The ego vehicle goes up and down the slope, but it sometimes does not cling to the road while keeping its pitch well. The predicted trajectory sticks to the road well before the goal, but it takes apart from the road after the goal, keeping the pitch as 0, and ends just before the obstacle.

According to the document on slope compensation, I'm currently using default setting(estimation by pitch), and it should works well since the pitch of the ego vehicle in rviz seems correct, but it doesn't.

So, as you told me, the strong degree of the slope could be the reason, and I checked the pitch of the car and it was about -13. I cannot sure about the maximum pitch that the Autoware can handle, so I want to ask if this slope is strong or not.

[VRichardJP]

Indeed there are 3 strange things in your videos:

• The vertical localization and pitch jumping. I am not sure which algorithm is responsible for that.
• the path ending in the sky and not on the ground.
• Autoware still not braking enough although the pitch is sometimes correct.

For the last one, you could try visualize the command send by autoware during the braking phase. I don't know what type of data is sent to Carla (actuation? target acceleration?), but you should see larger values in the slope than on a flat road (or than when slope compensation is disabled) Maybe the maximum brake is capped on autoware side?

[Kim-mins]

Thank you for the detailed response @VRichardJP!

For the 1st and 2nd items, I cannot know the reason too.. But for the 3rd one, I could plot the acceleration and speed value during the driving.

The bridge I'm currently using determines the control commands(throttles, brake,... -> which are for Carla) by processing the values(acceleration, speed, ...) from /control/command/control_cmd from Autoware. Here's the video plotting speed and acceleration during the driving in real-time: [video]

I don't know the exact meaning of the value acceleration, but I guess the ego vehicle accelerates when the value is > 0 and decelerates otherwise(< 0). When the car starts to go down(at 0:33 of the video), the acceleration also decreases, but the value is not below 0 and the value becomes negative at 0:38, which seems a soft brake.

[VRichardJP]

Autoware target acceleration is in m/s^2.

If I remember correctly, human-like smooth brake is usually below -0.5, while -1.0 is already quite strong (like stopping for a traffic light that just turned red).

Here it looks like the slope compensation is not strong enough (there might be a scaling factor for that). What happens if you disable the slope compensation? Do you get a similar profile (or worse?)

[Kim-mins]

Sorry @VRichardJP. I found the plotting of the video was wrong. The plotting was not real-time.. it had a performance issue. So I plotted the accumulated value on every 20 messages, and there seems no performance issue.

I got two videos, [with slope compensation] and [without slope compensation]

For the video with slope compenstation, (I don't know why, but) the ego vehicle makes hard brake just after getting to the flat road, and I could see that the car brake sharply and the speed becomes 0 in one second (So I could know that, that much of acceleration(-4 ~ -2) makes really hard brake). However, when the car is on the slope, the acceleration value is almost the same with the previous hard brake(at the flat road), but it does not stops well.

For the video without slope compenstation, the speed at the slope is a bit higher, but it seems there's no big difference on the situation.

I've run two versions of Autoware(prebuilt docker version / 20230215, 20230715) until today, and I remember that the old version(20230215) did stop behind the NPC vehicle at the slope. Here's the video for the old version: [video: old version](please refer to 0:30 of the video)

I also checked that the bridge code handling a control command is not changed from the old version.

So I have a question that, do you have any idea on the reason that the acceleration value makes different effects depending the road? If not, I'll double check my bridge.

[VRichardJP]

Yes, it looks like slope compensation is not working as expected. Actually, it's behavior is quite strange:

This is with slope compensation, right before the downward slope (23km/h):

This is without slope compensation, at the same spot (20km/h):

The remaining distance to goal point is more or less the same in both situations. However we can see that in the case with the slope compensation the Autoware has barely pushed the brake by the time it arrived at the downward slope, while in the case without slope compensation Autoware already started to brake rather strongly (-2m/s^2) before. In other words, slope compensation made things worse before the slope. There is definitely something fishy there...

@maxime-clem Can anyone from the planning team investigate this?

[VRichardJP]

Regarding the incorrect pitch/z localization, I think ekf is responsible for that. Both Z and orientation are smoothened over multiple NDT poses by the updateSimple1DFilters function in EKF. The behavior can be tuned by changing the parameters pose_additional_delay (default 0.0) and pose_smoothing_steps (default 5) in ekf_localizer.param.yaml.

Assuming you are using the default parameters and your simulated LIDAR sensor runs at 10Hz, it means it will take at least 0.5 seconds for z and pitch values to converge on a sudden slope variation. Actually it may even be worse, because it means that the initial pose sent to NDT is not that good during the transition pahse, so the localization result may not be that accurate.

I think you should be able to get better localization by setting pose_smoothen_steps to 1 or 2.

[maxime-clem]

Thank you for reporting the issue and for the initial investigation. I looked at the bag and the slope information (as calculated by the longitudinal controller) does not match the map seen in the video. At the end of the bag when ego collides with the obstacle, the measured slope (black) is positive but in the video the slope was negative (downward slope).

We can also see that there is a lot of error between the target velocity (orange) and the actual velocity (blue).

So my guess is that the issue is caused by a wrong slope information. Can you share your map so test it locally ? On your side you can try to set use_trajectory_for_pitch_calculation to true and see if it makes a difference.

I will try to setup Carla to reproduce the issue but I may not be able to do so before a while.

[Kim-mins]

Hi @maxime-clem! Thank you for the answer!

Here's the .pcd and .osm files I used(Town03 of Carla): [lanelet2_map.osm] [pointcloud_map.pcd]

I have one question for the trial on my side. Do you mean to enable the parameter use_trajectory_for_pitch_calculation here? For the setting with Carla, I'm using the bridge here(humble branch). I hope it would help you.

[maxime-clem]

Thank you for sharing your map. I was able to reproduce the issue using the simple_planning_simulator (after activating the road slope simulation with this PR https://github.com/autowarefoundation/sample_vehicle_launch/pull/55).

https://github.com/autowarefoundation/autoware.universe/assets/78338830/2244bf1b-f839-4ab3-95da-f80996df92a3

There was no difference when enabling parameter use_trajectory_for_pitch_calculation. For reference, here is the behavior with no slope compensation in the longitudinal controller.

https://github.com/autowarefoundation/autoware.universe/assets/78338830/3e0120dc-3247-4e32-ada4-7b0e5fdbb862

So in conclusion, it looks like a problem with the slope compensation not adjusting the deceleration enough to allow stopping in a downward slope.

I am not sure if someone will be able to work on this issue before a while. I will at least make sure it is discussed at the next working group meeting on January 18 (google calendar link).

[Kim-mins]

Thank you for the detailed investigation @maxime-clem! Then I'll wait for it. Thanks.

[mehmetdogru]

@brkay54 to test if any PR related slop compensation solves the issue or not

[brkay54]

Hi @Kim-mins, I can reproduce the issue like Maxime did. I checked the slope values of the road and I saw that it was higher than 0.1 rad (which is max and min slope for pitch in default parameters). When I changed it to a higher value (0.5 and -0.5). The vehicle can stop at the goal point. Could you try to change these parameters to a higher value if you are using these parameters with default value?

[Kim-mins]

Hi @brkay54! Thank you for the answer!

Unfortunately, the ego vehicle still go through the goal point for me.. I changed the parameter max_pitch_rad from 0.1 to 0.5, and min_pitch_rad from -0.1 to -0.5, as you suggested, but I could not observe any difference. I'm currently running the Autoware with the prebuilt version (docker, 20230715). Could you please tell me the version you are running?

[maxime-clem]

I tested the solution of @brkay54 and it completely solves the issue. The version of Autoware you are using (20230715) is missing some fixes to the slope compensation feature and this is probably why changing the parameters does not work in your case.

Can you try switching to a more recent version of Autoware ? Otherwise you can try cherry picking some of the fix PRs (I think all PRs to the pid_longitudinal_controller since July: https://github.com/autowarefoundation/autoware.universe/pulls?q=is%3Apr+is%3Aclosed+slope+sort%3Aupdated-desc).

[Kim-mins]

Thank you for the suggestion @maxime-clem!

I just realized that I can run the planning simulation of the latest version, and I could check the ego vehicle stops right at the goal point with the parameter @brkay54 provided. So I think the issue is gone now! I really appreciate to the hard work!! Thank you all!