Strange behaviors while using rviz ManualController plugin

[evshary]

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

While using the rviz2 plugin ManualController, the vehicle will lose control if I set positive and negative velocity repeatedly

Expected behavior

The vehicle should work no matter in the positive or negative velocity.

Actual behavior

The vehicle will lose control

Steps to reproduce

1. Set the vehicle to 10 km/h
2. Then set to -10 km/h
3. Set to 10 km/h again
4. The vehicle will accelerate and move forward crazily

Versions

Ubuntu 22.04 with latest humble Autoware.

Possible causes

I guess this is something wrong with handling acceleration in ManualController, but haven't taken a deeper look.

Additional context

Autoware galactic doesn't have the issue.

[mehmetdogru]

@evshary

Thank you for creating the issue.

I was not able to reproduce the issue by following Steps to reproduce in the description.

Could you share a video that is happening?

[evshary]

Sure, here is the video

2023-11-09.16-33-45.mp4

[mehmetdogru]

Thank you for the video @evshary, @TakaHoribe will ask TIER IV engineer who is responsible for the plugin.

[maxime-clem]

I found this issue can be fixed by forcing the car to go through the PARK gear when switching from reverse to drive.

autoware.universe/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

Lines 136 to 143 in 3ac90b0

	if (ackermann.longitudinal.speed > eps) {
	gear_cmd.command = GearCommand::DRIVE;
	} else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) {
	gear_cmd.command = GearCommand::REVERSE;
	ackermann.longitudinal.acceleration *= -1.0;
	} else {
	gear_cmd.command = GearCommand::PARK;
	}

This can be accomplished by changing the condition

autoware.universe/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

Line 136 in 3ac90b0

if (ackermann.longitudinal.speed > eps) {

to

    if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) {

The switch to PARK forces the car to completely stop when switching from negative to positive velocity (similar to the current switch from positive to negative) so it is not ideal.

Unrelated, but there might be a bug with the calculation of the time step dt.

autoware.universe/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

Line 225 in 3ac90b0

const double dt = 1.0 / 10.0;

Here it is constant but it should be calculation from the difference in the timestamps of the received VelocityReport.

[mehmetdogru]

@maxime-clem to see if a PR needs to be created

[maxime-clem]

I opened a PR to fix this issue: #6230

[mehmetdogru]

@evshary

Could you confirm if #6230 solves your issue so that we can close it?

[maxime-clem]

@evshary friendly ping.

[evshary]

Hi @mehmetdogru @maxime-clem Sorry that I didn't have time to test it recently. I hope I can give it a try next week.
But it's ok to close the issue without waiting for me.
Thank you for the help on solving the issue.

[maxime-clem]

@evshary It is fine if you can check it next week. I just want to make sure the proposed solution is suitable.
Thank you

[evshary]

@maxime-clem After some tests, I believe the bug is fixed now. Thank you!

[maxime-clem]

@evshary thank you for checking.
Are you able to approve the PR ?

[evshary]

Done.