fix(emergency_handler,mrm_handler): check for ego speed when determining the gear command

[danielsanchezaran]

Description

Currently, both handlers always output a DRIVE gear when an emergency message makes the ego stop, which causes a very dangerous behavior (at least in simulations) when the ego vehicle is going backwards and an emergency is triggered -> since the gear is changed to Drive and the ego is going backwards, the simulator gives a positive velocity to the ego and makes it shoot forward (See video). Note that I used a script to simulate the AEB module activating to make these tests easier.

Screencast.from.2024.05.28.15.24.35.webm

After this PR fix:

cap-.2024-06-04-16-21-19.mp4

Related links

Tests performed

PSIM

Notes for reviewers

Interface changes

ROS Topic Changes

ROS Parameter Changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[Autumn60]

@danielsanchezaran
@veqcc

Hi, I think the Line 585 changed in this PR contains a bug.
Can I fix it in my PR?

autoware.universe/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

Lines 582 to 586 in 5913c5c

	bool MrmHandler::isStopped()
	{
	constexpr auto th_stopped_velocity = 0.001;
	return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity);
	}

[veqcc]

@Autumn60
Oh I missed it..
Yes please, thank you.