diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_front_entity_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_front_entity_action.hpp index 59a57712d16..ecbb7d1c5ce 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_front_entity_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_front_entity_action.hpp @@ -42,6 +42,9 @@ class FollowFrontEntityAction : public entity_behavior::VehicleActionNode private: std::optional distance_to_front_entity_; + static constexpr double kWaypointInterval = 1.0; + static constexpr double kFrontEntityMargin = 5.0; + static constexpr double kSpeedStep = 2.0; }; } // namespace follow_lane_sequence } // namespace vehicle diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_lane_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_lane_action.hpp index 3d6086e209c..6f94820074d 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_lane_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/follow_lane_action.hpp @@ -43,6 +43,11 @@ class FollowLaneAction : public entity_behavior::VehicleActionNode private: std::optional target_lanelet_pose_; + static constexpr double kWaypointInterval = 1.0; + static constexpr double kFrontEntityStopMargin = 5.0; + static constexpr double kBoundingBoxHalfFactor = 0.5; + static constexpr double kStopLineMargin = 5.0; + static constexpr double kConflictingEntityMargin = 3.0; }; } // namespace follow_lane_sequence } // namespace vehicle diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.hpp index 2190da8336f..f7bc068c7f1 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.hpp @@ -43,7 +43,6 @@ class StopAtCrossingEntityAction : public entity_behavior::VehicleActionNode private: std::optional distance_to_stop_target_; - bool in_stop_sequence_; }; } // namespace follow_lane_sequence } // namespace vehicle diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_stop_line_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_stop_line_action.hpp index 1d6089a84ed..540db157ec8 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_stop_line_action.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_lane_sequence/stop_at_stop_line_action.hpp @@ -42,8 +42,12 @@ class StopAtStopLineAction : public entity_behavior::VehicleActionNode const traffic_simulator_msgs::msg::WaypointsArray & waypoints) override; private: - bool stopped_; std::optional distance_to_stopline_; + static constexpr double kWaypointInterval = 1.0; + static constexpr double kBoundingBoxHalfFactor = 0.5; + static constexpr double kStopMargin = 1.0; + static constexpr double kFrontStoplineMargin = 5.0; + static constexpr double kVelocityEpsilon = 0.001; }; } // namespace follow_lane_sequence } // namespace vehicle diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 4bfd1a1292b..3c844d67694 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -60,7 +60,7 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calcu double horizon = getHorizon(); const auto lanelet_pose = canonicalized_entity_status_->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( - lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); + lanelet_pose.s, lanelet_pose.s + horizon, kWaypointInterval, lanelet_pose.offset); trajectory = std::make_unique( reference_trajectory, lanelet_pose.s, lanelet_pose.s + horizon); return waypoints; @@ -127,15 +127,17 @@ BT::NodeStatus FollowFrontEntityAction::tick() if ( distance_to_front_entity_.value() >= (calculateStopDistance(behavior_parameter_.dynamic_constraints) + - vehicle_parameters.bounding_box.dimensions.x + 5)) { - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity + 2)); + vehicle_parameters.bounding_box.dimensions.x + kFrontEntityMargin)) { + setCanonicalizedEntityStatus( + calculateUpdatedEntityStatus(front_entity_linear_velocity + kSpeedStep)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } else if ( distance_to_front_entity_.value() <= calculateStopDistance(behavior_parameter_.dynamic_constraints)) { - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity - 2)); + setCanonicalizedEntityStatus( + calculateUpdatedEntityStatus(front_entity_linear_velocity - kSpeedStep)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index bc96ff16263..6424feb34af 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -46,7 +46,7 @@ const traffic_simulator_msgs::msg::WaypointsArray FollowLaneAction::calculateWay traffic_simulator_msgs::msg::WaypointsArray waypoints; const auto lanelet_pose = canonicalized_entity_status_->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( - lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); + lanelet_pose.s, lanelet_pose.s + getHorizon(), kWaypointInterval, lanelet_pose.offset); trajectory = std::make_unique( reference_trajectory, lanelet_pose.s, lanelet_pose.s + getHorizon()); return waypoints; @@ -97,7 +97,7 @@ BT::NodeStatus FollowLaneAction::tick() if ( distance_to_front_entity.value() <= calculateStopDistance(behavior_parameter_.dynamic_constraints) + - vehicle_parameters.bounding_box.dimensions.x + 5) { + vehicle_parameters.bounding_box.dimensions.x + kFrontEntityStopMargin) { return BT::NodeStatus::FAILURE; } } @@ -116,14 +116,14 @@ BT::NodeStatus FollowLaneAction::tick() if ( distance_to_stopline.value() <= calculateStopDistance(behavior_parameter_.dynamic_constraints) + - vehicle_parameters.bounding_box.dimensions.x * 0.5 + 5) { + vehicle_parameters.bounding_box.dimensions.x * kBoundingBoxHalfFactor + kStopLineMargin) { return BT::NodeStatus::FAILURE; } } if (distance_to_conflicting_entity) { if ( distance_to_conflicting_entity.value() < - (vehicle_parameters.bounding_box.dimensions.x + 3 + + (vehicle_parameters.bounding_box.dimensions.x + kConflictingEntityMargin + calculateStopDistance(behavior_parameter_.dynamic_constraints))) { return BT::NodeStatus::FAILURE; } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index 7294a502537..c9b744cfcc0 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -31,7 +31,6 @@ StopAtCrossingEntityAction::StopAtCrossingEntityAction( const std::string & name, const BT::NodeConfiguration & config) : entity_behavior::VehicleActionNode(name, config) { - in_stop_sequence_ = false; } const std::optional @@ -89,19 +88,15 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() if ( request_ != traffic_simulator::behavior::Request::NONE && request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } if (!canonicalized_entity_status_->isInLanelet()) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } if (!behavior_parameter_.see_around) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } if (getRightOfWayEntities(route_lanelets_).size() != 0) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } const auto waypoints = calculateWaypoints(); @@ -111,39 +106,26 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets_, *trajectory); + std::optional target_linear_speed; auto distance_to_stopline = traffic_simulator::distance::distanceToStopLine(route_lanelets_, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); - if (!distance_to_stop_target_) { - in_stop_sequence_ = false; + distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets_, *trajectory); + if (distance_to_stop_target_) { + target_linear_speed = calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x); + } else { return BT::NodeStatus::FAILURE; } if (distance_to_front_entity) { if (distance_to_front_entity.value() <= distance_to_stop_target_.value()) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } } if (distance_to_stopline) { if (distance_to_stopline.value() <= distance_to_stop_target_.value()) { - in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } } - std::optional target_linear_speed; - if (distance_to_stop_target_) { - target_linear_speed = calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x); - } else { - target_linear_speed = std::nullopt; - } - if (!distance_to_stop_target_) { - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); - setOutput("waypoints", waypoints); - setOutput("obstacle", calculateObstacle(waypoints)); - in_stop_sequence_ = false; - return BT::NodeStatus::SUCCESS; - } if (target_speed_) { if (target_speed_.value() > target_linear_speed.value()) { target_speed_ = target_linear_speed.value(); @@ -154,7 +136,6 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); - in_stop_sequence_ = true; return BT::NodeStatus::RUNNING; } } // namespace follow_lane_sequence diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 8cad27ad438..46470e97205 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -30,7 +30,6 @@ StopAtStopLineAction::StopAtStopLineAction( const std::string & name, const BT::NodeConfiguration & config) : entity_behavior::VehicleActionNode(name, config) { - stopped_ = false; } const std::optional StopAtStopLineAction::calculateObstacle( @@ -53,15 +52,12 @@ const std::optional StopAtStopLineAction: const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculateWaypoints() { - if (!canonicalized_entity_status_->isInLanelet()) { - THROW_SIMULATION_ERROR("failed to assign lane"); - } if (canonicalized_entity_status_->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); const auto lanelet_pose = canonicalized_entity_status_->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( - lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); + lanelet_pose.s, lanelet_pose.s + horizon, kWaypointInterval, lanelet_pose.offset); trajectory = std::make_unique( reference_trajectory, lanelet_pose.s, lanelet_pose.s + horizon); return waypoints; @@ -79,7 +75,8 @@ std::optional StopAtStopLineAction::calculateTargetSpeed(double current_ * @brief hard coded parameter!! 1.0 is a stop margin */ double rest_distance = - distance_to_stopline_.value() - (vehicle_parameters.bounding_box.dimensions.x * 0.5 + 1.0); + distance_to_stopline_.value() - + (vehicle_parameters.bounding_box.dimensions.x * kBoundingBoxHalfFactor + kStopMargin); if (rest_distance < calculateStopDistance(behavior_parameter_.dynamic_constraints)) { return 0; } @@ -92,7 +89,6 @@ BT::NodeStatus StopAtStopLineAction::tick() if ( request_ != traffic_simulator::behavior::Request::NONE && request_ != traffic_simulator::behavior::Request::FOLLOW_LANE) { - stopped_ = false; return BT::NodeStatus::FAILURE; } if (!behavior_parameter_.see_around) { @@ -101,6 +97,9 @@ BT::NodeStatus StopAtStopLineAction::tick() if (getRightOfWayEntities(route_lanelets_).size() != 0) { return BT::NodeStatus::FAILURE; } + if (!canonicalized_entity_status_->isInLanelet()) { + return BT::NodeStatus::FAILURE; + } const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; @@ -113,49 +112,37 @@ BT::NodeStatus StopAtStopLineAction::tick() const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets_, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stopline_) { - stopped_ = false; return BT::NodeStatus::FAILURE; } if (distance_to_front_entity) { if (distance_to_front_entity.value() <= distance_to_stopline_.value()) { - stopped_ = false; return BT::NodeStatus::FAILURE; } } if (distance_to_stop_target) { if (distance_to_stop_target.value() <= distance_to_stopline_.value()) { - stopped_ = false; return BT::NodeStatus::FAILURE; } } - if (std::fabs(canonicalized_entity_status_->getTwist().linear.x) < 0.001) { + if (std::fabs(canonicalized_entity_status_->getTwist().linear.x) < kVelocityEpsilon) { if (distance_to_stopline_) { - if (distance_to_stopline_.value() <= vehicle_parameters.bounding_box.dimensions.x + 5) { - stopped_ = true; + if ( + distance_to_stopline_.value() <= + vehicle_parameters.bounding_box.dimensions.x + kFrontStoplineMargin) { + 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; } } } - if (stopped_) { - if (!target_speed_) { - target_speed_ = hdmap_utils_->getSpeedLimit(route_lanelets_); - } - if (!distance_to_stopline_) { - stopped_ = false; - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value())); - setOutput("waypoints", waypoints); - setOutput("obstacle", calculateObstacle(waypoints)); - return BT::NodeStatus::SUCCESS; - } - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value())); - setOutput("waypoints", waypoints); - setOutput("obstacle", calculateObstacle(waypoints)); - return BT::NodeStatus::RUNNING; - } auto target_linear_speed = calculateTargetSpeed(canonicalized_entity_status_->getTwist().linear.x); if (!target_linear_speed) { - stopped_ = false; return BT::NodeStatus::FAILURE; } if (target_speed_) { @@ -166,7 +153,6 @@ BT::NodeStatus StopAtStopLineAction::tick() target_speed_ = target_linear_speed.value(); } setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed_.value())); - stopped_ = false; setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp index db82a74386f..9e3cb2ab0ef 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp @@ -118,12 +118,6 @@ BT::NodeStatus StopAtTrafficLightAction::tick() } else { return BT::NodeStatus::FAILURE; } - if (!distance_to_stop_target_) { - setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); - setOutput("waypoints", waypoints); - setOutput("obstacle", calculateObstacle(waypoints)); - return BT::NodeStatus::SUCCESS; - } if (target_speed_) { if (target_speed_.value() > target_linear_speed.value()) { target_speed_ = target_linear_speed.value(); diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index 9919e6ca916..28ea1e29b93 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -67,8 +67,7 @@ const traffic_simulator_msgs::msg::WaypointsArray LaneChangeAction::calculateWay waypoints.waypoints = straight_waypoints; const auto curve_waypoints = curve_->getTrajectory(current_s_, l, 1.0, true); std::copy( - straight_waypoints.begin(), straight_waypoints.end(), - std::back_inserter(waypoints.waypoints)); + curve_waypoints.begin(), curve_waypoints.end(), std::back_inserter(waypoints.waypoints)); } return waypoints; } else { @@ -171,6 +170,9 @@ BT::NodeStatus LaneChangeAction::tick() lane_change_velocity_ = curve_->getLength() / lane_change_parameters_->constraint.value; break; } + if (target_speed_) { + lane_change_velocity_ = std::clamp(lane_change_velocity_, 0.0, target_speed_.value()); + } } else { return BT::NodeStatus::FAILURE; }