RJD-1511/bug_fix

[robomic]

Abstract

This PR contains a bug fix for the issue described here.
After applying this fix, the scenario in question now passes.

Details

1. The problem arises when behavior_plugin_ptr_->getWaypoints() is called while the Blackboard does not contain the Waypoints value yet. In this function:
   

   scenario_simulator_v2/simulation/traffic_simulator/src/entity/vehicle_entity.cpp

   Lines 114 to 127 in ee61ede

   	auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray
   	{
   	try {
   	return behavior_plugin_ptr_->getWaypoints();
   	} catch (const std::runtime_error & e) {
   	if (not status_->laneMatchingSucceed()) {
   	THROW_SIMULATION_ERROR(
   	"Failed to calculate waypoints in NPC logics, please check Entity : ", name,
   	" is in a lane coordinate.");
   	} else {
   	THROW_SIMULATION_ERROR("Failed to calculate waypoint in NPC logics.");
   	}
   	}
   	}

2. Default-initializing the missing values (that is, Waypoints and Obstacle) in the vehicle constructor seems to be fixing the issue.
3. But after a deeper dive into the behavior tree actions, I have found an abnormality in the code:
   

   scenario_simulator_v2/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp

   Lines 69 to 138 in ee61ede

   	BT::NodeStatus FollowLaneAction::tick()
   	{
   	getBlackBoardValues();
   	if (
   	request != traffic_simulator::behavior::Request::NONE &&
   	request != traffic_simulator::behavior::Request::FOLLOW_LANE) {
   	return BT::NodeStatus::FAILURE;
   	}
   	if (!canonicalized_entity_status->laneMatchingSucceed()) {
   	stopEntity();
   	const auto waypoints = traffic_simulator_msgs::msg::WaypointsArray{};
   	setOutput("waypoints", waypoints);
   	setOutput("obstacle", calculateObstacle(waypoints));
   	return BT::NodeStatus::RUNNING;
   	}
   	const auto waypoints = calculateWaypoints();
   	if (waypoints.waypoints.empty()) {
   	return BT::NodeStatus::FAILURE;
   	}
   	if (behavior_parameter.see_around) {
   	if (getRightOfWayEntities(route_lanelets).size() != 0) {
   	return BT::NodeStatus::FAILURE;
   	}
   	if (trajectory == nullptr) {
   	return BT::NodeStatus::FAILURE;
   	}
   	auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
   	if (distance_to_front_entity) {
   	if (
   	distance_to_front_entity.value() <=
   	calculateStopDistance(behavior_parameter.dynamic_constraints) +
   	vehicle_parameters.bounding_box.dimensions.x + 5) {
   	return BT::NodeStatus::FAILURE;
   	}
   	}
   	const auto distance_to_traffic_stop_line =
   	getDistanceToTrafficLightStopLine(route_lanelets, *trajectory);
   	if (distance_to_traffic_stop_line) {
   	if (distance_to_traffic_stop_line.value() <= getHorizon()) {
   	return BT::NodeStatus::FAILURE;
   	}
   	}
   	auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory);
   	auto distance_to_conflicting_entity =
   	getDistanceToConflictingEntity(route_lanelets, *trajectory);
   	if (distance_to_stopline) {
   	if (
   	distance_to_stopline.value() <=
   	calculateStopDistance(behavior_parameter.dynamic_constraints) +
   	vehicle_parameters.bounding_box.dimensions.x * 0.5 + 5) {
   	return BT::NodeStatus::FAILURE;
   	}
   	}
   	if (distance_to_conflicting_entity) {
   	if (
   	distance_to_conflicting_entity.value() <
   	(vehicle_parameters.bounding_box.dimensions.x + 3 +
   	calculateStopDistance(behavior_parameter.dynamic_constraints))) {
   	return BT::NodeStatus::FAILURE;
   	}
   	}
   	}
   	if (!target_speed) {
   	target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
   	}
   	setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value()));
   	setOutput("waypoints", waypoints);
   	setOutput("obstacle", calculateObstacle(waypoints));
   	return BT::NodeStatus::RUNNING;
   	}

• There are multiple actions overloading tick() method (such as StopAtStopLineAction or FollowFrontEntityAction).
• if the tick() method returns BT::NodeStatus::RUNNING or BT::NodeStatus::SUCCESS there is always setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints));.
• if the tick() method returns BT::NodeStatus::FAILURE waypoints and obstacle is never set.
• the only exception to this rule is changed in this PR.
• removing this abnormality also fixes the issue.

References

Regressions TODO

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[sonarqubecloud]

Quality Gate passed

Issues
0 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud