Skip to content

Commit

Permalink
Merge pull request #1134 from tier4/cherry-pick/intersection2
Browse files Browse the repository at this point in the history 
feat(intersection): cherry-pick awf autowarefoundation#6328 + autowarefoundation#6299 + autowarefoundation#6325
  • Loading branch information
@shmpwk
shmpwk authored Feb 8, 2024
2 parents 9336a4f + 02aafe3 commit 7bb9128
Show file tree
Hide file tree
Showing 10 changed files with 479 additions and 227 deletions.
29 changes: 28 additions & 1 deletion planning/behavior_velocity_intersection_module/config/intersection.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,15 @@
enable_pass_judge_before_default_stopline: false

stuck_vehicle:
target_type:
car: true
bus: true
truck: true
trailer: true
motorcycle: false
bicycle: false
unknown: false

turn_direction:
left: true
right: true
Expand All @@ -27,6 +36,14 @@
disable_against_private_lane: true

yield_stuck:
target_type:
car: true
bus: true
truck: true
trailer: true
motorcycle: false
bicycle: false
unknown: false
turn_direction:
left: true
right: true
Expand All @@ -37,6 +54,14 @@
consider_wrong_direction_vehicle: false
collision_detection_hold_time: 0.5
min_predicted_path_confidence: 0.05
target_type:
car: true
bus: true
truck: true
trailer: true
motorcycle: true
bicycle: true
unknown: false
velocity_profile:
use_upstream: true
minimum_upstream_velocity: 0.01
Expand All @@ -56,7 +81,9 @@
duration: 3.0
object_dist_to_stopline: 10.0
ignore_on_amber_traffic_light:
object_expected_deceleration: 2.0
object_expected_deceleration:
car: 2.0
bike: 5.0
ignore_on_red_traffic_light:
object_margin_to_path: 2.0
avoid_collision_by_acceleration:
Expand Down
380 changes: 250 additions & 130 deletions planning/behavior_velocity_intersection_module/src/manager.cpp
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion planning/behavior_velocity_intersection_module/src/object_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects()
parked_objects_.clear();
};

std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects()
std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
{
std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
all_objects.insert(
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_velocity_intersection_module/src/object_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ class ObjectInfo
return predicted_object_;
};

std::optional<CollisionInterval> is_unsafe() const
std::optional<CollisionInterval> unsafe_info() const
{
if (safe_under_traffic_control_) {
return std::nullopt;
Expand Down Expand Up @@ -246,7 +246,7 @@ class ObjectInfoManager

const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }

std::vector<std::shared_ptr<ObjectInfo>> allObjects();
std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;

const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
getObjectsMap()
Expand Down
41 changes: 25 additions & 16 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,6 @@ IntersectionModule::IntersectionModule(
const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
node_(node),
lane_id_(lane_id),
associative_ids_(associative_ids),
turn_direction_(turn_direction),
Expand Down Expand Up @@ -88,10 +87,10 @@ IntersectionModule::IntersectionModule(
}

decision_state_pub_ =
node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
ego_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
"~/debug/intersection/ego_ttc", 1);
object_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
"~/debug/intersection/object_ttc", 1);
}

Expand All @@ -105,11 +104,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
const auto decision_result = modifyPathVelocityDetail(path, stop_reason);
prev_decision_result_ = decision_result;

const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
intersection::formatDecisionResult(decision_result);
std_msgs::msg::String decision_result_msg;
decision_result_msg.data = decision_type;
decision_state_pub_->publish(decision_result_msg);
{
const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
intersection::formatDecisionResult(decision_result);
std_msgs::msg::String decision_result_msg;
decision_result_msg.data = decision_type;
decision_state_pub_->publish(decision_result_msg);
}

prepareRTCStatus(decision_result, *path);

Expand Down Expand Up @@ -225,6 +226,14 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
updateObjectInfoManagerCollision(
path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
safely_passed_2nd_judge_line);
for (const auto & object_info : object_info_manager_.attentionObjects()) {
if (!object_info->unsafe_info()) {
continue;
}
setObjectsOfInterestData(
object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
object_info->predicted_object().shape, ColorName::RED);
}

const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] =
detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
Expand All @@ -241,17 +250,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
"no collision is detected", "ego can safely pass the intersection at this rate"};
}

const bool collision_on_1st_attention_lane =
has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST);
// ==========================================================================================
// this state is very dangerous because ego is very close/over the boundary of 1st attention lane
// and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this
// case, possible another collision may be expected on the 2nd attention lane too.
// ==========================================================================================
std::string safety_report = safety_diag;
if (
is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line &&
is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
if (const bool collision_on_1st_attention_lane =
has_collision &&
(collision_position == intersection::CollisionInterval::LanePosition::FIRST);
is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() &&
!is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
safety_report +=
"\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st "
"attention lane, which is dangerous.";
Expand Down Expand Up @@ -375,15 +384,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
closest_idx,
first_attention_stopline_idx,
occlusion_wo_tl_pass_judge_line_idx,
safety_diag};
safety_report};
}

// ==========================================================================================
// following remaining block is "has_traffic_light_"
//
// if ego is stuck by static occlusion in the presence of traffic light, start timeout count
// ==========================================================================================
const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED;
const bool is_static_occlusion = std::holds_alternative<StaticallyOccluded>(occlusion_status);
const bool is_stuck_by_static_occlusion =
stoppedAtPosition(
occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) &&
Expand Down
78 changes: 55 additions & 23 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <string>
#include <tuple>
#include <utility>
#include <variant>
#include <vector>

namespace behavior_velocity_planner
Expand Down Expand Up @@ -73,8 +74,20 @@ class IntersectionModule : public SceneModuleInterface
bool straight;
};

struct TargetType
{
bool car;
bool bus;
bool truck;
bool trailer;
bool motorcycle;
bool bicycle;
bool unknown;
};

struct StuckVehicle
{
TargetType target_type;
TurnDirection turn_direction;
bool use_stuck_stopline;
double stuck_vehicle_detect_dist;
Expand All @@ -84,6 +97,7 @@ class IntersectionModule : public SceneModuleInterface

struct YieldStuck
{
TargetType target_type;
TurnDirection turn_direction;
double distance_threshold;
} yield_stuck;
Expand All @@ -93,6 +107,7 @@ class IntersectionModule : public SceneModuleInterface
bool consider_wrong_direction_vehicle;
double collision_detection_hold_time;
double min_predicted_path_confidence;
TargetType target_type;
struct VelocityProfile
{
bool use_upstream;
Expand Down Expand Up @@ -123,7 +138,11 @@ class IntersectionModule : public SceneModuleInterface
} yield_on_green_traffic_light;
struct IgnoreOnAmberTrafficLight
{
double object_expected_deceleration;
struct ObjectExpectedDeceleration
{
double car;
double bike;
} object_expected_deceleration;
} ignore_on_amber_traffic_light;
struct IgnoreOnRedTrafficLight
{
Expand Down Expand Up @@ -166,16 +185,25 @@ class IntersectionModule : public SceneModuleInterface
} debug;
};

enum OcclusionType {
//! occlusion is not detected
NOT_OCCLUDED,
//! occlusion is not caused by dynamic objects
STATICALLY_OCCLUDED,
//! occlusion is caused by dynamic objects
DYNAMICALLY_OCCLUDED,
//! actual occlusion does not exist, only disapproved by RTC
RTC_OCCLUDED,
//! occlusion is not detected
struct NotOccluded
{
double best_clearance_distance{-1.0};
};
//! occlusion is not caused by dynamic objects
struct StaticallyOccluded
{
double best_clearance_distance{0.0};
};
//! occlusion is caused by dynamic objects
struct DynamicallyOccluded
{
double best_clearance_distance{0.0};
};
//! actual occlusion does not exist, only disapproved by RTC
using RTCOccluded = std::monostate;
using OcclusionType =
std::variant<NotOccluded, StaticallyOccluded, DynamicallyOccluded, RTCOccluded>;

struct DebugData
{
Expand Down Expand Up @@ -297,11 +325,9 @@ class IntersectionModule : public SceneModuleInterface
bool getOcclusionSafety() const { return occlusion_safety_; }
double getOcclusionDistance() const { return occlusion_stop_distance_; }
void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }
bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }

private:
rclcpp::Node & node_;

/**
***********************************************************
***********************************************************
Expand Down Expand Up @@ -413,7 +439,7 @@ class IntersectionModule : public SceneModuleInterface
* @defgroup occlusion-variables [var] occlusion detection variables
* @{
*/
OcclusionType prev_occlusion_status_;
OcclusionType prev_occlusion_status_{NotOccluded{}};

//! debouncing for the first brief stop at the default stopline
StateMachine before_creep_state_machine_;
Expand Down Expand Up @@ -516,7 +542,7 @@ class IntersectionModule : public SceneModuleInterface
*/
std::optional<size_t> getStopLineIndexFromMap(
const intersection::InterpolatedPathInfo & interpolated_path_info,
lanelet::ConstLanelet assigned_lanelet);
lanelet::ConstLanelet assigned_lanelet) const;

/**
* @brief generate IntersectionStopLines
Expand All @@ -527,15 +553,15 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::ConstLanelet & first_attention_lane,
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
const intersection::InterpolatedPathInfo & interpolated_path_info,
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const;

/**
* @brief generate IntersectionLanelets
*/
intersection::IntersectionLanelets generateObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const lanelet::ConstLanelet assigned_lanelet);
const lanelet::ConstLanelet assigned_lanelet) const;

/**
* @brief generate PathLanelets
Expand All @@ -546,14 +572,15 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::CompoundPolygon3d & first_conflicting_area,
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
const size_t closest_idx) const;

/**
* @brief generate discretized detection lane linestring.
*/
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
/** @} */

private:
Expand Down Expand Up @@ -603,6 +630,9 @@ class IntersectionModule : public SceneModuleInterface
bool isTargetStuckVehicleType(
const autoware_auto_perception_msgs::msg::PredictedObject & object) const;

bool isTargetYieldStuckVehicleType(
const autoware_auto_perception_msgs::msg::PredictedObject & object) const;

/**
* @brief check stuck
*/
Expand Down Expand Up @@ -660,7 +690,8 @@ class IntersectionModule : public SceneModuleInterface
* @attention this function has access to value() of intersection_lanelets_,
* intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
*/
OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info);
OcclusionType detectOcclusion(
const intersection::InterpolatedPathInfo & interpolated_path_info) const;
/** @} */

private:
Expand Down Expand Up @@ -723,7 +754,7 @@ class IntersectionModule : public SceneModuleInterface
*/
std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus(
const size_t closest_idx, const size_t collision_stopline_idx,
const intersection::IntersectionStopLines & intersection_stoplines);
const intersection::IntersectionStopLines & intersection_stoplines) const;

/**
* @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
Expand Down Expand Up @@ -755,7 +786,8 @@ class IntersectionModule : public SceneModuleInterface
* @brief return if collision is detected and the collision position
*/
CollisionStatus detectCollision(
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line);
const bool is_over_1st_pass_judge_line,
const std::optional<bool> is_over_2nd_pass_judge_line) const;

std::optional<size_t> checkAngleForTargetLanelets(
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
Expand All @@ -771,7 +803,7 @@ class IntersectionModule : public SceneModuleInterface
TimeDistanceArray calcIntersectionPassingTime(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
const intersection::IntersectionStopLines & intersection_stoplines,
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
/** @} */

mutable DebugData debug_data_;
Expand Down
Loading

0 comments on commit 7bb9128

Please sign in to comment.