fix(follow_trajectory_action): fix cooperation with Autoware, fix speed limits

[dmoszynski]

Description

Abstract

Ego moved too fast (did not respect the limits) in the case of the 2 test scenarios shared on jira RJD-834.

• The main cause was the not passing of the maxSpeed controller parameter to FollowTrajectoryAction - when action is used in EgoEntitySimulation as an Autoware "disturbance".
• In addition, I’ve noticed that FollowTrajectoryAction does not take into account the speed limits from lanelet2.

Changes related to this issue:

• Moved execution of FollowTrajectoryAction from EgoEntitySimulation to EgoVehicle - added a flag to activate action update. Also ensured that Ego status is overwritten only for this action.

• Added ability to setup FollowTrajectoryAction (used in cooperation with Autoware) parameters via requestSpeedChange and setVelocityLimit in EgoVehicle

• Added setting of lanelet_pose to FollowTrajectoryAction update - without this, the EntityStatus after makeUpdatedStatus had lanelet_pose_valid = false which did not allow lanelet2 limits to be processed.
  Note: matching_distance equal to 3.0 is necessary to ensure that the lanelet2 position updates successfully for the scenarios from this task.

• Added setup of waypoints in route_planner when using FollowTrajectoryAction - this is necessary in order to obtain correct lanelet2 limits using route_lanelets VehicleEntity::requestFollowTrajectory, route_lanelets are used here.

• Added consideration of current linear speed in the FollowTrajectoryAction in the behavior tree pedestrian_tree, vehicle_tree and in ego_entity_simulation - If target_speed is set its value is used, if it is not set then the value of the current linear speed is used. lanelet2 limit is completely ignored because of using FollowTrajectoryAction outside lanelet2.

Background

Changes unrelated to the issue:

• Added to FollowTrajectoryAction the case when a waypoint is passed in a step innerProduct < 0.0 - also improved the calculation of current_velocity.

• Added small improvement to FollowTrajectoryAction - part without waypoint arrival time follow_waypoint_controller.

Details

Video showing the run of a FollowTrajectoryAction for Ego controlled by Autoware. target_speed is taken as the current linear speed at the time when the action is triggered.

video_1280.mp4

Destructive Changes

• Now if the time to arrive at any waypoint is not defined and target_speed is not set, its value is taken as the current speed, previously it was the maximum speed from BehaviorParameter (default 50 m/s).
• EgoEntity::getBehaviorParameter() has been modified, removed the assignment of max_acceleration = 0 to the returned BehaviorParameter.

[dmoszynski]

@yamacir-kit
I've replied to all comments. I've already implemented and solved some of them.
For the rest, I would ask you to reply and make a decision.

[dmoszynski]

@yamacir-kit
I finished making changes to PR, but unfortunately, the PR didn't achieve the green github CI, I’ve re-run the test job again, however, the issue still occurs. Because I believe that this is not a problem on the PR side (details below) I ran the evaluator and proved that there is no regression - details here.

• Issue seems to be related to generation of unit test result file for a few packages (e.g. geometry).
  

• Issue occurs since the merge of the PR with the master.
  

• I’ve analyzed the changes in PR and didn't find anything that could cause such errors.

• Especially that no changes are related to unittests such as geometry package.

• I also ran unittests locally for these packages and got Passed for all of them - below are the logs.
  tests_result.zip

[dmoszynski]

@yamacir-kit
I've:

• developed the description according to the current guidelines.
• resolved all conflicts and tested locally the scenarios from the task - everything works fine.
• provided a green CI github, but it was necessary to comment out one hyperlink as it was causing error, details below.

Link

[yamacir-kit]

@dmoszynski
Please resolve the conflict.

[dmoszynski]

@yamacir-kit
Done (tested locally).