Planning: more for planning module (#44)

apollo.sh

@@ -104,6 +104,7 @@ function generate_build_targets() {
     //modules/localization/proto/...
     //modules/monitor/...
     //modules/perception/proto/...
+    //modules/planning:planning_component_lib
     //modules/planning/proto/...
     "
   else
@@ -121,6 +122,7 @@ function generate_build_targets() {
       //modules/localization/proto/...
       //modules/monitor/...
       //modules/perception/proto/...
+      //modules/planning:planning_component_lib
       //modules/planning/proto/...
     "
   fi

modules/planning/BUILD

@@ -5,18 +5,16 @@ package(default_visibility = ["//visibility:public"])
 cc_library(
     name = "planning_component_lib",
     srcs = [
-        #"planning_base.cc",
         "planning_component.cc",
     ],
     hdrs = [
-        #"planning_base.h",
         "planning_component.h",
-        #"std_planning.h",
     ],
     copts = ["-DMODULE_NAME=\\\"planning\\\""],
     deps = [
+        ":planning_lib",
         "//framework:cybertron",
-        "//modules/common/util:thread_pool",
+        "//modules/common/util:message_util",
         "//modules/localization/proto:localization_proto",
         "//modules/perception/proto:perception_proto",
         "//modules/planning/proto:planning_proto",
@@ -27,41 +25,25 @@ cc_library(
 cc_library(
     name = "planning_lib",
     srcs = [
-        "navi_planning.cc",
         "planning_base.cc",
-        "std_planning.cc",
     ],
     hdrs = [
-        "navi_planning.h",
-        "planning.h",
         "planning_base.h",
-        "std_planning.h",
     ],
     deps = [
         "//modules/common",
-        "//modules/common:apollo_app",
-        "//modules/common/adapters:adapter_manager",
         "//modules/common/configs:config_gflags",
         "//modules/common/math:quaternion",
         "//modules/common/proto:pnc_point_proto",
-        "//modules/common/util:thread_pool",
-        "//modules/common/vehicle_state:vehicle_state_provider",
+        "//modules/common/util:message_util",
+        "//modules/localization/proto:localization_proto",
         "//modules/map/hdmap:hdmap_util",
         "//modules/perception/proto:perception_proto",
-        "//modules/planning/common:planning_common",
-        "//modules/planning/common/trajectory:trajectory_stitcher",
-        "//modules/planning/planner:planner_dispatcher",
-        "//modules/planning/planner/em:em_planner",
-        "//modules/planning/planner/lattice:lattice_planner",
-        "//modules/planning/planner/navi:navi_planner",
-        "//modules/planning/planner/open_space:open_space_planner",
-        "//modules/planning/planner/rtk:rtk_planner",
+        "//modules/planning/common:planning_gflags",
+        "//modules/planning/proto:planning_config_proto",
         "//modules/planning/proto:planning_proto",
         "//modules/planning/proto:traffic_rule_config_proto",
-        "//modules/planning/reference_line:reference_line_provider",
-        "//modules/planning/toolkits/deciders",
         "//modules/prediction/proto:prediction_proto",
-        "@ros//:ros_common",
     ],
 )
 

modules/planning/common/frame.cc

@@ -86,7 +86,7 @@ bool Frame::Rerouting() {
     AERROR << "Rerouting not supported in navigation mode";
     return false;
   }
-  auto *adapter_manager = AdapterManager::Instance();
+  auto adapter_manager = AdapterManager::Instance();
   if (adapter_manager->GetRoutingResponse()->Empty()) {
     AERROR << "No previous routing available";
     return false;

modules/planning/planning_base.cc

@@ -20,70 +20,34 @@
 #include <list>
 #include <vector>
 
-#include "google/protobuf/repeated_field.h"
-
-#include "modules/common/adapters/adapter_manager.h"
-#include "modules/common/math/quaternion.h"
 #include "modules/common/time/time.h"
-#include "modules/common/vehicle_state/vehicle_state_provider.h"
 #include "modules/map/hdmap/hdmap_util.h"
-#include "modules/planning/common/planning_context.h"
 #include "modules/planning/common/planning_gflags.h"
-#include "modules/planning/common/trajectory/trajectory_stitcher.h"
-#include "modules/planning/planner/em/em_planner.h"
-#include "modules/planning/planner/lattice/lattice_planner.h"
-#include "modules/planning/planner/navi/navi_planner.h"
-#include "modules/planning/planner/open_space/open_space_planner.h"
-#include "modules/planning/planner/rtk/rtk_replay_planner.h"
-#include "modules/planning/toolkits/deciders/traffic_decider.h"
 
 namespace apollo {
 namespace planning {
 
-using apollo::common::ErrorCode;
-using apollo::common::Status;
 using apollo::common::TrajectoryPoint;
-using apollo::common::VehicleState;
-using apollo::common::VehicleStateProvider;
-using apollo::common::adapter::AdapterManager;
 using apollo::common::time::Clock;
 using apollo::hdmap::HDMapUtil;
 
 PlanningBase::~PlanningBase() {}
 
-bool PlanningBase::IsVehicleStateValid(const VehicleState& vehicle_state) {
-  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
-      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
-      std::isnan(vehicle_state.kappa()) ||
-      std::isnan(vehicle_state.linear_velocity()) ||
-      std::isnan(vehicle_state.linear_acceleration())) {
-    return false;
-  }
-  return true;
-}
-
-void PlanningBase::PublishPlanningPb(ADCTrajectory* trajectory_pb,
-                                     double timestamp) {
+void PlanningBase::PublishPlanningPb(const double timestamp,
+                                     ADCTrajectory* trajectory_pb) {
   trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
-  if (AdapterManager::GetPrediction() &&
-      !AdapterManager::GetPrediction()->Empty()) {
-    const auto& prediction =
-        AdapterManager::GetPrediction()->GetLatestObserved();
+  if (prediction_obstacles_ != nullptr &&
+      !prediction_obstacles_->has_header()) {
     trajectory_pb->mutable_header()->set_lidar_timestamp(
-        prediction.header().lidar_timestamp());
+        prediction_obstacles_->header().lidar_timestamp());
     trajectory_pb->mutable_header()->set_camera_timestamp(
-        prediction.header().camera_timestamp());
+        prediction_obstacles_->header().camera_timestamp());
     trajectory_pb->mutable_header()->set_radar_timestamp(
-        prediction.header().radar_timestamp());
+        prediction_obstacles_->header().radar_timestamp());
   }
 
   // TODO(all): integrate reverse gear
   trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
-  if (AdapterManager::GetRoutingResponse() &&
-      !AdapterManager::GetRoutingResponse()->Empty()) {
-    trajectory_pb->mutable_routing_header()->CopyFrom(
-        AdapterManager::GetRoutingResponse()->GetLatestObserved().header());
-  }
 
   if (FLAGS_use_planning_fallback &&
       trajectory_pb->trajectory_point_size() == 0) {
@@ -102,24 +66,20 @@ void PlanningBase::PublishPlanningPb(ADCTrajectory* trajectory_pb,
       p.set_relative_time(p.relative_time() + dt);
     }
   }
-  Publish(trajectory_pb);
 }
 
 void PlanningBase::SetFallbackTrajectory(ADCTrajectory* trajectory_pb) {
   CHECK_NOTNULL(trajectory_pb);
   // use planning trajecotry from last cycle
-  auto* last_planning = AdapterManager::GetPlanning();
-  if (last_planning != nullptr && !last_planning->Empty()) {
-    const auto& traj = last_planning->GetLatestObserved();
-
+  if (last_planning_ != nullptr) {
     const double current_time_stamp = trajectory_pb->header().timestamp_sec();
-    const double pre_time_stamp = traj.header().timestamp_sec();
+    const double pre_time_stamp = last_planning_->header().timestamp_sec();
 
-    for (int i = 0; i < traj.trajectory_point_size(); ++i) {
-      const double t = traj.trajectory_point(i).relative_time() +
+    for (int i = 0; i < last_planning_->trajectory_point_size(); ++i) {
+      const double t = last_planning_->trajectory_point(i).relative_time() +
                        pre_time_stamp - current_time_stamp;
       auto* p = trajectory_pb->add_trajectory_point();
-      p->CopyFrom(traj.trajectory_point(i));
+      p->CopyFrom(last_planning_->trajectory_point(i));
       p->set_relative_time(t);
     }
   }

modules/planning/planning_base.h

@@ -22,20 +22,22 @@
 #include <utility>
 #include <vector>
 
-#include "ctpl/ctpl_stl.h"
-
+#include "modules/canbus/proto/chassis.pb.h"
 #include "modules/common/proto/pnc_point.pb.h"
+#include "modules/localization/proto/localization.pb.h"
+#include "modules/perception/proto/traffic_light_detection.pb.h"
 #include "modules/planning/proto/planning.pb.h"
 #include "modules/planning/proto/planning_config.pb.h"
 #include "modules/planning/proto/traffic_rule_config.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+#include "modules/routing/proto/routing.pb.h"
 
-#include "modules/common/adapters/adapter_manager.h"
-#include "modules/common/apollo_app.h"
 #include "modules/common/status/status.h"
-#include "modules/common/vehicle_state/vehicle_state_provider.h"
-#include "modules/planning/common/trajectory/publishable_trajectory.h"
-#include "modules/planning/planner/planner.h"
-#include "modules/planning/planner/planner_dispatcher.h"
+#include "modules/map/hdmap/hdmap.h"
+//#include "modules/common/vehicle_state/vehicle_state_provider.h"
+//#include "modules/planning/common/trajectory/publishable_trajectory.h"
+//#include "modules/planning/planner/planner.h"
+//#include "modules/planning/planner/planner_dispatcher.h"
 
 /**
  * @namespace apollo::planning
@@ -49,15 +51,19 @@ namespace planning {
  *
  * @brief PlanningBase module main class.
  */
-class PlanningBase : public apollo::common::ApolloApp {
+class PlanningBase {
  public:
   PlanningBase() = default;
   virtual ~PlanningBase();
 
-  virtual void RunOnce() = 0;
-
-  // Watch dog timer
-  virtual void OnTimer(const ros::TimerEvent&) = 0;
+  virtual void RunOnce(
+      const std::shared_ptr<prediction::PredictionObstacles>&
+          prediction_obstacles,
+      const std::shared_ptr<canbus::Chassis>& chassis,
+      const std::shared_ptr<localization::LocalizationEstimate>&
+          localization_estimate,
+      const perception::TrafficLightDetection,
+      const routing::RoutingResponse& routing) = 0;
 
   /**
    * @brief Plan the trajectory given current vehicle state
@@ -68,30 +74,15 @@ class PlanningBase : public apollo::common::ApolloApp {
       ADCTrajectory* trajectory) = 0;
 
  protected:
-  void PublishPlanningPb(ADCTrajectory* trajectory_pb, double timestamp);
+  void PublishPlanningPb(const double timestamp, ADCTrajectory* trajectory_pb);
+  void SetFallbackTrajectory(ADCTrajectory* trajectory_pb);
 
-  /**
-   * @brief Fill the header and publish the planning message.
-   */
-  void Publish(planning::ADCTrajectory* trajectory) {
-    using apollo::common::adapter::AdapterManager;
-    AdapterManager::FillPlanningHeader(Name(), trajectory);
-    AdapterManager::PublishPlanning(*trajectory);
-  }
-
-  bool IsVehicleStateValid(const common::VehicleState& vehicle_state);
-  virtual void SetFallbackTrajectory(ADCTrajectory* cruise_trajectory);
-  virtual bool CheckPlanningConfig() = 0;
-
- protected:
-  double start_time_ = 0.0;
-  PlanningConfig config_;
-  TrafficRuleConfigs traffic_rule_configs_;
   const hdmap::HDMap* hdmap_ = nullptr;
-  std::unique_ptr<Planner> planner_;
-  std::unique_ptr<PublishableTrajectory> last_publishable_trajectory_;
-  ros::Timer timer_;
-  std::unique_ptr<PlannerDispatcher> planner_dispatcher_;
+  const std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles_;
+  const std::shared_ptr<canbus::Chassis> chassis_;
+  const std::shared_ptr<localization::LocalizationEstimate>
+      localization_estimate_;
+  const std::shared_ptr<ADCTrajectory> last_planning_;
 };
 
 }  // namespace planning

modules/planning/planning_component.cc

@@ -15,6 +15,8 @@
  *****************************************************************************/
 #include "modules/planning/planning_component.h"
 
+#include "modules/common/util/message_util.h"
+
 namespace apollo {
 namespace planning {
 
@@ -50,17 +52,21 @@ bool PlanningComponent::Proc(
     const std::shared_ptr<canbus::Chassis>& chassis,
     const std::shared_ptr<localization::LocalizationEstimate>&
         localization_estimate) {
+  routing::RoutingResponse routing_copy;
   {
     std::lock_guard<std::mutex> lock(mutex_);
-    // planning_base_->SetRouting(routing);
+    routing_copy = routing_;
   }
+  perception::TrafficLightDetection traffic_light_copy;
   {
     std::lock_guard<std::mutex> lock(mutex_);
-    // planning_base_->SetTrafficLight(traffic_light_);
+    traffic_light_copy = traffic_light_;
   }
 
   ADCTrajectory adc_trajectory_pb;
   // TODO(Liangliang) implement here
+
+  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
   writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
   return true;
 }

modules/planning/toolkits/deciders/rerouting.cc

@@ -74,7 +74,7 @@ bool Rerouting::ChangeLaneFailRerouting() {
   // 5. If the end of current passage region is further than kPrepareRoutingTime
   // * speed, no rerouting
   double adc_s = reference_line_info_->AdcSlBoundary().end_s();
-  const auto* vehicle_state = common::VehicleStateProvider::Instance();
+  const auto vehicle_state = common::VehicleStateProvider::Instance();
   double speed = vehicle_state->linear_velocity();
   const double prepare_rerouting_time =
       config_.rerouting().prepare_rerouting_time();