Planning : update open space planner interface and use internal debug proto

modules/planning/open_space/open_space_trajectory_generator.cc

@@ -38,6 +38,7 @@ using apollo::common::TrajectoryPoint;
 using apollo::common::VehicleState;
 using apollo::common::math::Box2d;
 using apollo::common::math::Vec2d;
+using apollo::planning_internal::Trajectories;
 
 Status OpenSpaceTrajectoryGenerator::Init(
     const PlannerOpenSpaceConfig& planner_open_space_config) {
@@ -212,40 +213,45 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
   // Every time update, use trajectory_partition to store each ADCTrajectory
   double relative_time = 0.0;
   double distance_s = 0.0;
-  ADCTrajectories trajectory_partition;
-  ADCTrajectory* current_trajectory = trajectory_partition.add_adc_trajectory();
+
+  Trajectories trajectory_partition;
+  gear_positions_.clear();
+
+  ::apollo::common::Trajectory* current_trajectory =
+      trajectory_partition.add_trajectory();
   // set initial gear position for first ADCTrajectory depending on v
   // and check potential edge cases
   if (horizon_ < 3)
     return Status(ErrorCode::PLANNING_ERROR, "Invalid trajectory length!");
   if (state_result_ds(3, 0) > -1e-3 && state_result_ds(3, 1) > -1e-3 &&
       state_result_ds(3, 2) > -1e-3) {
-    current_trajectory->set_gear(canbus::Chassis::GEAR_DRIVE);
+    gear_positions_.push_back(canbus::Chassis::GEAR_DRIVE);
   } else {
     if (state_result_ds(3, 0) < 1e-3 && state_result_ds(3, 1) < 1e-3 &&
         state_result_ds(3, 2) < 1e-3) {
-      current_trajectory->set_gear(canbus::Chassis::GEAR_REVERSE);
+      gear_positions_.push_back(canbus::Chassis::GEAR_REVERSE);
     } else {
       return Status(ErrorCode::PLANNING_ERROR, "Invalid trajectory start!");
     }
   }
+
   // partition trajectory points into each trajectory
   for (std::size_t i = 0; i < horizon_ + 1; i++) {
     // shift from GEAR_DRIVE to GEAR_REVERSE if v < 0
     // then add a new trajectory with GEAR_REVERSE
     if (state_result_ds(3, i) < -1e-3 &&
-        current_trajectory->gear() == canbus::Chassis::GEAR_DRIVE) {
-      current_trajectory = trajectory_partition.add_adc_trajectory();
-      current_trajectory->set_gear(canbus::Chassis::GEAR_REVERSE);
+        gear_positions_.back() == canbus::Chassis::GEAR_DRIVE) {
+      current_trajectory = trajectory_partition.add_trajectory();
+      gear_positions_.push_back(canbus::Chassis::GEAR_REVERSE);
       distance_s = 0.0;
       relative_time = 0.0;
     }
     // shift from GEAR_REVERSE to GEAR_DRIVE if v > 0
     // then add a new trajectory with GEAR_DRIVE
     if (state_result_ds(3, i) > 1e-3 &&
-        current_trajectory->gear() == canbus::Chassis::GEAR_REVERSE) {
-      current_trajectory = trajectory_partition.add_adc_trajectory();
-      current_trajectory->set_gear(canbus::Chassis::GEAR_DRIVE);
+        gear_positions_.back() == canbus::Chassis::GEAR_REVERSE) {
+      current_trajectory = trajectory_partition.add_trajectory();
+      gear_positions_.push_back(canbus::Chassis::GEAR_DRIVE);
       distance_s = 0.0;
       relative_time = 0.0;
     }
@@ -266,7 +272,7 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
     }
     point->mutable_path_point()->set_s(distance_s);
     int gear_drive = 1;
-    if (current_trajectory->gear() == canbus::Chassis::GEAR_REVERSE)
+    if (gear_positions_.back() == canbus::Chassis::GEAR_REVERSE)
       gear_drive = -1;
 
     point->set_v(state_result_ds(3, i) * gear_drive);
@@ -282,7 +288,8 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
 }
 
 apollo::common::Status OpenSpaceTrajectoryGenerator::UpdateTrajectory(
-    ADCTrajectories* adc_trajectories) {
+    Trajectories* adc_trajectories,
+    std::vector<canbus::Chassis::GearPosition>* gear_positions) {
   adc_trajectories->CopyFrom(trajectory_partition_);
   return Status::OK();
 }

modules/planning/open_space/open_space_trajectory_generator.h

@@ -36,6 +36,7 @@
 #include "modules/planning/open_space/hybrid_a_star.h"
 #include "modules/planning/proto/planner_open_space_config.pb.h"
 #include "modules/planning/proto/planning_config.pb.h"
+#include "modules/planning/proto/planning_internal.pb.h"
 
 /*
 Initially inspired by "Optimization-Based Collision Avoidance" from Xiaojing
@@ -89,7 +90,8 @@ class OpenSpaceTrajectoryGenerator {
   void BuildPredictedEnvironment(const std::vector<const Obstacle*>& obstacles);
 
   apollo::common::Status UpdateTrajectory(
-      ADCTrajectories* trajectory_partition);
+      ::apollo::planning_internal::Trajectories* trajectory_partition,
+      std::vector<::apollo::canbus::Chassis::GearPosition>* gear_positions);
 
   void Stop();
 
@@ -114,7 +116,8 @@ class OpenSpaceTrajectoryGenerator {
   Eigen::MatrixXd ego_;
   std::vector<double> XYbounds_;
 
-  ADCTrajectories trajectory_partition_;
+  ::apollo::planning_internal::Trajectories trajectory_partition_;
+  std::vector<::apollo::canbus::Chassis::GearPosition> gear_positions_;
 };
 
 }  // namespace planning

modules/planning/planner/open_space/open_space_planner.cc

@@ -92,8 +92,10 @@ apollo::common::Status OpenSpacePlanner::Plan(
     // 3. Check if trajectory updated, if so, update internal
     // trajectory_partition_;
     if (trajectory_updated_) {
-      open_space_trajectory_generator_->UpdateTrajectory(
-          &trajectory_partition_);
+      trajectory_partition_.Clear();
+      gear_positions_.clear();
+      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
+                                                         &gear_positions_);
       AINFO << "Trajectory caculation updated, new results : "
             << trajectory_partition_.ShortDebugString();
     }
@@ -101,7 +103,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
     // If the vehicle each the end point of current trajectory and stop
     // Then move to the next Trajectory
     current_trajectory_ =
-        trajectory_partition_.adc_trajectory(current_trajectory_index_);
+        trajectory_partition_.trajectory(current_trajectory_index_);
 
     TrajectoryPoint end_point = current_trajectory_.trajectory_point(
         current_trajectory_.trajectory_point_size() - 1);
@@ -115,7 +117,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
         std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
             planner_open_space_config_.max_theta_error_to_end_point() &&
         (current_trajectory_index_ <
-         trajectory_partition_.adc_trajectory_size() - 1)) {
+         trajectory_partition_.trajectory_size() - 1)) {
       current_trajectory_index_ += 1;
     }
 
@@ -123,7 +125,8 @@ apollo::common::Status OpenSpacePlanner::Plan(
     // return error status
 
     if (IsCollisionFreeTrajectory(current_trajectory_)) {
-      frame->mutable_trajectory()->CopyFrom(current_trajectory_);
+      // Convet current trajectory to publishable_trajectory
+      //  frame->mutable_trajectory()->CopyFrom(current_trajectory_);
       return Status::OK();
     } else {
       // If collision happens, return wrong planning status and estop
@@ -156,8 +159,10 @@ apollo::common::Status OpenSpacePlanner::Plan(
 
     // 3. If status is OK, update vehicle trajectory;
     if (status == Status::OK()) {
-      open_space_trajectory_generator_->UpdateTrajectory(
-          &trajectory_partition_);
+      trajectory_partition_.Clear();
+      gear_positions_.clear();
+      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
+                                                         &gear_positions_);
       AINFO << "Trajectory caculation updated, new results : "
             << trajectory_partition_.ShortDebugString();
     } else {
@@ -166,7 +171,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
     }
 
     current_trajectory_ =
-        trajectory_partition_.adc_trajectory(current_trajectory_index_);
+        trajectory_partition_.trajectory(current_trajectory_index_);
 
     TrajectoryPoint end_point = current_trajectory_.trajectory_point(
         current_trajectory_.trajectory_point_size() - 1);
@@ -180,7 +185,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
         std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
             planner_open_space_config_.max_theta_error_to_end_point() &&
         (current_trajectory_index_ <
-         trajectory_partition_.adc_trajectory_size() - 1)) {
+         trajectory_partition_.trajectory_size() - 1)) {
       current_trajectory_index_ += 1;
     }
 
@@ -221,16 +226,16 @@ void OpenSpacePlanner::Stop() {
 }
 
 bool OpenSpacePlanner::IsCollisionFreeTrajectory(
-    const ADCTrajectory& adc_trajectory) {
-  CHECK_LE(adc_trajectory.trajectory_point().size(),
+    const apollo::common::Trajectory& trajectory) {
+  CHECK_LE(trajectory.trajectory_point().size(),
            predicted_bounding_rectangles_.size());
   const auto& vehicle_config =
       common::VehicleConfigHelper::Instance()->GetConfig();
   double ego_length = vehicle_config.vehicle_param().length();
   double ego_width = vehicle_config.vehicle_param().width();
 
-  for (int i = 0; i < adc_trajectory.trajectory_point().size(); ++i) {
-    const auto& trajectory_point = adc_trajectory.trajectory_point(i);
+  for (int i = 0; i < trajectory.trajectory_point().size(); ++i) {
+    const auto& trajectory_point = trajectory.trajectory_point(i);
     double ego_theta = trajectory_point.path_point().theta();
     Box2d ego_box(
         {trajectory_point.path_point().x(), trajectory_point.path_point().y()},

modules/planning/planner/open_space/open_space_planner.h

@@ -85,7 +85,7 @@ class OpenSpacePlanner : public Planner {
 
   void GenerateTrajectoryThread();
 
-  bool IsCollisionFreeTrajectory(const ADCTrajectory& adc_trajectory);
+  bool IsCollisionFreeTrajectory(const common::Trajectory& trajectory);
 
   void BuildPredictedEnvironment(const std::vector<const Obstacle*>& obstacles);
 
@@ -118,8 +118,9 @@ class OpenSpacePlanner : public Planner {
   std::mutex open_space_mutex_;
 
   int current_trajectory_index_;
-  ADCTrajectory current_trajectory_;
-  ADCTrajectories trajectory_partition_;
+  apollo::common::Trajectory current_trajectory_;
+  apollo::planning_internal::Trajectories trajectory_partition_;
+  std::vector<::apollo::canbus::Chassis::GearPosition> gear_positions_;
 
   std::vector<std::vector<common::math::Box2d>> predicted_bounding_rectangles_;