From 15d0408e6e3bb2a6e4e4763804fbd5290cab6ca2 Mon Sep 17 00:00:00 2001 From: Jiangtao Hu Date: Thu, 20 Jul 2017 15:50:24 -0700 Subject: [PATCH] Add EMPlanningData. (#100) --- modules/common/adapters/adapter.h | 4 + modules/common/adapters/adapter_manager.cc | 4 +- modules/common/adapters/adapter_manager.h | 2 +- modules/common/adapters/message_adapters.h | 2 +- modules/common/log.h | 10 ++ modules/control/control.cc | 6 +- .../integration_tests/control_test_base.cc | 4 +- modules/control/tools/control_tester.cc | 2 +- .../backend/simulation_world_service.cc | 6 +- .../backend/simulation_world_service.h | 2 +- modules/map/pnc_map/path.cc | 2 +- modules/map/pnc_map/path.h | 2 +- modules/planning/BUILD | 1 + modules/planning/common/BUILD | 19 +++- modules/planning/common/em_planning_data.cc | 99 +++++++++++++++++++ modules/planning/common/em_planning_data.h | 57 +++++++++++ modules/planning/common/planning_data.h | 3 +- modules/planning/optimizer/dp_poly_path/BUILD | 2 +- modules/planning/planner/em/em_planner.cc | 19 +++- modules/planning/planning.cc | 9 +- modules/planning/reference_line/BUILD | 2 +- 21 files changed, 230 insertions(+), 27 deletions(-) create mode 100644 modules/planning/common/em_planning_data.cc create mode 100644 modules/planning/common/em_planning_data.h diff --git a/modules/common/adapters/adapter.h b/modules/common/adapters/adapter.h index 39ad26b0d8..65ae4b1ded 100644 --- a/modules/common/adapters/adapter.h +++ b/modules/common/adapters/adapter.h @@ -232,6 +232,10 @@ class Adapter { header->set_sequence_num(++seq_num_); } + uint32_t GetSeqNum() const { + return seq_num_; + } + private: // HasSequenceNumber returns false for non-proto-message data types. template diff --git a/modules/common/adapters/adapter_manager.cc b/modules/common/adapters/adapter_manager.cc index 7e93ed4978..572d055aee 100644 --- a/modules/common/adapters/adapter_manager.cc +++ b/modules/common/adapters/adapter_manager.cc @@ -84,8 +84,8 @@ void AdapterManager::Init(const AdapterManagerConfig &configs) { config.message_history_limit()); break; case AdapterConfig::PLANNING_TRAJECTORY: - EnablePlanningTrajectory(FLAGS_planning_trajectory_topic, config.mode(), - config.message_history_limit()); + EnablePlanning(FLAGS_planning_trajectory_topic, config.mode(), + config.message_history_limit()); break; case AdapterConfig::PREDICTION: EnablePrediction(FLAGS_prediction_topic, config.mode(), diff --git a/modules/common/adapters/adapter_manager.h b/modules/common/adapters/adapter_manager.h index 10052bb0b1..9546e12ed3 100644 --- a/modules/common/adapters/adapter_manager.h +++ b/modules/common/adapters/adapter_manager.h @@ -187,7 +187,7 @@ class AdapterManager { REGISTER_ADAPTER(Monitor); REGISTER_ADAPTER(Pad); REGISTER_ADAPTER(PerceptionObstacles); - REGISTER_ADAPTER(PlanningTrajectory); + REGISTER_ADAPTER(Planning); REGISTER_ADAPTER(Prediction); REGISTER_ADAPTER(TrafficLightDetection); diff --git a/modules/common/adapters/message_adapters.h b/modules/common/adapters/message_adapters.h index 6f92c05203..4012580285 100644 --- a/modules/common/adapters/message_adapters.h +++ b/modules/common/adapters/message_adapters.h @@ -55,7 +55,7 @@ using MonitorAdapter = Adapter; using PadAdapter = Adapter<::apollo::control::PadMessage>; using PerceptionObstaclesAdapter = Adapter<::apollo::perception::PerceptionObstacles>; -using PlanningTrajectoryAdapter = Adapter<::apollo::planning::ADCTrajectory>; +using PlanningAdapter = Adapter<::apollo::planning::ADCTrajectory>; using PredictionAdapter = Adapter<::apollo::prediction::PredictionObstacles>; using TrafficLightDetectionAdapter = Adapter<::apollo::perception::TrafficLightDetection>; diff --git a/modules/common/log.h b/modules/common/log.h index 6ec67c4a04..e8de039bcf 100644 --- a/modules/common/log.h +++ b/modules/common/log.h @@ -22,6 +22,7 @@ #define MODULES_COMMON_LOG_H_ #include "glog/logging.h" +#include "glog/raw_logging.h" #define ADEBUG VLOG(4) << "[DEBUG] " #define AINFO VLOG(3) << "[INFO] " @@ -29,4 +30,13 @@ #define AERROR LOG(ERROR) #define AFATAL LOG(FATAL) +// quit if condition is met +#define QUIT_IF(CONDITION, RET, LEVEL, MSG, ...) \ +do { \ + if (CONDITION) { \ + RAW_LOG(LEVEL, MSG, ##__VA_ARGS__); \ + return RET; \ + } \ +} while (0); + #endif // MODULES_COMMON_LOG_H_ diff --git a/modules/control/control.cc b/modules/control/control.cc index f5aa269793..a669d3442d 100644 --- a/modules/control/control.cc +++ b/modules/control/control.cc @@ -75,8 +75,8 @@ Status Control::Init() { CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized."; - CHECK(AdapterManager::GetPlanningTrajectory()) - << "PlanningTrajectory is not initialized."; + CHECK(AdapterManager::GetPlanning()) + << "Planning is not initialized."; CHECK(AdapterManager::GetPad()) << "Pad is not initialized."; @@ -244,7 +244,7 @@ Status Control::CheckInput() { chassis_ = chassis_adapter->GetLatestObserved(); ADEBUG << "Received chassis:" << chassis_.ShortDebugString(); - auto trajectory_adapter = AdapterManager::GetPlanningTrajectory(); + auto trajectory_adapter = AdapterManager::GetPlanning(); if (trajectory_adapter->Empty()) { AINFO << "No planning msg yet. "; return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "No planning msg"); diff --git a/modules/control/integration_tests/control_test_base.cc b/modules/control/integration_tests/control_test_base.cc index c3781c2d60..01f45061ef 100644 --- a/modules/control/integration_tests/control_test_base.cc +++ b/modules/control/integration_tests/control_test_base.cc @@ -76,8 +76,8 @@ bool ControlTestBase::test_control() { FLAGS_test_localization_file); } if (!FLAGS_test_planning_file.empty()) { - AdapterManager::FeedPlanningTrajectoryProtoFile(FLAGS_test_data_dir + - FLAGS_test_planning_file); + AdapterManager::FeedPlanningProtoFile(FLAGS_test_data_dir + + FLAGS_test_planning_file); } if (!FLAGS_test_chassis_file.empty()) { AdapterManager::FeedChassisProtoFile(FLAGS_test_data_dir + diff --git a/modules/control/tools/control_tester.cc b/modules/control/tools/control_tester.cc index 25af6ff6a1..f3e149e29e 100644 --- a/modules/control/tools/control_tester.cc +++ b/modules/control/tools/control_tester.cc @@ -67,7 +67,7 @@ int main(int argc, char **argv) { AdapterManager::FeedChassisProtoFile(FLAGS_chassis_test_file); AdapterManager::FeedLocalizationProtoFile(FLAGS_l10n_test_file); AdapterManager::FeedPadProtoFile(FLAGS_pad_msg_test_file); - AdapterManager::FeedPlanningTrajectoryProtoFile(FLAGS_planning_test_file); + AdapterManager::FeedPlanningProtoFile(FLAGS_planning_test_file); sleep_for(std::chrono::milliseconds(1000 / FLAGS_feed_frequency)); } AINFO << "Successfully fed proto files."; diff --git a/modules/dreamview/backend/simulation_world_service.cc b/modules/dreamview/backend/simulation_world_service.cc index 0ac61c429f..634c5e5563 100644 --- a/modules/dreamview/backend/simulation_world_service.cc +++ b/modules/dreamview/backend/simulation_world_service.cc @@ -37,7 +37,7 @@ using apollo::common::adapter::AdapterManager; using apollo::common::adapter::MonitorAdapter; using apollo::common::adapter::LocalizationAdapter; using apollo::common::adapter::ChassisAdapter; -using apollo::common::adapter::PlanningTrajectoryAdapter; +using apollo::common::adapter::PlanningAdapter; using apollo::common::config::VehicleConfigHelper; using apollo::common::monitor::MonitorMessage; using apollo::common::monitor::MonitorMessageItem; @@ -252,7 +252,7 @@ void UpdateSimulationWorld(const Chassis &chassis, } template <> -void UpdateSimulationWorld( +void UpdateSimulationWorld( const ADCTrajectory &trajectory, SimulationWorld *world) { const double cutoff_time = world->auto_driving_car().timestamp_sec(); const double header_time = trajectory.header().timestamp_sec(); @@ -329,7 +329,7 @@ const SimulationWorld &SimulationWorldService::Update() { UpdateWithLatestObserved("Chassis", AdapterManager::GetChassis(), &world_); UpdateWithLatestObserved("Localization", AdapterManager::GetLocalization(), &world_); - UpdateWithLatestObserved("Planning", AdapterManager::GetPlanningTrajectory(), + UpdateWithLatestObserved("Planning", AdapterManager::GetPlanning(), &world_); UpdateWithLatestObserved("PerceptionObstacles", AdapterManager::GetPerceptionObstacles(), &world_); diff --git a/modules/dreamview/backend/simulation_world_service.h b/modules/dreamview/backend/simulation_world_service.h index 3da625bc57..261660b7b1 100644 --- a/modules/dreamview/backend/simulation_world_service.h +++ b/modules/dreamview/backend/simulation_world_service.h @@ -60,7 +60,7 @@ void UpdateSimulationWorld( const apollo::canbus::Chassis &chassis, SimulationWorld *world); template <> -void UpdateSimulationWorld( +void UpdateSimulationWorld( const apollo::planning::ADCTrajectory &trajectory, SimulationWorld *world); template<> diff --git a/modules/map/pnc_map/path.cc b/modules/map/pnc_map/path.cc index 6c2ea33259..e476cc3e91 100644 --- a/modules/map/pnc_map/path.cc +++ b/modules/map/pnc_map/path.cc @@ -155,7 +155,7 @@ void Path::init() { init_lane_segments(); init_point_index(); init_width(); - init_overlaps(); + // init_overlaps(); } void Path::init_points() { diff --git a/modules/map/pnc_map/path.h b/modules/map/pnc_map/path.h index 1c6cc0d7bd..35b89772c6 100644 --- a/modules/map/pnc_map/path.h +++ b/modules/map/pnc_map/path.h @@ -284,7 +284,7 @@ class Path { void init_lane_segments(); void init_width(); void init_point_index(); - void init_overlaps(); + // void init_overlaps(); double get_sample(const std::vector& samples, const double s) const; diff --git a/modules/planning/BUILD b/modules/planning/BUILD index 8b1ce20eb4..64d083ef7b 100644 --- a/modules/planning/BUILD +++ b/modules/planning/BUILD @@ -19,6 +19,7 @@ cc_library( "//modules/common/vehicle_state", "//modules/decision/proto:decision_proto", "//modules/perception/proto:perception_proto", + "//modules/planning/common:data_center", "//modules/planning/common:planning_common", "//modules/planning/planner/em:em_planner", "//modules/planning/planner/rtk:rtk_planner", diff --git a/modules/planning/common/BUILD b/modules/planning/common/BUILD index 31e59d2ffc..835602db54 100644 --- a/modules/planning/common/BUILD +++ b/modules/planning/common/BUILD @@ -111,12 +111,29 @@ cc_library( ":decision_data", "//modules/common/proto:path_point_proto", "//modules/planning/common/trajectory:publishable_trajectory", - "//modules/planning/reference_line:lib_reference_line", + "//modules/planning/reference_line", "//modules/map/proto:map_proto", "@eigen//:eigen", ], ) +cc_library( + name = "em_planning_data", + srcs = [ + "em_planning_data.cc", + ], + hdrs = [ + "em_planning_data.h", + ], + deps = [ + ":planning_data", + "//modules/common/proto:path_point_proto", + "//modules/planning/common/path:path_data", + "//modules/planning/common/speed", + "//modules/planning/math:double", + ] +) + cc_library( name = "environment", srcs = [ diff --git a/modules/planning/common/em_planning_data.cc b/modules/planning/common/em_planning_data.cc new file mode 100644 index 0000000000..2341543d6d --- /dev/null +++ b/modules/planning/common/em_planning_data.cc @@ -0,0 +1,99 @@ +/****************************************************************************** + * 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/common/em_planning_data.h" +#include "glog/logging.h" + +#include "modules/common/proto/path_point.pb.h" +#include "modules/planning/math/double.h" + +namespace apollo { +namespace planning { + +std::string EMPlanningData::type() const { + return "EMPlanningData"; +} + +void EMPlanningData::init(const std::size_t num_iter) { + _num_iter = num_iter; + _path_data_vec = std::vector < PathData > (num_iter, PathData()); + _speed_data_vec = std::vector < SpeedData > (num_iter + 1, SpeedData()); +} + +std::size_t EMPlanningData::num_iter() const { + return _num_iter; +} + +const PathData& EMPlanningData::path_data(const std::size_t index) const { + return _path_data_vec[index]; +} + +const SpeedData& EMPlanningData::speed_data(const std::size_t index) const { + return _speed_data_vec[index]; +} + +PathData* EMPlanningData::mutable_path_data(const std::size_t index) { + if (index >= _path_data_vec.size()) { + return nullptr; + } + return &_path_data_vec[index]; +} + +SpeedData* EMPlanningData::mutable_speed_data(const std::size_t index) { + if (index >= _speed_data_vec.size()) { + return nullptr; + } + return &_speed_data_vec[index]; +} + +bool EMPlanningData::aggregate(const double time_resolution) { + CHECK(time_resolution > 0.0); + CHECK(_computed_trajectory.num_of_points() == 0); + + const SpeedData& speed_data = _speed_data_vec.back(); + const PathData& path_data = _path_data_vec.back(); + for (double cur_rel_time = 0.0; cur_rel_time < speed_data.total_time(); + cur_rel_time += time_resolution) { + + SpeedPoint speed_point; + QUIT_IF(!speed_data.get_speed_point_with_time(cur_rel_time, &speed_point), false, + ERROR, "Fail to get speed point with relative time %f", cur_rel_time); + + apollo::common::PathPoint path_point; + // TODO temp fix speed point s out of path point bound, need further refine later + if (speed_point.s() > path_data.path().param_length()) { + break; + } + QUIT_IF(!path_data.get_path_point_with_path_s(speed_point.s(), &path_point), false, + ERROR, "Fail to get path data with s %f, path total length %f", + speed_point.s(), path_data.path().param_length()); + + apollo::common::TrajectoryPoint trajectory_point; + trajectory_point.mutable_path_point()->CopyFrom(path_point); + trajectory_point.set_v(speed_point.v()); + trajectory_point.set_a(speed_point.a()); + trajectory_point.set_relative_time( + _init_planning_point.relative_time() + speed_point.t()); + _computed_trajectory.add_trajectory_point(trajectory_point); + } + return true; +} +} // namespace planning +} // namespace apollo diff --git a/modules/planning/common/em_planning_data.h b/modules/planning/common/em_planning_data.h new file mode 100644 index 0000000000..7f17fbf418 --- /dev/null +++ b/modules/planning/common/em_planning_data.h @@ -0,0 +1,57 @@ +/****************************************************************************** + * 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_COMMON_EM_PLANNING_DATA_H +#define MODULES_PLANNING_COMMON_EM_PLANNING_DATA_H + +#include "modules/planning/common/planning_data.h" +#include "modules/planning/common/path/path_data.h" +#include "modules/planning/common/speed/speed_data.h" + +namespace apollo { +namespace planning { + +class EMPlanningData : public PlanningData { + public: + EMPlanningData() = default; + virtual std::string type() const; + + void init(const std::size_t num_iter); + + std::size_t num_iter() const; + const PathData& path_data(const std::size_t index) const; + const SpeedData& speed_data(const std::size_t index) const; + + PathData* mutable_path_data(const std::size_t index); + SpeedData* mutable_speed_data(const std::size_t index); + + // aggregate final result together by some configuration + bool aggregate(const double time_resolution); + + private: + std::size_t _num_iter = 0; + std::vector _path_data_vec; + std::vector _speed_data_vec; +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_COMMON_EM_PLANNING_DATA_H diff --git a/modules/planning/common/planning_data.h b/modules/planning/common/planning_data.h index ae24fe19dc..6bc14d6870 100644 --- a/modules/planning/common/planning_data.h +++ b/modules/planning/common/planning_data.h @@ -15,13 +15,14 @@ *****************************************************************************/ /** - * @file planning_data.h + * @file **/ #ifndef MODULES_PLANNING_COMMON_PLANNING_DATA_H_ #define MODULES_PLANNING_COMMON_PLANNING_DATA_H_ #include "modules/common/proto/path_point.pb.h" +#include "modules/planning/reference_line/reference_line.h" #include "modules/planning/common/decision_data.h" #include "modules/planning/common/trajectory/publishable_trajectory.h" #include "modules/planning/reference_line/reference_line.h" diff --git a/modules/planning/optimizer/dp_poly_path/BUILD b/modules/planning/optimizer/dp_poly_path/BUILD index 66af75dc9b..b050c6d14a 100644 --- a/modules/planning/optimizer/dp_poly_path/BUILD +++ b/modules/planning/optimizer/dp_poly_path/BUILD @@ -33,6 +33,6 @@ cc_library( "//modules/planning/math:sl_analytic_transformation", "//modules/planning/math/curve1d:polynomial_curve1d", "//modules/planning/math/curve1d:quintic_polynomial_curve1d", - "//modules/planning/reference_line:lib_reference_line", + "//modules/planning/reference_line", ], ) diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc index 42a6daba8b..018a211b29 100644 --- a/modules/planning/planner/em/em_planner.cc +++ b/modules/planning/planner/em/em_planner.cc @@ -22,6 +22,7 @@ #include "modules/common/log.h" #include "modules/common/util/string_tokenizer.h" #include "modules/planning/common/data_center.h" +#include "modules/planning/common/frame.h" #include "modules/planning/common/planning_gflags.h" #include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h" @@ -31,10 +32,20 @@ namespace planning { using apollo::common::TrajectoryPoint; using apollo::common::vehicle_state::VehicleState; -EMPlanner::EMPlanner() {} +EMPlanner::EMPlanner() { +} bool EMPlanner::MakePlan(const TrajectoryPoint& start_point, - std::vector* discretized_trajectory) { + std::vector* discretized_trajectory) { + DataCenter* data_center = DataCenter::instance(); + Frame* frame = data_center->current_frame(); + +// frame->set_planning_data(task->create_planning_data_instance()); +// frame->mutable_planning_data()->set_reference_line(reference_line); +// frame->mutable_planning_data()->set_decision_data(decision_data); +// frame->mutable_planning_data()->set_init_planning_point( +// frame->environment().vehicle_state_proxy().init_point(data_center->last_frame())); + return true; } @@ -65,8 +76,8 @@ std::vector EMPlanner::GenerateInitSpeedProfile( // assume the time resolution is 0.1 std::size_t num_time_steps = - static_cast(FLAGS_trajectory_time_length / - FLAGS_trajectory_time_resolution) + 1; + static_cast(FLAGS_trajectory_time_length + / FLAGS_trajectory_time_resolution) + 1; std::vector speed_profile; speed_profile.reserve(num_time_steps); diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index 9c3b99b1cb..f1e1f8994b 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -18,6 +18,7 @@ #include "modules/common/adapters/adapter_manager.h" #include "modules/common/time/time.h" +#include "modules/planning/common/data_center.h" #include "modules/planning/common/planning_gflags.h" #include "modules/planning/planner/em/em_planner.h" #include "modules/planning/planner/rtk/rtk_replay_planner.h" @@ -115,13 +116,15 @@ void Planning::RunOnce() { apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) + planning_cycle_time; + DataCenter::instance()->init_frame(AdapterManager::GetPlanning()->GetSeqNum() + 1); + std::vector planning_trajectory; bool res_planning = Plan(vehicle_state, is_on_auto_mode, execution_start_time, &planning_trajectory); if (res_planning) { ADCTrajectory trajectory_pb = ToADCTrajectory(execution_start_time, planning_trajectory); - AdapterManager::PublishPlanningTrajectory(trajectory_pb); + AdapterManager::PublishPlanning(trajectory_pb); AINFO << "Planning succeeded"; } else { AINFO << "Planning failed"; @@ -269,8 +272,8 @@ ADCTrajectory Planning::ToADCTrajectory( const double header_time, const std::vector& discretized_trajectory) { ADCTrajectory trajectory_pb; - AdapterManager::FillPlanningTrajectoryHeader("planning", - trajectory_pb.mutable_header()); + AdapterManager::FillPlanningHeader("planning", + trajectory_pb.mutable_header()); trajectory_pb.mutable_header()->set_timestamp_sec(header_time); diff --git a/modules/planning/reference_line/BUILD b/modules/planning/reference_line/BUILD index 043bc88d7b..e2a13ad7ea 100644 --- a/modules/planning/reference_line/BUILD +++ b/modules/planning/reference_line/BUILD @@ -1,7 +1,7 @@ package(default_visibility = ["//visibility:public"]) cc_library( - name = "lib_reference_line", + name = "reference_line", srcs = [ "reference_point.cc", "reference_line.cc", -- GitLab