From 3a9aefdf7c3be6daf560bfec79c07881fee31944 Mon Sep 17 00:00:00 2001 From: Dong Li Date: Sun, 19 Nov 2017 17:11:48 -0800 Subject: [PATCH] planning: added the feature of stop at end of reference line when needed. * handle the case that reference line generation failed or too slow --- modules/planning/common/frame.cc | 4 + modules/planning/common/frame.h | 2 + modules/planning/common/planning_gflags.cc | 2 + modules/planning/common/planning_gflags.h | 1 + modules/planning/conf/planning_config.pb.txt | 3 + modules/planning/proto/planning_config.proto | 1 + modules/planning/tasks/traffic_decider/BUILD | 2 + .../traffic_decider/reference_line_end.cc | 90 +++++++++++++++++++ .../traffic_decider/reference_line_end.h | 46 ++++++++++ .../tasks/traffic_decider/traffic_decider.cc | 7 +- 10 files changed, 156 insertions(+), 2 deletions(-) create mode 100644 modules/planning/tasks/traffic_decider/reference_line_end.cc create mode 100644 modules/planning/tasks/traffic_decider/reference_line_end.h diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index f29d119d3e..df3656c77c 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -71,6 +71,10 @@ void Frame::CreatePredictionObstacles( } } +const common::VehicleState &Frame::vehicle_state() const { + return vehicle_state_; +} + bool Frame::Rerouting() { auto *adapter_manager = AdapterManager::instance(); if (adapter_manager->GetRoutingResponse()->Empty()) { diff --git a/modules/planning/common/frame.h b/modules/planning/common/frame.h index c8fbb4b667..47a0c20bac 100644 --- a/modules/planning/common/frame.h +++ b/modules/planning/common/frame.h @@ -77,6 +77,8 @@ class Frame { bool Rerouting(); + const common::VehicleState &vehicle_state() const; + private: /** * @brief create obstacles from prediction input. diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc index f4d668b798..f1be5fc3bc 100644 --- a/modules/planning/common/planning_gflags.cc +++ b/modules/planning/common/planning_gflags.cc @@ -183,6 +183,8 @@ DEFINE_double(virtual_stop_wall_length, 0.1, "virtual stop wall length (meters)"); DEFINE_double(virtual_stop_wall_height, 2.0, "virtual stop wall height (meters)"); +DEFINE_string(reference_line_end_obstacle_id, "REF_END", + "Obstacle id for the end of reference line obstacle"); // Prediction Part DEFINE_double(prediction_total_time, 5.0, "Total prediction time"); diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h index beb6d21e24..f507c21683 100644 --- a/modules/planning/common/planning_gflags.h +++ b/modules/planning/common/planning_gflags.h @@ -104,6 +104,7 @@ DECLARE_double(follow_min_time_sec); DECLARE_string(destination_obstacle_id); DECLARE_double(virtual_stop_wall_length); DECLARE_double(virtual_stop_wall_height); +DECLARE_string(reference_line_end_obstacle_id); DECLARE_double(prediction_total_time); DECLARE_bool(align_prediction_time); diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt index 19a7b7ff4f..9cfaccdc6d 100644 --- a/modules/planning/conf/planning_config.pb.txt +++ b/modules/planning/conf/planning_config.pb.txt @@ -152,3 +152,6 @@ rule_config : { rule_config : { rule_id: REROUTING } +rule_config : { + rule_id: REFERENCE_LINE_END +} diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto index 0a2f066fe2..336db4f144 100644 --- a/modules/planning/proto/planning_config.proto +++ b/modules/planning/proto/planning_config.proto @@ -25,6 +25,7 @@ message RuleConfig { CROSSWALK = 3; CLEAR_ZONE = 4; REROUTING = 5; + REFERENCE_LINE_END = 6; } optional RuleId rule_id = 1; } diff --git a/modules/planning/tasks/traffic_decider/BUILD b/modules/planning/tasks/traffic_decider/BUILD index 862b93e7e4..d65734178e 100644 --- a/modules/planning/tasks/traffic_decider/BUILD +++ b/modules/planning/tasks/traffic_decider/BUILD @@ -9,6 +9,7 @@ cc_library( "clear_zone.cc", "crosswalk.cc", "object_priority.cc", + "reference_line_end.cc", "rerouting.cc", "signal_light.cc", ], @@ -17,6 +18,7 @@ cc_library( "clear_zone.h", "crosswalk.h", "object_priority.h", + "reference_line_end.h", "rerouting.h", "signal_light.h", "traffic_rule.h", diff --git a/modules/planning/tasks/traffic_decider/reference_line_end.cc b/modules/planning/tasks/traffic_decider/reference_line_end.cc new file mode 100644 index 0000000000..c063215da7 --- /dev/null +++ b/modules/planning/tasks/traffic_decider/reference_line_end.cc @@ -0,0 +1,90 @@ +/****************************************************************************** + * 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/reference_line_end.h" + +#include "modules/common/proto/pnc_point.pb.h" + +#include "modules/common/configs/vehicle_config_helper.h" +#include "modules/planning/common/planning_gflags.h" + +namespace apollo { +namespace planning { + +using apollo::perception::TrafficLight; +using apollo::perception::TrafficLightDetection; + +ReferenceLineEnd::ReferenceLineEnd(const RuleConfig& config) + : TrafficRule(config) {} + +bool ReferenceLineEnd::ApplyRule(Frame* frame, + ReferenceLineInfo* const reference_line_info) { + const auto& reference_line = reference_line_info->reference_line(); + double remain_s = + reference_line.Length() - reference_line_info->AdcSlBoundary().end_s(); + const double& velocity = frame->vehicle_state().linear_velocity(); + const double stop_acc = std::fabs(common::VehicleConfigHelper::GetConfig() + .vehicle_param() + .max_deceleration() / + 3.0); + const double stop_s = velocity * velocity / (2.0 * stop_acc) + + FLAGS_virtual_stop_wall_length + + FLAGS_stop_distance_destination; + if (stop_s < remain_s) { + ADEBUG << "have enough reference line to drive on"; + return true; + } + // Need to create an obstacle at the end of reference line to stop the + // vehicle + double left_width = 0.0; + double right_width = 0.0; + double obstacle_start_s = + reference_line.Length() - FLAGS_virtual_stop_wall_length; + reference_line.GetLaneWidth(obstacle_start_s, &left_width, &right_width); + + auto center_point = reference_line.GetReferencePoint( + obstacle_start_s + FLAGS_virtual_stop_wall_length / 2.0); + + common::math::Box2d stop_box{center_point, center_point.heading(), + FLAGS_virtual_stop_wall_length, + left_width + right_width}; + + auto* obstacle = frame->AddStaticVirtualObstacle( + FLAGS_reference_line_end_obstacle_id + "_" + + reference_line_info->Lanes().Id(), + stop_box); + auto* path_obstacle = reference_line_info->AddObstacle(obstacle); + auto* path_decision = reference_line_info->path_decision(); + auto stop_point = reference_line.GetReferencePoint( + obstacle_start_s - FLAGS_stop_distance_destination); + ObjectDecisionType stop; + stop.mutable_stop(); + stop.mutable_stop()->set_distance_s(-FLAGS_stop_distance_destination); + stop.mutable_stop()->set_stop_heading(stop_point.heading()); + stop.mutable_stop()->mutable_stop_point()->set_x(stop_point.x()); + stop.mutable_stop()->mutable_stop_point()->set_y(stop_point.y()); + stop.mutable_stop()->mutable_stop_point()->set_z(0.0); + path_decision->AddLongitudinalDecision( + RuleConfig::RuleId_Name(config_.rule_id()), path_obstacle->Id(), stop); + return true; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/tasks/traffic_decider/reference_line_end.h b/modules/planning/tasks/traffic_decider/reference_line_end.h new file mode 100644 index 0000000000..88edf0d1fc --- /dev/null +++ b/modules/planning/tasks/traffic_decider/reference_line_end.h @@ -0,0 +1,46 @@ +/****************************************************************************** + * 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_REFERENCE_LINE_END_H_ +#define MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_REFERENCE_LINE_END_H_ + +#include + +#include "modules/planning/tasks/traffic_decider/traffic_rule.h" + +namespace apollo { +namespace planning { + +/** + * This class decides whether we should send rerouting request based on traffic + * situation. + */ +class ReferenceLineEnd : public TrafficRule { + public: + explicit ReferenceLineEnd(const RuleConfig& config); + virtual ~ReferenceLineEnd() = default; + + bool ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info); +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_TASKS_TRAFFIC_DECIDER_REFERENCE_LINE_END_H_ diff --git a/modules/planning/tasks/traffic_decider/traffic_decider.cc b/modules/planning/tasks/traffic_decider/traffic_decider.cc index d3fc04b71c..8b86b29bc7 100644 --- a/modules/planning/tasks/traffic_decider/traffic_decider.cc +++ b/modules/planning/tasks/traffic_decider/traffic_decider.cc @@ -24,6 +24,7 @@ #include "modules/planning/common/planning_gflags.h" #include "modules/planning/tasks/traffic_decider/backside_vehicle.h" #include "modules/planning/tasks/traffic_decider/crosswalk.h" +#include "modules/planning/tasks/traffic_decider/reference_line_end.h" #include "modules/planning/tasks/traffic_decider/rerouting.h" #include "modules/planning/tasks/traffic_decider/signal_light.h" @@ -39,7 +40,6 @@ void TrafficDecider::RegisterRules() { [](const RuleConfig &config) -> TrafficRule * { return new BacksideVehicle(config); }); - rule_factory_.Register(RuleConfig::SIGNAL_LIGHT, [](const RuleConfig &config) -> TrafficRule * { return new SignalLight(config); @@ -49,11 +49,14 @@ void TrafficDecider::RegisterRules() { [](const RuleConfig &config) -> TrafficRule * { return new Crosswalk(config); }); - rule_factory_.Register(RuleConfig::REROUTING, [](const RuleConfig &config) -> TrafficRule * { return new Rerouting(config); }); + rule_factory_.Register(RuleConfig::REFERENCE_LINE_END, + [](const RuleConfig &config) -> TrafficRule * { + return new ReferenceLineEnd(config); + }); } bool TrafficDecider::Init(const PlanningConfig &config) { -- GitLab