From e0a8520e35465e2598d411000e09492cf3325e94 Mon Sep 17 00:00:00 2001 From: Dong Li Date: Wed, 2 Aug 2017 10:49:57 -0700 Subject: [PATCH] remove remove PlanningObject and refactor Obstacle class --- modules/planning/common/BUILD | 18 +- modules/planning/common/decision_data.cc | 11 +- modules/planning/common/decision_data.h | 6 +- modules/planning/common/frame.cc | 40 +-- modules/planning/common/frame.h | 11 +- modules/planning/common/obstacle.cc | 91 ++++--- modules/planning/common/obstacle.h | 87 +++---- modules/planning/common/planning_data.h | 1 - modules/planning/common/planning_object.cc | 52 ---- modules/planning/common/planning_object.h | 58 ----- .../optimizer/dp_poly_path/dp_road_graph.cc | 61 ++--- .../optimizer/dp_poly_path/trajectory_cost.cc | 31 +-- .../optimizer/dp_st_speed/dp_st_graph.cc | 4 +- .../optimizer/dp_st_speed/dp_st_graph.h | 4 +- .../dp_st_speed/dp_st_speed_optimizer.cc | 2 +- .../qp_spline_path/qp_frenet_frame.cc | 22 +- modules/planning/optimizer/st_graph/BUILD | 1 - .../st_graph/st_boundary_mapper_impl.cc | 232 +++++++++--------- 18 files changed, 281 insertions(+), 451 deletions(-) delete mode 100644 modules/planning/common/planning_object.cc delete mode 100644 modules/planning/common/planning_object.h diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD index b79d02857f..8895215916 100644 --- a/modules/planning/common/BUILD +++ b/modules/planning/common/BUILD @@ -39,9 +39,11 @@ cc_library( ], deps = [ "//modules/common/math:box2d", + "//modules/common/math:polygon2d", "//modules/perception/proto:perception_proto", - "//modules/planning/common:planning_object", "//modules/planning/common:planning_util", + "//modules/planning/proto:planning_proto", + "//modules/prediction/proto:prediction_proto", ], ) @@ -58,20 +60,6 @@ cc_library( ], ) -cc_library( - name = "planning_object", - srcs = [ - "planning_object.cc", - ], - hdrs = [ - "planning_object.h", - ], - deps = [ - "//modules/common/math:polygon2d", - "//modules/planning/proto:planning_proto", - ], -) - cc_library( name = "planning_data", srcs = [ diff --git a/modules/planning/common/decision_data.cc b/modules/planning/common/decision_data.cc index 1e228ad1eb..7621c7240b 100644 --- a/modules/planning/common/decision_data.cc +++ b/modules/planning/common/decision_data.cc @@ -27,7 +27,7 @@ const DecisionResult &DecisionData::Decision() const { return decision_; } DecisionResult *DecisionData::MutableDecision() { return &decision_; } -const std::list &DecisionData::Obstacles() const { +const std::vector &DecisionData::Obstacles() const { return obstacles_; } @@ -47,12 +47,13 @@ std::vector *DecisionData::MutableDynamicObstacles() { return &dynamic_obstacles_; } -void DecisionData::AddObstacle(const Obstacle &obstacle) { +void DecisionData::AddObstacle(Obstacle *obstacle) { obstacles_.push_back(obstacle); - if (obstacle.Type() == perception::PerceptionObstacle::UNKNOWN_UNMOVABLE) { - static_obstacles_.push_back(&(obstacles_.back())); + if (obstacle->Perception().type() == + perception::PerceptionObstacle::UNKNOWN_UNMOVABLE) { + static_obstacles_.push_back(obstacle); } else { - dynamic_obstacles_.push_back(&(obstacles_.back())); + dynamic_obstacles_.push_back(obstacle); } } diff --git a/modules/planning/common/decision_data.h b/modules/planning/common/decision_data.h index 3e24e4ee15..7b6a6d3576 100644 --- a/modules/planning/common/decision_data.h +++ b/modules/planning/common/decision_data.h @@ -36,16 +36,16 @@ class DecisionData { const DecisionResult &Decision() const; DecisionResult *MutableDecision(); - const std::list &Obstacles() const; + const std::vector &Obstacles() const; const std::vector &StaticObstacles() const; const std::vector &DynamicObstacles() const; std::vector *MutableStaticObstacles(); std::vector *MutableDynamicObstacles(); - void AddObstacle(const Obstacle &obstacle); + void AddObstacle(Obstacle *obstacle); private: DecisionResult decision_; - std::list obstacles_; + std::vector obstacles_; std::vector static_obstacles_; std::vector dynamic_obstacles_; }; diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc index 554e6b5bc8..b34d334cc5 100644 --- a/modules/planning/common/frame.cc +++ b/modules/planning/common/frame.cc @@ -60,35 +60,19 @@ const common::TrajectoryPoint &Frame::PlanningStartPoint() const { return planning_start_point_; } -void Frame::SetDecisionDataFromPrediction( - const prediction::PredictionObstacles &prediction_obstacles) { - for (const auto &prediction_obstacle : - prediction_obstacles.prediction_obstacle()) { - Obstacle obstacle; - auto &perception_obstacle = prediction_obstacle.perception_obstacle(); - obstacle.SetId(std::to_string(perception_obstacle.id())); - obstacle.SetType(perception_obstacle.type()); - obstacle.SetHeight(perception_obstacle.height()); - obstacle.SetWidth(perception_obstacle.width()); - obstacle.SetLength(perception_obstacle.length()); - double theta = perception_obstacle.theta(); - obstacle.SetHeading(theta); - double speed = perception_obstacle.velocity().x() * cos(theta) + - perception_obstacle.velocity().y() * sin(theta); - obstacle.SetSpeed(speed); - - for (const auto &trajectory : prediction_obstacle.trajectory()) { - obstacle.add_prediction_trajectory(trajectory); - } - - mutable_planning_data()->mutable_decision_data()->AddObstacle(obstacle); - } -} - void Frame::SetPrediction(const prediction::PredictionObstacles &prediction) { prediction_ = prediction; } +void Frame::CreateObstacles(const prediction::PredictionObstacles &prediction) { + std::list > obstacles; + Obstacle::CreateObstacles(prediction, &obstacles); + for (auto &ptr : obstacles) { + mutable_planning_data()->mutable_decision_data()->AddObstacle(ptr.get()); + obstacles_.Add(ptr->Id(), std::move(ptr)); + } +} + bool Frame::Init() { if (!pnc_map_) { AERROR << "map is null, call SetMap() first"; @@ -109,7 +93,7 @@ bool Frame::Init() { return false; } if (FLAGS_enable_prediction) { - SetDecisionDataFromPrediction(prediction_); + CreateObstacles(prediction_); } return true; } @@ -172,8 +156,8 @@ bool Frame::SmoothReferenceLine() { return true; } -const ObstacleTable &Frame::GetObstacleTable() const { return obstacle_table_; } -ObstacleTable *Frame::MutableObstacleTable() { return &obstacle_table_; } +const Obstacles &Frame::GetObstacles() const { return obstacles_; } +Obstacles *Frame::MutableObstacles() { return &obstacles_; } std::string Frame::DebugString() const { return "Frame: " + std::to_string(sequence_num_); diff --git a/modules/planning/common/frame.h b/modules/planning/common/frame.h index e935b5325e..9cafbc9706 100644 --- a/modules/planning/common/frame.h +++ b/modules/planning/common/frame.h @@ -39,7 +39,7 @@ namespace apollo { namespace planning { -using ObstacleTable = IndexedList; +using Obstacles = IndexedList; class Frame { public: @@ -61,8 +61,8 @@ class Frame { PlanningData *mutable_planning_data(); std::string DebugString() const; - const ObstacleTable &GetObstacleTable() const; - ObstacleTable *MutableObstacleTable(); + const Obstacles &GetObstacles() const; + Obstacles *MutableObstacles(); void set_computed_trajectory(const PublishableTrajectory &trajectory); const PublishableTrajectory &computed_trajectory() const; @@ -70,15 +70,14 @@ class Frame { private: bool CreateReferenceLineFromRouting(); bool SmoothReferenceLine(); - void SetDecisionDataFromPrediction( - const prediction::PredictionObstacles &prediction_obstacles); + void CreateObstacles(const prediction::PredictionObstacles &prediction); private: common::TrajectoryPoint planning_start_point_; hdmap::RoutingResponse routing_result_; prediction::PredictionObstacles prediction_; - ObstacleTable obstacle_table_; + Obstacles obstacles_; uint32_t sequence_num_ = 0; hdmap::Path hdmap_path_; localization::Pose init_pose_; diff --git a/modules/planning/common/obstacle.cc b/modules/planning/common/obstacle.cc index 46c45b80ce..7a279b3432 100644 --- a/modules/planning/common/obstacle.cc +++ b/modules/planning/common/obstacle.cc @@ -22,6 +22,7 @@ #include +#include "modules/common/log.h" #include "modules/planning/common/planning_util.h" namespace apollo { @@ -29,48 +30,25 @@ namespace planning { const std::string& Obstacle::Id() const { return id_; } -void Obstacle::SetId(const std::string& id) { id_ = id; } - -double Obstacle::Height() const { return height_; } - -void Obstacle::SetHeight(const double height) { height_ = height; } - -double Obstacle::Width() const { return width_; } - -void Obstacle::SetWidth(const double width) { width_ = width; } - -double Obstacle::Length() const { return length_; } - -void Obstacle::SetLength(const double length) { length_ = length; } - -double Obstacle::Heading() const { return heading_; } - -void Obstacle::SetHeading(const double heading) { heading_ = heading; } - -common::math::Box2d Obstacle::BoundingBox() const { - return ::apollo::common::math::Box2d(center_, heading_, length_, width_); -} - -const perception::PerceptionObstacle::Type& Obstacle::Type() const { - return type_; -} - -void Obstacle::SetType(const perception::PerceptionObstacle::Type& type) { - type_ = type; -} - -double Obstacle::Speed() const { return speed_; } - -void Obstacle::SetSpeed(const double speed) { speed_ = speed; } - -common::TrajectoryPoint Obstacle::get_point_at( - const prediction::Trajectory& trajectory, +Obstacle::Obstacle(const std::string& id, + const perception::PerceptionObstacle& perception_obstacle, + const prediction::Trajectory& trajectory) + : id_(id), + trajectory_(trajectory), + perception_obstacle_(perception_obstacle), + perception_bounding_box_({perception_obstacle_.position().x(), + perception_obstacle_.position().y()}, + perception_obstacle_.theta(), + perception_obstacle_.length(), + perception_obstacle_.width()) {} + +common::TrajectoryPoint Obstacle::GetPointAtTime( const double relative_time) const { auto comp = [](const common::TrajectoryPoint p, const double relative_time) { return p.relative_time() < relative_time; }; - const auto& points = trajectory.trajectory_point(); + const auto& points = trajectory_.trajectory_point(); auto it_lower = std::lower_bound(points.begin(), points.end(), relative_time, comp); @@ -80,5 +58,44 @@ common::TrajectoryPoint Obstacle::get_point_at( return util::interpolate(*(it_lower - 1), *it_lower, relative_time); } +common::math::Box2d Obstacle::GetBoundingBox( + const common::TrajectoryPoint& point) const { + return common::math::Box2d({point.path_point().x(), point.path_point().y()}, + point.path_point().theta(), + perception_obstacle_.length(), + perception_obstacle_.width()); +} + +const common::math::Box2d& Obstacle::PerceptionBoundingBox() const { + return perception_bounding_box_; +} + +const prediction::Trajectory& Obstacle::Trajectory() const { + return trajectory_; +} + +const perception::PerceptionObstacle& Obstacle::Perception() const { + return perception_obstacle_; +} + +void Obstacle::CreateObstacles( + const prediction::PredictionObstacles& predictions, + std::list>* obstacles) { + if (!obstacles) { + AERROR << "the provided obstacles is empty"; + return; + } + for (const auto& prediction_obstacle : predictions.prediction_obstacle()) { + const auto perception_id = prediction_obstacle.perception_obstacle().id(); + int trajectory_index = 0; + for (const auto& trajectory : prediction_obstacle.trajectory()) { + std::string obstacle_id = std::to_string(perception_id) + "_" + + std::to_string(trajectory_index); + obstacles->emplace_back(std::unique_ptr(new Obstacle( + obstacle_id, prediction_obstacle.perception_obstacle(), trajectory))); + } + } +} + } // namespace planning } // namespace apollo diff --git a/modules/planning/common/obstacle.h b/modules/planning/common/obstacle.h index 7cf161e328..e7bce47951 100644 --- a/modules/planning/common/obstacle.h +++ b/modules/planning/common/obstacle.h @@ -21,72 +21,65 @@ #ifndef MODULES_PLANNING_COMMON_OBSTACLE_H_ #define MODULES_PLANNING_COMMON_OBSTACLE_H_ -#include +#include #include -#include "modules/common/math/box2d.h" #include "modules/perception/proto/perception_obstacle.pb.h" -#include "modules/planning/common/planning_object.h" +#include "modules/planning/proto/decision.pb.h" +#include "modules/prediction/proto/prediction_obstacle.pb.h" + +#include "modules/common/math/box2d.h" +#include "modules/common/math/vec2d.h" namespace apollo { namespace planning { -class Obstacle : public PlanningObject { +class Obstacle { public: Obstacle() = default; - const std::string &Id() const; - - void SetId(const std::string &id); - - const perception::PerceptionObstacle::Type &Type() const; - void SetType(const perception::PerceptionObstacle::Type &type); - - double Height() const; - void SetHeight(const double height); + Obstacle(const std::string &id, + const perception::PerceptionObstacle &perception, + const prediction::Trajectory &trajectory); - double Width() const; - void SetWidth(const double width); - - double Length() const; - void SetLength(const double length); - - double Heading() const; - void SetHeading(const double heading); + const std::string &Id() const; - double Speed() const; - void SetSpeed(const double speed); + common::TrajectoryPoint GetPointAtTime(const double time) const; + common::math::Box2d GetBoundingBox( + const common::TrajectoryPoint &point) const; + /** + * @brief get the perception bounding box + */ + const common::math::Box2d &PerceptionBoundingBox() const; - apollo::common::math::Vec2d Center() const { return center_; }; + const prediction::Trajectory &Trajectory() const; - apollo::common::math::Box2d BoundingBox() const; + const perception::PerceptionObstacle &Perception() const; - const std::vector &prediction_trajectories() const { - return trajectories_; + // FIXME fake functions, will be migrate out soon. + const std::vector &Decisions() const { + return decisions_; } - void add_prediction_trajectory( - const prediction::Trajectory &prediction_trajectory) { - trajectories_.push_back(prediction_trajectory); - } - - common::TrajectoryPoint get_point_at(const prediction::Trajectory &trajectory, - const double time) const; + std::vector *MutableDecisions() { return &decisions_; } + + /** + * @brief This is a helper function that can create obstacles from prediction + * data. The original prediction may have multiple trajectories for each + * obstacle. But this function will create one obstacle for each trajectory. + * @param predictions The prediction results + * @param obstacles The output obstacles saved in a list of unique_ptr. + */ + static void CreateObstacles( + const prediction::PredictionObstacles &predictions, + std::list> *obstacles); private: std::string id_ = 0; - - // TODO: (Liangliang) set those parameters. - double height_ = 0.0; - double width_ = 0.0; - double length_ = 0.0; - double heading_ = 0.0; - // NOTICE: check is speed_ is set before usage. - double speed_ = 0.0; - - std::vector trajectories_; - ::apollo::common::math::Vec2d center_; - perception::PerceptionObstacle::Type type_ = - perception::PerceptionObstacle::VEHICLE; + prediction::Trajectory trajectory_; + perception::PerceptionObstacle perception_obstacle_; + // FIXME move out later + std::vector decisions_; + common::math::Box2d perception_bounding_box_; }; } // namespace planning diff --git a/modules/planning/common/planning_data.h b/modules/planning/common/planning_data.h index e18d10a027..71dae90015 100644 --- a/modules/planning/common/planning_data.h +++ b/modules/planning/common/planning_data.h @@ -30,7 +30,6 @@ #include "modules/planning/common/decision_data.h" #include "modules/planning/common/path/path_data.h" #include "modules/planning/common/planning_data.h" -#include "modules/planning/common/planning_object.h" #include "modules/planning/common/speed/speed_data.h" #include "modules/planning/common/trajectory/publishable_trajectory.h" #include "modules/planning/reference_line/reference_line.h" diff --git a/modules/planning/common/planning_object.cc b/modules/planning/common/planning_object.cc deleted file mode 100644 index 6e82aaec8a..0000000000 --- a/modules/planning/common/planning_object.cc +++ /dev/null @@ -1,52 +0,0 @@ -/****************************************************************************** - * 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: planning_object.cc - * - **/ - -#include "modules/planning/common/planning_object.h" - -namespace apollo { -namespace planning { - -PlanningObject::PlanningObjectType PlanningObject::ObjectType() const { - return object_type_; -} - -PlanningObject::PlanningObjectType* PlanningObject::MutableObjectType() { - return &object_type_; -} - -const std::vector& PlanningObject::Decisions() const { - return decisions_; -} - -std::vector* PlanningObject::MutableDecisions() { - return &decisions_; -} - -const apollo::common::math::Polygon2d& PlanningObject::Polygon() const { - return polygon_; -} - -apollo::common::math::Polygon2d* PlanningObject::MutablePolygon() { - return &polygon_; -} - -} // namespace planning -} // namespace apollo diff --git a/modules/planning/common/planning_object.h b/modules/planning/common/planning_object.h deleted file mode 100644 index c8e91a6a2f..0000000000 --- a/modules/planning/common/planning_object.h +++ /dev/null @@ -1,58 +0,0 @@ -/****************************************************************************** - * 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: planning_object.h - * - **/ - -#ifndef MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_ -#define MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_ - -#include - -#include "modules/common/math/polygon2d.h" -#include "modules/planning/proto/decision.pb.h" - -namespace apollo { -namespace planning { - -class PlanningObject { - public: - enum class PlanningObjectType { - OBSTACLE = 0, - MAP_OBJECT = 1, - }; - - public: - PlanningObject() = default; - virtual PlanningObjectType ObjectType() const; - virtual PlanningObjectType* MutableObjectType(); - virtual const apollo::common::math::Polygon2d& Polygon() const; - virtual apollo::common::math::Polygon2d* MutablePolygon(); - virtual const std::vector& Decisions() const; - virtual std::vector* MutableDecisions(); - - private: - PlanningObjectType object_type_; - apollo::common::math::Polygon2d polygon_; - std::vector decisions_; -}; - -} // namespace planning -} // namespace apollo - -#endif // MODULES_PLANNING_COMMON_PLANNING_OBJECT_H_ diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc index fc54947166..9adb61e0ed 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc @@ -264,7 +264,8 @@ bool DpRoadGraph::compute_object_decision_from_path( for (std::size_t i = 0; i < static_obstacles->size(); ++i) { bool ignore = true; - const auto &static_obstacle_box = (*static_obstacles)[i]->BoundingBox(); + const auto &static_obstacle_box = + (*static_obstacles)[i]->PerceptionBoundingBox(); common::SLPoint obs_sl; if (!reference_line.get_point_in_frenet_frame( @@ -330,8 +331,7 @@ bool DpRoadGraph::compute_object_decision_from_path( size_t evaluate_times = static_cast( std::floor(total_time / config_.eval_time_interval())); for (size_t i = 0; i < dynamic_obstacles->size(); ++i) { - const auto &trajectories = - (*dynamic_obstacles)[i]->prediction_trajectories(); + auto *obstacle = (*dynamic_obstacles)[i]; // list of Box2d for ego car given heuristic speed profile std::vector<::apollo::common::math::Box2d> ego_by_time; @@ -340,40 +340,31 @@ bool DpRoadGraph::compute_object_decision_from_path( AERROR << "fill_ego_by_time error"; } - for (size_t j = 0; j < trajectories.size(); ++j) { - const auto &trajectory = trajectories[j]; - std::vector<::apollo::common::math::Box2d> obstacle_by_time; - for (size_t time = 0; time <= evaluate_times; ++time) { - auto traj_point = (*dynamic_obstacles)[i]->get_point_at( - trajectory, time * config_.eval_time_interval()); - ::apollo::common::math::Vec2d center_point = { - traj_point.path_point().x(), traj_point.path_point().y()}; - ::apollo::common::math::Box2d obstacle_box = { - center_point, traj_point.path_point().theta(), - (*dynamic_obstacles)[i]->BoundingBox().length(), - (*dynamic_obstacles)[i]->BoundingBox().width()}; - obstacle_by_time.push_back(obstacle_box); - } + std::vector<::apollo::common::math::Box2d> obstacle_by_time; + for (size_t time = 0; time <= evaluate_times; ++time) { + auto traj_point = + obstacle->GetPointAtTime(time * config_.eval_time_interval()); + obstacle_by_time.push_back(obstacle->GetBoundingBox(traj_point)); + } - // if two lists of boxes collide - if (obstacle_by_time.size() != ego_by_time.size()) { - AINFO << "dynamic_obstacle_by_time size[" << obstacle_by_time.size() - << "] != ego_by_time[" << ego_by_time.size() - << "] from heuristic_speed_data"; - continue; - } + // if two lists of boxes collide + if (obstacle_by_time.size() != ego_by_time.size()) { + AINFO << "dynamic_obstacle_by_time size[" << obstacle_by_time.size() + << "] != ego_by_time[" << ego_by_time.size() + << "] from heuristic_speed_data"; + continue; + } - // judge for collision - for (size_t k = 0; k < obstacle_by_time.size(); ++k) { - if (ego_by_time[k].DistanceTo(obstacle_by_time[k]) < - FLAGS_dynamic_decision_follow_range) { - // Follow - ObjectDecisionType object_follow; - ObjectFollow *object_follow_ptr = object_follow.mutable_follow(); - object_follow_ptr->set_distance_s(FLAGS_dp_path_decision_buffer); - (*dynamic_obstacles)[i]->MutableDecisions()->push_back(object_follow); - break; - } + // judge for collision + for (size_t k = 0; k < obstacle_by_time.size(); ++k) { + if (ego_by_time[k].DistanceTo(obstacle_by_time[k]) < + FLAGS_dynamic_decision_follow_range) { + // Follow + ObjectDecisionType object_follow; + ObjectFollow *object_follow_ptr = object_follow.mutable_follow(); + object_follow_ptr->set_distance_s(FLAGS_dp_path_decision_buffer); + (*dynamic_obstacles)[i]->MutableDecisions()->push_back(object_follow); + break; } } } diff --git a/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc index b4dc4c46fa..2056984f17 100644 --- a/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc +++ b/modules/planning/optimizer/dp_poly_path/trajectory_cost.cc @@ -50,31 +50,22 @@ TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config, // Mapping Static obstacle for (const auto ptr_static_obstacle : decision_data.StaticObstacles()) { - Vec2d center_point = ptr_static_obstacle->Center(); - Box2d obstacle_box = {center_point, ptr_static_obstacle->Heading(), - ptr_static_obstacle->BoundingBox().length(), - ptr_static_obstacle->BoundingBox().width()}; - static_obstacle_boxes_.push_back(std::move(obstacle_box)); + static_obstacle_boxes_.push_back( + ptr_static_obstacle->PerceptionBoundingBox()); } // Mapping dynamic obstacle for (const auto ptr_dynamic_obstacle : decision_data.DynamicObstacles()) { - for (const auto trajectory : - ptr_dynamic_obstacle->prediction_trajectories()) { - std::vector obstacle_by_time; - for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) { - TrajectoryPoint traj_point = ptr_dynamic_obstacle->get_point_at( - trajectory, time * config.eval_time_interval()); - Vec2d center_point = {traj_point.path_point().x(), - traj_point.path_point().y()}; - Box2d obstacle_box = {center_point, traj_point.path_point().theta(), - ptr_dynamic_obstacle->BoundingBox().length(), - ptr_dynamic_obstacle->BoundingBox().width()}; - obstacle_by_time.push_back(std::move(obstacle_box)); - } - dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time)); - dynamic_obstacle_probability_.push_back(trajectory.probability()); + const auto &trajectory = ptr_dynamic_obstacle->Trajectory(); + std::vector obstacle_by_time; + for (std::size_t time = 0; time < evaluate_times_ + 1; ++time) { + TrajectoryPoint traj_point = ptr_dynamic_obstacle->GetPointAtTime( + time * config.eval_time_interval()); + obstacle_by_time.push_back( + ptr_dynamic_obstacle->GetBoundingBox(traj_point)); } + dynamic_obstacle_trajectory_.push_back(std::move(obstacle_by_time)); + dynamic_obstacle_probability_.push_back(trajectory.probability()); } CHECK(dynamic_obstacle_trajectory_.size() == dynamic_obstacle_probability_.size()); diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc b/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc index 0832a3caef..57762a08eb 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc +++ b/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc @@ -40,7 +40,7 @@ DpStGraph::DpStGraph(const DpStSpeedConfig& dp_config) Status DpStGraph::Search(const StGraphData& st_graph_data, DecisionData* const decision_data, - SpeedData* const speed_data, ObstacleTable* table) { + SpeedData* const speed_data, Obstacles* table) { init_point_ = st_graph_data.init_point(); if (st_graph_data.path_data_length() < @@ -283,7 +283,7 @@ Status DpStGraph::retrieve_speed_profile(SpeedData* const speed_data) const { Status DpStGraph::get_object_decision(const StGraphData& st_graph_data, const SpeedData& speed_profile, - ObstacleTable* obstacles) const { + Obstacles* obstacles) const { if (speed_profile.speed_vector().size() < 2) { const std::string msg = "dp_st_graph failed to get speed profile."; AERROR << msg; diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_graph.h b/modules/planning/optimizer/dp_st_speed/dp_st_graph.h index 538e044a4c..8fb893b1c0 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_graph.h +++ b/modules/planning/optimizer/dp_st_speed/dp_st_graph.h @@ -45,7 +45,7 @@ class DpStGraph { apollo::common::Status Search(const StGraphData& st_graph_data, DecisionData* const decision_data, SpeedData* const speed_data, - ObstacleTable* table); + Obstacles* table); private: apollo::common::Status InitCostTable(); @@ -57,7 +57,7 @@ class DpStGraph { apollo::common::Status get_object_decision(const StGraphData& st_graph_data, const SpeedData& speed_profile, - ObstacleTable* obstacles) const; + Obstacles* obstacles) const; apollo::common::Status CalculateTotalCost(const StGraphData& st_graph_data); void CalculateCostAt(const StGraphData& st_graph_data, const uint32_t r, diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc b/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc index 2287b1df66..17a11bb2c4 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc +++ b/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc @@ -96,7 +96,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data, DpStGraph st_graph(dp_st_speed_config_); if (!st_graph .Search(st_graph_data, decision_data, speed_data, - frame_->MutableObstacleTable()) + frame_->MutableObstacles()) .ok()) { const std::string msg = "Failed to search graph with dynamic programming."; AERROR << msg; diff --git a/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc b/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc index b84862ba8f..474b2aa1d1 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc @@ -185,14 +185,7 @@ bool QpFrenetFrame::mapping_dynamic_obstacle_with_decision( const Obstacle& obstacle) { const std::vector& decision = obstacle.Decisions(); - if (decision.size() != obstacle.prediction_trajectories().size()) { - AERROR << "prediction_trajectories size[" - << obstacle.prediction_trajectories().size() - << " does NOT match decision size[" << decision.size() << "]."; - return true; - } - - for (uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) { + for (uint32_t i = 0; i < decision.size(); ++i) { if (!decision[i].has_nudge()) { continue; } @@ -200,18 +193,13 @@ bool QpFrenetFrame::mapping_dynamic_obstacle_with_decision( double buffer = std::fabs(nudge.distance_l()); int nudge_side = nudge.type() == ObjectNudge::RIGHT_NUDGE ? 1 : -1; - const prediction::Trajectory& predicted_trajectory = - obstacle.prediction_trajectories()[i]; for (const SpeedPoint& veh_point : _discretized_veh_loc) { double time = veh_point.t(); - common::TrajectoryPoint trajectory_point = - obstacle.get_point_at(predicted_trajectory, time); + common::TrajectoryPoint trajectory_point = obstacle.GetPointAtTime(time); common::math::Vec2d xy_point(trajectory_point.path_point().x(), trajectory_point.path_point().y()); - common::math::Box2d obs_box(xy_point, - trajectory_point.path_point().theta(), - obstacle.Length(), obstacle.Width()); + common::math::Box2d obs_box = obstacle.GetBoundingBox(trajectory_point); // project obs_box on reference line std::vector corners; obs_box.GetAllCorners(&corners); @@ -282,7 +270,7 @@ bool QpFrenetFrame::mapping_static_obstacle_with_decision( const auto& nudge = decision.nudge(); const double buffer = std::fabs(nudge.distance_l()); if (nudge.type() == ObjectNudge::RIGHT_NUDGE) { - common::math::Box2d box = obstacle.BoundingBox(); + const auto& box = obstacle.PerceptionBoundingBox(); std::vector corners; box.GetAllCorners(&corners); if (!mapping_polygon(corners, buffer, -1, &_static_obstacle_bound)) { @@ -291,7 +279,7 @@ bool QpFrenetFrame::mapping_static_obstacle_with_decision( return false; } } else { - common::math::Box2d box = obstacle.BoundingBox(); + const auto& box = obstacle.PerceptionBoundingBox(); std::vector corners; box.GetAllCorners(&corners); if (!mapping_polygon(corners, buffer, 1, &_static_obstacle_bound)) { diff --git a/modules/planning/optimizer/st_graph/BUILD b/modules/planning/optimizer/st_graph/BUILD index c26e560811..cee60c4a3a 100644 --- a/modules/planning/optimizer/st_graph/BUILD +++ b/modules/planning/optimizer/st_graph/BUILD @@ -67,7 +67,6 @@ cc_library( "//modules/planning/common:frame", "//modules/planning/common:obstacle", "//modules/planning/common:planning_gflags", - "//modules/planning/common:planning_object", "//modules/planning/common:speed_limit", "//modules/planning/common/path:discretized_path", "//modules/planning/common/path:frenet_frame_path", diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc index 7483644125..aa7dab9c81 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc @@ -235,11 +235,12 @@ Status StBoundaryMapperImpl::MapObstacleWithPredictionTrajectory( std::vector lower_points; std::vector upper_points; - const double speed = obstacle.Speed(); + const auto& speed = obstacle.Perception().velocity(); + const double scalar_speed = std::hypot(speed.x(), speed.y()); const double minimal_follow_time = st_boundary_config().minimal_follow_time(); double follow_distance = -1.0; if (obj_decision.has_follow()) { - follow_distance = std::fmax(speed * minimal_follow_time, + follow_distance = std::fmax(scalar_speed * minimal_follow_time, std::fabs(obj_decision.follow().distance_s())) + vehicle_param().front_edge_to_center(); } @@ -247,118 +248,111 @@ Status StBoundaryMapperImpl::MapObstacleWithPredictionTrajectory( bool skip = true; std::vector boundary_points; const auto& adc_path_points = path_data.discretized_path().points(); - if (obstacle.prediction_trajectories().size() == 0) { + const auto& trajectory = obstacle.Trajectory(); + if (trajectory.trajectory_point_size() == 0) { AWARN << "Obstacle (id = " << obstacle.Id() << ") has NO prediction trajectory."; } - for (uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) { - const auto& trajectory = obstacle.prediction_trajectories()[i]; - for (int j = 0; j < trajectory.trajectory_point_size(); ++j) { - const auto& trajectory_point = trajectory.trajectory_point(j); - // TODO(all): fix trajectory point relative time issue. - double trajectory_point_time = trajectory_point.relative_time(); - const Box2d obs_box( - Vec2d(trajectory_point.path_point().x(), - trajectory_point.path_point().y()), - trajectory_point.path_point().theta(), - obstacle.Length() * st_boundary_config().expending_coeff(), - obstacle.Width() * st_boundary_config().expending_coeff()); - int64_t low = 0; - int64_t high = adc_path_points.size() - 1; - bool find_low = false; - bool find_high = false; - while (low < high) { - if (find_low && find_high) { - break; - } - if (!find_low) { - if (!CheckOverlap(adc_path_points[low], vehicle_param(), obs_box, - st_boundary_config().boundary_buffer())) { - ++low; - } else { - find_low = true; - } - } - if (!find_high) { - if (!CheckOverlap(adc_path_points[high], vehicle_param(), obs_box, - st_boundary_config().boundary_buffer())) { - --high; - } else { - find_high = true; - } + for (int j = 0; j < trajectory.trajectory_point_size(); ++j) { + const auto& trajectory_point = trajectory.trajectory_point(j); + // TODO(all): fix trajectory point relative time issue. + double trajectory_point_time = trajectory_point.relative_time(); + const Box2d obs_box = obstacle.GetBoundingBox(trajectory_point); + int64_t low = 0; + int64_t high = adc_path_points.size() - 1; + bool find_low = false; + bool find_high = false; + while (low < high) { + if (find_low && find_high) { + break; + } + if (!find_low) { + if (!CheckOverlap(adc_path_points[low], vehicle_param(), obs_box, + st_boundary_config().boundary_buffer())) { + ++low; + } else { + find_low = true; } } - if (find_high && find_low) { - lower_points.emplace_back( - adc_path_points[low].s() - st_boundary_config().point_extension(), - trajectory_point_time); - upper_points.emplace_back( - adc_path_points[high].s() + st_boundary_config().point_extension(), - trajectory_point_time); - } else { - if (obj_decision.has_yield() || obj_decision.has_overtake()) { - AINFO << "Point[" << j << "] cannot find low or high index."; + if (!find_high) { + if (!CheckOverlap(adc_path_points[high], vehicle_param(), obs_box, + st_boundary_config().boundary_buffer())) { + --high; + } else { + find_high = true; } } + } + if (find_high && find_low) { + lower_points.emplace_back( + adc_path_points[low].s() - st_boundary_config().point_extension(), + trajectory_point_time); + upper_points.emplace_back( + adc_path_points[high].s() + st_boundary_config().point_extension(), + trajectory_point_time); + } else { + if (obj_decision.has_yield() || obj_decision.has_overtake()) { + AINFO << "Point[" << j << "] cannot find low or high index."; + } + } - if (lower_points.size() > 0) { - boundary_points.clear(); - const double buffer = st_boundary_config().follow_buffer(); - boundary_points.emplace_back(lower_points.at(0).s() - buffer, - lower_points.at(0).t()); - boundary_points.emplace_back(lower_points.back().s() - buffer, - lower_points.back().t()); - boundary_points.emplace_back(upper_points.back().s() + buffer + - st_boundary_config().boundary_buffer(), - upper_points.back().t()); - boundary_points.emplace_back(upper_points.at(0).s() + buffer, - upper_points.at(0).t()); - if (lower_points.at(0).t() > lower_points.back().t() || - upper_points.at(0).t() > upper_points.back().t()) { - AWARN << "lower/upper points are reversed."; - } + if (lower_points.size() > 0) { + boundary_points.clear(); + const double buffer = st_boundary_config().follow_buffer(); + boundary_points.emplace_back(lower_points.at(0).s() - buffer, + lower_points.at(0).t()); + boundary_points.emplace_back(lower_points.back().s() - buffer, + lower_points.back().t()); + boundary_points.emplace_back(upper_points.back().s() + buffer + + st_boundary_config().boundary_buffer(), + upper_points.back().t()); + boundary_points.emplace_back(upper_points.at(0).s() + buffer, + upper_points.at(0).t()); + if (lower_points.at(0).t() > lower_points.back().t() || + upper_points.at(0).t() > upper_points.back().t()) { + AWARN << "lower/upper points are reversed."; + } - // change boundary according to obj_decision. - StGraphBoundary::BoundaryType b_type = - StGraphBoundary::BoundaryType::UNKNOWN; - if (obj_decision.has_follow()) { - boundary_points.at(0).set_s(boundary_points.at(0).s() - - follow_distance); - boundary_points.at(1).set_s(boundary_points.at(1).s() - - follow_distance); - boundary_points.at(3).set_t(-1.0); - b_type = StGraphBoundary::BoundaryType::FOLLOW; - } else if (obj_decision.has_yield()) { - const double dis = std::fabs(obj_decision.yield().distance_s()); - // TODO(all): remove the arbitrary numbers in this part. - if (boundary_points.at(0).s() - dis < 0.0) { - boundary_points.at(0).set_s( - std::fmax(boundary_points.at(0).s() - 2.0, 0.0)); - } else { - boundary_points.at(0).set_s( - std::fmax(boundary_points.at(0).s() - dis, 0.0)); - } - if (boundary_points.at(1).s() - dis < 0.0) { - boundary_points.at(1).set_s( - std::fmax(boundary_points.at(0).s() - 4.0, 0.0)); - } else { - boundary_points.at(1).set_s( - std::fmax(boundary_points.at(0).s() - dis, 0.0)); - } - b_type = StGraphBoundary::BoundaryType::YIELD; - } else if (obj_decision.has_overtake()) { - const double dis = std::fabs(obj_decision.overtake().distance_s()); - boundary_points.at(2).set_s(boundary_points.at(2).s() + dis); - boundary_points.at(3).set_s(boundary_points.at(3).s() + dis); + // change boundary according to obj_decision. + StGraphBoundary::BoundaryType b_type = + StGraphBoundary::BoundaryType::UNKNOWN; + if (obj_decision.has_follow()) { + boundary_points.at(0).set_s(boundary_points.at(0).s() - + follow_distance); + boundary_points.at(1).set_s(boundary_points.at(1).s() - + follow_distance); + boundary_points.at(3).set_t(-1.0); + b_type = StGraphBoundary::BoundaryType::FOLLOW; + } else if (obj_decision.has_yield()) { + const double dis = std::fabs(obj_decision.yield().distance_s()); + // TODO(all): remove the arbitrary numbers in this part. + if (boundary_points.at(0).s() - dis < 0.0) { + boundary_points.at(0).set_s( + std::fmax(boundary_points.at(0).s() - 2.0, 0.0)); + } else { + boundary_points.at(0).set_s( + std::fmax(boundary_points.at(0).s() - dis, 0.0)); } - - const double area = GetArea(boundary_points); - if (Double::compare(area, 0.0) > 0) { - boundary->emplace_back(boundary_points); - boundary->back().set_boundary_type(b_type); - skip = false; + if (boundary_points.at(1).s() - dis < 0.0) { + boundary_points.at(1).set_s( + std::fmax(boundary_points.at(0).s() - 4.0, 0.0)); + } else { + boundary_points.at(1).set_s( + std::fmax(boundary_points.at(0).s() - dis, 0.0)); } + b_type = StGraphBoundary::BoundaryType::YIELD; + } else if (obj_decision.has_overtake()) { + const double dis = std::fabs(obj_decision.overtake().distance_s()); + boundary_points.at(2).set_s(boundary_points.at(2).s() + dis); + boundary_points.at(3).set_s(boundary_points.at(3).s() + dis); + } + + const double area = GetArea(boundary_points); + if (Double::compare(area, 0.0) > 0) { + boundary->emplace_back(boundary_points); + boundary->back().set_boundary_type(b_type); + skip = false; } } } @@ -381,22 +375,18 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory( return Status(ErrorCode::PLANNING_ERROR, msg); } - const double speed = obstacle.Speed(); - const double heading = obstacle.Heading(); - const Vec2d center = obstacle.Center(); - const Box2d box(center, heading, - obstacle.Length() * st_boundary_config().expending_coeff(), - obstacle.Width() * st_boundary_config().expending_coeff()); + const auto& speed = obstacle.Perception().velocity(); + const double scalar_speed = std::hypot(speed.x(), speed.y()); - const PathPoint ref_point = - reference_line.get_reference_point(center.x(), center.y()); - const double speed_coeff = std::cos(heading - ref_point.theta()); + const auto& perception = obstacle.Perception(); + const PathPoint ref_point = reference_line.get_reference_point( + perception.position().x(), perception.position().y()); + const double speed_coeff = std::cos(perception.theta() - ref_point.theta()); if (speed_coeff < 0.0) { - std::string msg = common::util::StrCat( - "Obstacle is moving opposite to the reference line. Obstacle heading: ", - heading, "; ref_point.theta(): ", ref_point.theta()); - AERROR << msg; - return Status(ErrorCode::PLANNING_ERROR, msg); + AERROR << "Obstacle is moving opposite to the reference line. Obstacle: " + << perception.DebugString(); + return common::Status(ErrorCode::PLANNING_ERROR, + "obstacle is moving opposite the reference line"); } const auto& point = path_data.discretized_path().points()[0]; @@ -414,10 +404,10 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory( } double follow_speed = 0.0; - if (speed > st_boundary_config().follow_speed_threshold()) { + if (scalar_speed > st_boundary_config().follow_speed_threshold()) { follow_speed = st_boundary_config().follow_speed_threshold() * speed_coeff; } else { - follow_speed = speed * speed_coeff * + follow_speed = scalar_speed * speed_coeff * st_boundary_config().follow_speed_damping_factor(); } @@ -443,9 +433,9 @@ Status StBoundaryMapperImpl::MapObstacleWithoutPredictionTrajectory( boundary->emplace_back(boundary_points); const double characteristic_length = - std::fmax( - speed * speed_coeff * st_boundary_config().minimal_follow_time(), - std::fabs(obj_decision.follow().distance_s())) + + std::fmax(scalar_speed * speed_coeff * + st_boundary_config().minimal_follow_time(), + std::fabs(obj_decision.follow().distance_s())) + VehicleConfigHelper::GetConfig().vehicle_param().front_edge_to_center() + st_boundary_config().follow_buffer(); -- GitLab