fix(follow_trajectory_action): adapt to work with considering slopes

[dmoszynski]

Description

Jira ticket: link

Regressions: link

Abstract

Fixes

• fix NaN time in the EntityStatus at FollowTrajectoryAction start,
• fix matching distance for toLaneletPose() in FollowTrajectoryAction,
• fix matching distance in EgoEntitySimulation and EntityBase,
• fix FollowTrajectoryAction for slopes (use the distance along the lanes and take into account the pitch),
  • note: I’ve made sure that FollowTrajectoryAction works properly also outside lanes, as it has been until now.

Background

I’ve adapted FollowTrajectoryAction to get the Passed result for scenario RoutingAction.FollowTrajectoryAction-autoware.yaml.

NaN time - details

• The issue was related to the NaN time in the EntityStatus at FollowTrajectoryAction start.
• This occurred due to the missing setting of the time equal to 0 in Entities statuses at the start of the simulation (calling startNpcLogic()) - commit with the fix, most significant changes:
  • Add set current_time in startNpcLogic() for EntityManager
  • Add set current_time in startNpcLogic() for EntityBase

Matching distance - details

• The issue was related to the failure to match the lane when changing lanes.

• The issue has arisen as a result of the changes involved in the unification of the matching_distance values when matching Entity position to lane - source PR.

• This occurred due to missing adaptation of FollowTrajectoryAction to the new approach, to be more precise matching_distance on the BehaviorTree side was different than in TrafficSimulator - commit with the fix, most significant changes:

  • Fix matching distance in FollowTrajectoryAction for pedestrian
  • Fix matching distance in FollowTrajectoryAction for vehicle
  • Fix matching distance in FollowTrajectoryAction for ego
  • Fix matching distance in fillLaneletPose() for EntityBase
  • Fix matching distance in getMatchedLaneletPoseFromEntityStatus() for EgoEntitySimulation

Slope adaptation - details

After making the above changes, FollowTrajectoryAction successfully arrives at all waypoints, but there is a trouble with having enough accuracy when stopping at the last one.

• This issue has arisen as a result of changes involved in the introduction of consideration of the slope in the position of Entity - source PR.
• I investigated this, and after setting consider_pose_by_road_slope to false, FollowTrajectoryAction achieves sufficient accuracy, which confirmed that these slope-related changes are causing this issue.
  • Note: An additional code modification was required, related to temporarily revert this change - unfortunately, it seems that consider_pose_by_road_slope has no effect on HdMapUtils::toMapPose(). Despite setting consider_pose_by_road_slope to false, orientation is still calculated with the slope taken into account.

To better understand the issue, I’ve developed an additional scenario "Scenario with slopes" in which there are higher and longer slopes. FollowTrajectoryAction is triggered after 1 second from the start of the scenario and its waypoints form a Polyline is similar to the one marked in red in the figure.

Link to scenario with slopes and map files.

1. The execution of the scenario with slopes and scenario **autoware.yaml before any changes are presented below.

1.1 Before changes - Scenario with slopes - NPC:

As can be seen, the NPC does not move on the lanelet, but moves along the Polyline created by waypoints directly, so that sometimes it is above the lanelet and sometimes below it. The pitch is all the time equal to 0.
In such a case, the calculation of the remaining distance using the euclidean distance works correctly - FollowTrajectoryAction chooses the acceleration well, as a result of which the NPC, after accelerating, moves at a constant speed from which it brakes at the last waypoint.

slope3_no_fix_npc.trimmed.mp4

1.2 Before changes - Scenario with slopes - EGO

As can be seen, the position (in Oz axis) of the Ego, compared to the case for the NPC, is aligned to the lanelet. Because of this, the position on the output of the action after the FollowTrajectoryAction step is different from that on the input of the action in the next step. As a result of that, Ego has a position other than the expected (set) one, FollowTrajectoryAction continues to set accelerations all the time, and Ego does not have a constant speed. The pitch (on FollowTrajectoryAction side) is all the time equal to 0.

slope3_no_fix_ego.trimmed.mp4

1.3 Before changes - **autoware.yaml - NPC

As can be seen, in this case FollowTrajectoryAction works well, and pitch slopes and Oz axis changes are so small that they are unnoticeable.

autoware_no_fix_npc.trimmed.mp4

1.4 Before changes - **autoware.yaml - EGO

However, for Ego, issues arise. They are caused by the same reason as described in 1.2 - the position at the output of FollowTrajectoryAction is different from that at the input in the next step. Ego's velocity changes unexpectedly when changing lanes (where there are differences in the Oz axis and pitch). Because of these issues, FollowTrajectoryAction does not work properly and has a large inaccuracy when stopping.

autoware_no_fix_ego.trimmed.mp4

2. Based on an analysis of the NPCVehicle's behavior in "Scenario with slopes", I’ve developed the fixes required to work with the changes that introduce slope consideration - commit with fixes (adaptation for slopes). The execution of the scenario with slopes and scenario **autoware.yaml after these changes are presented below.

Most significant changes:

• use distance_along_lanelet for the calculations of the remaining distance
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 73 to 90 in dfb711f

  	auto distance_along_lanelet =
  	[&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double {
  	if (const auto from_lanelet_pose =
  	hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance);
  	from_lanelet_pose) {
  	if (const auto to_lanelet_pose =
  	hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance);
  	to_lanelet_pose) {
  	if (const auto distance = hdmap_utils->getLongitudinalDistance(
  	from_lanelet_pose.value(), to_lanelet_pose.value());
  	distance) {
  	return distance.value();
  	}
  	}
  	}
  	return hypot(from, to);
  	};

• use fill_lanelet_data_and_adjust_orientation for lanelet position filling, taking pitch from lanelet and applying it to entity orientation
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 91 to 113 in dfb711f

  	auto fill_lanelet_data_and_adjust_orientation =
  	[&](traffic_simulator_msgs::msg::EntityStatus & status) {
  	if (const auto lanelet_pose =
  	hdmap_utils->toLaneletPose(status.pose, status.bounding_box, false, matching_distance);
  	lanelet_pose) {
  	status.lanelet_pose = lanelet_pose.value();
  	const CatmullRomSpline spline(hdmap_utils->getCenterPoints(status.lanelet_pose.lanelet_id));
  	const auto lanelet_quaternion = spline.getPose(status.lanelet_pose.s, true).orientation;
  	const auto lanelet_rpy = convertQuaternionToEulerAngle(lanelet_quaternion);
  	const auto entity_rpy = convertQuaternionToEulerAngle(status.pose.orientation);
  	// adjust orientation in EntityStatus.pose (only pitch) and in EntityStatus.LaneletPose
  	status.pose.orientation =
  	convertEulerAngleToQuaternion(geometry_msgs::build<geometry_msgs::msg::Vector3>()
  	.x(entity_rpy.x)
  	.y(lanelet_rpy.y)
  	.z(entity_rpy.z));
  	status.lanelet_pose.rpy =
  	convertQuaternionToEulerAngle(getRotation(lanelet_quaternion, status.pose.orientation));
  	status.lanelet_pose_valid = true;
  	} else {
  	status.lanelet_pose_valid = false;
  	}
  	};

• calc desired_velocity vector using pitch along lanelet (if possible) and yaw on the target
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 378 to 399 in dfb711f

  	} else if (const auto desired_velocity =
  	[&]() {
  	/*
  	Note: The followingMode in OpenSCENARIO is passed as
  	variable dynamic_constraints_ignorable. the value of the
  	variable is `followingMode == position`.
  	*/
  	if (polyline_trajectory.dynamic_constraints_ignorable) {
  	const auto dx = target_position.x - position.x;
  	const auto dy = target_position.y - position.y;
  	// if entity is on lane use pitch from lanelet, otherwise use pitch on target
  	const auto pitch =
  	entity_status.lanelet_pose_valid
  	? -convertQuaternionToEulerAngle(entity_status.pose.orientation).y
  	: std::atan2(target_position.z - position.z, std::hypot(dy, dx));
  	// use yaw on target
  	const auto yaw = std::atan2(dy, dx);
  	return geometry_msgs::build<geometry_msgs::msg::Vector3>()
  	.x(std::cos(pitch) * std::cos(yaw) * desired_speed)
  	.y(std::cos(pitch) * std::sin(yaw) * desired_speed)
  	.z(std::sin(pitch) * desired_speed);
  	} else {

• consider pitch in current_velocity vector
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 419 to 428 in dfb711f

  	} else if (const auto current_velocity =
  	[&]() {
  	const auto pitch =
  	-convertQuaternionToEulerAngle(entity_status.pose.orientation).y;
  	const auto yaw = convertQuaternionToEulerAngle(entity_status.pose.orientation).z;
  	return geometry_msgs::build<geometry_msgs::msg::Vector3>()
  	.x(std::cos(pitch) * std::cos(yaw) * speed)
  	.y(std::cos(pitch) * std::sin(yaw) * speed)
  	.z(std::sin(pitch) * speed);
  	}();

• use desired_velocity to calc entity orientation
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 583 to 596 in dfb711f

  	updated_status.pose.orientation = [&]() {
  	if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) {
  	// do not change orientation if there is no designed_velocity vector
  	return entity_status.pose.orientation;
  	} else {
  	// if there is a designed_velocity vector, set the orientation in the direction of it
  	const geometry_msgs::msg::Vector3 direction =
  	geometry_msgs::build<geometry_msgs::msg::Vector3>()
  	.x(0.0)
  	.y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y)))
  	.z(std::atan2(desired_velocity.y, desired_velocity.x));
  	return convertEulerAngleToQuaternion(direction);
  	}
  	}();

• after the step, match the orientation to the lanelet
  

  scenario_simulator_v2/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

  Lines 598 to 603 in dfb711f

  	/*
  	After the step, i.e. movement in the direction of designed_velocity, it is necessary to adjust
  	the pose of the entity to the lane - if possible, the pitch orientation may be
  	changed as a result because the slope of the lane may be different
  	*/
  	fill_lanelet_data_and_adjust_orientation(updated_status);

Results:

• For NPC FollowTrajectoryAction works correctly and as expected.
• I noticed that Ego executes the FollowTrajectoryAction in the scenario in a different way than the NPC - despite the fact that it is then controlled by the same method.
• The changes related to adaptation to consider slopes were not sufficient to get Passed.

2.1 Adaptation for slopes - Scenario with slopes - NPC:

As can be seen, FollowTrajectoryAction moves the NPC along the lanalet taking into account its slope - as expected. Accelerations are also calculated correctly, ensuring that the NPC reaches a constant speed and stops with sufficient accuracy.

slope3_fix_npc.trimmed.mp4

2.2 Adaptation for slopes - Scenario with slopes - EGO

As can be seen, despite the improvement for Npc, the issue for Ego still exists - speeds change in unexpected ways - after the FollowTrajectoryAction step, the input position is different than the output position in the previous step.

slope3_no_additional_fix_ego.trimmed.mp4

2.3 Adaptation for slopes - **autoware.yaml - NPC

As can be seen now, when the NPC is aligned in the Oz axis to the lanelet and the pitch is taken into account, there are speed changes during lane changes here. This is because the waypoints are in other lanes, so that the distance along the lanelet cannot be calculated, and the euclidean distance is used. (Here the position in Oz is different on both lanes - I checked it, however, I did not take a screen shot...). However, here, for the NPC, after the FollowTrajectoryAction step, the input position is the same as the output position in the previous step.

autoware_fix_npc.trimmed.mp4

2.4 Adaptation for slopes- **autoware.yaml - EGO

As can be seen, the Ego behaves similarly to the NPC - here there are also speed changes caused by the position in Oz. However, for Ego, after the FollowTrajectoryAction step, the input position is different than the output position in the previous step. For this reason, FollowTrajectoryAction cannot achieve the expected accuracy.

autoware_no_additional_fix_ego.trimmed.mp4

3. After analysis, I found the source of the issue, which was related to the way of overwriting positions in EgoEntitySimulation. I’ve developed a fix, thanks to which the position read is the same as written and FollowTrajectoryAction works properly for Ego - commit with fix (fix for Ego). The execution of the scenario with slopes and scenario **autoware.yaml after these changes are presented below.

Most significant changes:

• getZ() - in SimModelDelaySteerAccGeared
• overwrite Oz position in model and read it - in EgoEntitySimulator

Results:

• The issue caused that the read pose was different from the written pose (which should not happen, as the pose is overwritten instead of updated)
• Now FollowTrajectoryAction works properly for slopes as well, both for NPCVehicle and for Ego.

3.1 Fix for Ego - Scenario with slopes - NPC:

The same as 2.1.

3.2 Fix for Ego - Scenario with slopes - EGO

As can be seen, now the Ego behaves exactly the same as the NPC - after the FollowTrajectoryAction step, the input position is the same as the output position in the previous step. FollowTrajectoryAction ensures stopping at the last point with sufficient accuracy.

slope3_fix_ego.trimmed.mp4

3.3 Fix for Ego - **autoware.yaml - NPC

The same as 2.3.

3.4 Fix for Ego - **autoware.yaml - EGO

As can be seen, now the Ego behaves exactly the same as the NPC - after the FollowTrajectoryAction step, the input position is the same as the output position in the previous step. FollowTrajectoryAction ensures stopping at the last point with sufficient accuracy.

autoware_fix_ego.trimmed.mp4

Destructive Changes

Oz position in SimModelInterface

• This change is necessary for FollowTrajectoryAction to work properly. Entities move along lanelets, where pitch and Oz are taken into account. If FollowTrajectoryAction is to overwrite Ego positions, the position in Oz cannot just be ignored.
• Currently, the virtual getZ() method has been implemented, which returns 0 for every model except the one currently in use.
• The position in Oz is set only when the position is overwritten, it is not calculated in any way. To be more precise, if the update() in SimModelInterface is used, everything still works on 2D, and the position in Oz is only used during overwrite() - that is, only by FollowTrajectoryAction.
• I believe that all SimModelInterface should be adapted to 3D, due to the fact that all the rest of the code in ss2 currently uses 3D and Autoware uses 3D.

Using distance along lanelet2 if possible instead of euclidean distance

• This seems to be necessary for FollowTrajectoryAction to work properly.
• However, this introduces potential issues when the position on the lanelet is not unique, i.e., for example, when passing through lanelet connections, the entity is once on the previous one once on the one in front, and in the next step it is again on the previous one. For example, the following lanelet_ids: {1,2}, each of length 50, assume that in step (1) the Entity is at lanelet_id=1 and s=49.9, in the next step (2) it will be at lanelet_id=2 and s=0. 0, but in step (3) it may be again at lanelet_id=1 and s=49.9. This happened during my tests and it is worth to ensure that such situations do not occur, as it causes the distance along the lanelet to change in unexpected ways.
• It is also important to take into account the issue of calculating the distance during lane changes, which cause the distance to change in unexpected ways.

[github-actions]

This PR edits vehicle model that is copied from simple_planning_simulator. Please consider making changes to the original code to avoid confusion or consult developers (@hakuturu583, @yamacir-kit and @HansRobo ).

[dmoszynski]

@yamacir-kit
PR is ready for re-review (regressions results).

[MertClk]

https://eigen.tuxfamily.org/dox/TopicPitfalls.html

In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type.

• The position of kinematic_state topic is not updated #1302

const Eigen::Vector3d relative_position =

This should fix it.

[hakuturu583]

Thank you for finding problem!
I also think it should be fix.

[MertClk]

Thank you @hakuturu583, it was my pleasure.