diff --git a/modules/planning/common/path_obstacle.cc b/modules/planning/common/path_obstacle.cc index e31e6bb11d3f1ad9595a849e03168fb007de5000..abeb00cba2e07b7fc232d1c9917e360d1f3a44d4 100644 --- a/modules/planning/common/path_obstacle.cc +++ b/modules/planning/common/path_obstacle.cc @@ -391,5 +391,9 @@ void PathObstacle::SetStBoundary(const StBoundary& boundary) { st_boundary_ = boundary; } +void PathObstacle::SetStBoundaryType(const StBoundary::BoundaryType type) { + st_boundary_.SetBoundaryType(type); +} + } // namespace planning } // namespace apollo diff --git a/modules/planning/common/path_obstacle.h b/modules/planning/common/path_obstacle.h index 2c9724e98e31258e2006c5460c05d91e51e56fab..dad78950c14f7cfd2e6f1911f9f3bc45498c9825 100644 --- a/modules/planning/common/path_obstacle.h +++ b/modules/planning/common/path_obstacle.h @@ -98,6 +98,8 @@ class PathObstacle { void SetStBoundary(const StBoundary& boundary); + void SetStBoundaryType(const StBoundary::BoundaryType type); + void EraseStBoundary(); bool HasLongitudinalDecision() const; diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc index d905be528f32422a8efa907312e8973d53cce1ee..1fadd01dab1d2b408568ea784b7649d2c24e3dfb 100644 --- a/modules/planning/common/planning_gflags.cc +++ b/modules/planning/common/planning_gflags.cc @@ -136,6 +136,9 @@ DEFINE_double(stop_max_distance_buffer, 4.0, "distance buffer of passing stop line"); DEFINE_double(stop_min_speed, 0.1, "min speed for computing stop"); DEFINE_double(stop_max_deceleration, 6.0, "max deceleration"); +/// Clear Zone +DEFINE_string(clear_zone_virtual_object_id_prefix, "CZ_", + "prefix for converting clear zone id to virtual object id"); /// traffic light DEFINE_string(signal_light_virtual_object_id_prefix, "SL_", "prefix for converting signal id to virtual object id"); diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h index adb2e181fdedcaad5a742d6beb112bf3af2acd28..45723efffb6de36b504ead0e621b45fac5b6ff5f 100644 --- a/modules/planning/common/planning_gflags.h +++ b/modules/planning/common/planning_gflags.h @@ -101,6 +101,8 @@ DECLARE_bool(enable_follow_accel_constraint); DECLARE_double(stop_max_distance_buffer); DECLARE_double(stop_min_speed); DECLARE_double(stop_max_deceleration); +/// Clear Zone +DECLARE_string(clear_zone_virtual_object_id_prefix); /// triffic light DECLARE_string(signal_light_virtual_object_id_prefix); DECLARE_double(max_deacceleration_for_yellow_light_stop); diff --git a/modules/planning/common/speed/st_boundary.h b/modules/planning/common/speed/st_boundary.h index 62a9444840be57e9e17993dcd9888866b20c8718..529746edf6955537bbbdc4e81f0475932e0a43cf 100644 --- a/modules/planning/common/speed/st_boundary.h +++ b/modules/planning/common/speed/st_boundary.h @@ -59,6 +59,7 @@ class StBoundary : public common::math::Polygon2d { FOLLOW, YIELD, OVERTAKE, + KEEP_CLEAR, }; bool IsEmpty() const { return lower_points_.empty(); } diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto index e1b7f93cb2b65f81c63b5eaeebbbd70c5ef4b582..c2b9d9c6d1c9efc2b13208905df0e1d788f08c0f 100644 --- a/modules/planning/proto/planning_config.proto +++ b/modules/planning/proto/planning_config.proto @@ -23,6 +23,7 @@ message RuleConfig { BACKSIDE_VEHICLE = 1; SIGNAL_LIGHT = 2; CROSSWALK = 3; + CLEAR_ZONE = 4; } optional RuleId rule_id = 1; } diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto index 0d7a5997c4641a936dbf4cba98fb116f8863bdbc..a788ed16a9c15cf6f107bb5a4c2d3fe585579811 100644 --- a/modules/planning/proto/planning_internal.proto +++ b/modules/planning/proto/planning_internal.proto @@ -25,6 +25,7 @@ message StGraphBoundaryDebug { ST_BOUNDARY_TYPE_FOLLOW = 3; ST_BOUNDARY_TYPE_YIELD = 4; ST_BOUNDARY_TYPE_OVERTAKE = 5; + ST_BOUNDARY_TYPE_KEEP_CLEAR = 6; } optional string name = 1; repeated apollo.common.SpeedPoint point = 2; diff --git a/modules/planning/tasks/speed_optimizer.cc b/modules/planning/tasks/speed_optimizer.cc index d8d535d6dab419ee1861ed66f04b48700ec6c6e1..2cec211635c46aeeb289cc2fc5b27b761001dcfc 100644 --- a/modules/planning/tasks/speed_optimizer.cc +++ b/modules/planning/tasks/speed_optimizer.cc @@ -130,6 +130,10 @@ void SpeedOptimizer::RecordSTGraphDebug(const StGraphData& st_graph_data, case StBoundary::BoundaryType::YIELD: boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD); break; + case StBoundary::BoundaryType::KEEP_CLEAR: + boundary_debug->set_type( + StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR); + break; } for (const auto& point : boundary->points()) { diff --git a/modules/planning/tasks/traffic_decider/BUILD b/modules/planning/tasks/traffic_decider/BUILD index 977bf6c23d0eee03a09806b910f94cfef205f1d3..43253147766ab933a23b7194c00967355ff7f6b9 100644 --- a/modules/planning/tasks/traffic_decider/BUILD +++ b/modules/planning/tasks/traffic_decider/BUILD @@ -6,11 +6,13 @@ cc_library( name = "traffic_rules", srcs = [ "backside_vehicle.cc", + "clear_zone.cc", "crosswalk.cc", "signal_light.cc", ], hdrs = [ "backside_vehicle.h", + "clear_zone.h", "crosswalk.h", "signal_light.h", "traffic_rule.h", diff --git a/modules/planning/tasks/traffic_decider/clear_zone.cc b/modules/planning/tasks/traffic_decider/clear_zone.cc new file mode 100644 index 0000000000000000000000000000000000000000..fc3a214ee1e68351d5409d7f77a65991c478a1fe --- /dev/null +++ b/modules/planning/tasks/traffic_decider/clear_zone.cc @@ -0,0 +1,107 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * @file + **/ + +#include "modules/planning/tasks/traffic_decider/clear_zone.h" + +#include +#include + +#include "modules/common/proto/pnc_point.pb.h" + +#include "modules/common/adapters/adapter_manager.h" +#include "modules/common/vehicle_state/vehicle_state.h" +#include "modules/planning/common/planning_gflags.h" + +namespace apollo { +namespace planning { + +using apollo::common::adapter::AdapterManager; +using apollo::perception::TrafficLight; +using apollo::perception::TrafficLightDetection; + +ClearZone::ClearZone(const RuleConfig& config) : TrafficRule(config) {} + +bool ClearZone::ApplyRule(Frame* frame, + ReferenceLineInfo* const reference_line_info) { + frame_ = frame; + reference_line_info_ = reference_line_info; + const auto& map_path = reference_line_info_->reference_line().map_path(); + const auto& clear_zones = map_path.junction_overlaps(); + for (const auto& clear_zone : clear_zones) { + if (!BuildClearZoneObstacle(clear_zone)) { + AERROR << "Failed to build clear zone : " << clear_zone.object_id; + return false; + } + } + return true; +} + +bool ClearZone::BuildClearZoneObstacle( + const hdmap::PathOverlap& clear_zone_overlap) { + const double front_edge_s = reference_line_info_->AdcSlBoundary().end_s(); + if (clear_zone_overlap.start_s < front_edge_s) { + ADEBUG << "adc is already inside clear zone " + << clear_zone_overlap.object_id << ", skip this clear zone"; + return true; + } + common::SLPoint sl_point; + sl_point.set_s(clear_zone_overlap.start_s); + sl_point.set_l(0.0); + common::math::Vec2d start_xy; + const auto& reference_line = reference_line_info_->reference_line(); + if (!reference_line.SLToXY(sl_point, &start_xy)) { + AERROR << "Failed to get xy from sl: " << sl_point.DebugString(); + return false; + } + double left_lane_width = 0.0; + double right_lane_width = 0.0; + if (!reference_line.GetLaneWidth(clear_zone_overlap.start_s, &left_lane_width, + &right_lane_width)) { + AERROR << "Failed to get lane width at s = " << clear_zone_overlap.start_s; + return false; + } + common::math::Vec2d end_xy; + sl_point.set_s(clear_zone_overlap.end_s); + if (!reference_line_info_->reference_line().SLToXY(sl_point, &end_xy)) { + AERROR << "Failed to get xy from sl: " << sl_point.DebugString(); + return false; + } + common::math::Box2d clear_zone_box{ + common::math::LineSegment2d(start_xy, end_xy), + left_lane_width + right_lane_width}; + const auto obstacle_id = + FLAGS_clear_zone_virtual_object_id_prefix + clear_zone_overlap.object_id; + auto* obstacle = + frame_->AddStaticVirtualObstacle(obstacle_id, clear_zone_box); + if (!obstacle) { + AERROR << "Failed to create obstacle " << obstacle_id << " in frame"; + return false; + } + auto* path_obstacle = reference_line_info_->AddObstacle(obstacle); + if (!path_obstacle) { + AERROR << "Failed to create path_obstacle for " << obstacle_id; + return false; + } + path_obstacle->SetStBoundaryType(StBoundary::BoundaryType::KEEP_CLEAR); + return true; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/tasks/traffic_decider/clear_zone.h b/modules/planning/tasks/traffic_decider/clear_zone.h new file mode 100644 index 0000000000000000000000000000000000000000..75a5fa36a2f444b3a3a733b949e57f4a63c1ab70 --- /dev/null +++ b/modules/planning/tasks/traffic_decider/clear_zone.h @@ -0,0 +1,54 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * @file + **/ + +#ifndef MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_ +#define MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_ + +#include +#include +#include + +#include "modules/perception/proto/traffic_light_detection.pb.h" + +#include "modules/planning/tasks/traffic_decider/traffic_rule.h" + +namespace apollo { +namespace planning { + +/** + * This class creates a virtual obstacle for each keep clear region. + */ +class ClearZone : public TrafficRule { + public: + explicit ClearZone(const RuleConfig& config); + virtual ~ClearZone() = default; + + bool ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info); + + private: + bool BuildClearZoneObstacle(const hdmap::PathOverlap& clear_zone_overlap); + ReferenceLineInfo* reference_line_info_; + Frame* frame_; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_CLEAR_ZONE_H_