Planning: OpenSpace: add unscaled warm start and smoothed trajectory in chart

modules/planning/open_space/open_space_trajectory_generator.cc

@@ -202,8 +202,10 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
 
   // record debug info
   if (FLAGS_enable_record_debug) {
+    open_space_debug_.Clear();
     RecordDebugInfo(xWS, uWS, l_warm_up, n_warm_up, dual_l_result_ds,
-                    dual_n_result_ds, XYbounds_, obstalce_list);
+                    dual_n_result_ds, state_result_ds, control_result_ds,
+                    XYbounds_, obstalce_list);
   }
   // rescale the states to the world frame
   for (size_t i = 0; i < horizon_ + 1; i++) {
@@ -224,6 +226,11 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
   Status trajectory_partition_status =
       TrajectoryPartition(state_result_ds, control_result_ds, time_result_ds);
 
+  // record debug info
+  if (trajectory_partition_status.ok() && FLAGS_enable_record_debug) {
+    RecordDebugInfo();
+  }
+
   return trajectory_partition_status;
 }
 
@@ -244,16 +251,15 @@ void OpenSpaceTrajectoryGenerator::RecordDebugInfo(
     const Eigen::MatrixXd& l_warm_up, const Eigen::MatrixXd& n_warm_up,
     const Eigen::MatrixXd& dual_l_result_ds,
     const Eigen::MatrixXd& dual_n_result_ds,
+    const Eigen::MatrixXd& state_result_ds,
+    const Eigen::MatrixXd& control_result_ds,
     const std::vector<double>& XYbounds,
     ThreadSafeIndexedObstacles* obstalce_list) {
-  open_space_debug_.Clear();
-  // load trajectories by optimization and partition
-  open_space_debug_.mutable_trajectories()->CopyFrom(trajectory_partition_);
   // load warm start trajectory
-  auto* warm_start_trajecotry =
-      open_space_debug_.mutable_warm_start_trajecotry();
+  auto* warm_start_trajectory =
+      open_space_debug_.mutable_warm_start_trajectory();
   for (size_t i = 0; i < horizon_; i++) {
-    auto* warm_start_point = warm_start_trajecotry->add_vehicle_motion_point();
+    auto* warm_start_point = warm_start_trajectory->add_vehicle_motion_point();
     warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_x(
         xWS(0, i));
     warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_y(
@@ -265,7 +271,7 @@ void OpenSpaceTrajectoryGenerator::RecordDebugInfo(
     warm_start_point->set_steer(uWS(0, i));
     warm_start_point->mutable_trajectory_point()->set_a(uWS(1, i));
   }
-  auto* warm_start_point = warm_start_trajecotry->add_vehicle_motion_point();
+  auto* warm_start_point = warm_start_trajectory->add_vehicle_motion_point();
   warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_x(
       xWS(0, horizon_));
   warm_start_point->mutable_trajectory_point()->mutable_path_point()->set_y(
@@ -301,6 +307,31 @@ void OpenSpaceTrajectoryGenerator::RecordDebugInfo(
       open_space_debug_.add_optimized_dual_miu(dual_n_result_ds(j, i));
     }
   }
+
+  // load smoothed trajectory
+  auto* smoothed_trajectory = open_space_debug_.mutable_smoothed_trajectory();
+  for (size_t i = 0; i < horizon_; i++) {
+    auto* smoothed_point = smoothed_trajectory->add_vehicle_motion_point();
+    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_x(
+        state_result_ds(0, i));
+    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_y(
+        state_result_ds(1, i));
+    smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_theta(
+        state_result_ds(2, i));
+    smoothed_point->mutable_trajectory_point()->set_v(state_result_ds(3, i));
+    smoothed_point->set_steer(control_result_ds(0, i));
+    smoothed_point->mutable_trajectory_point()->set_a(control_result_ds(1, i));
+  }
+  auto* smoothed_point = smoothed_trajectory->add_vehicle_motion_point();
+  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_x(
+      state_result_ds(0, horizon_));
+  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_y(
+      state_result_ds(1, horizon_));
+  smoothed_point->mutable_trajectory_point()->mutable_path_point()->set_theta(
+      state_result_ds(2, horizon_));
+  smoothed_point->mutable_trajectory_point()->set_v(
+      state_result_ds(3, horizon_));
+
   // load xy boundary (xmin, xmax, ymin, ymax)
   open_space_debug_.add_xy_boundary(XYbounds[0]);
   open_space_debug_.add_xy_boundary(XYbounds[1]);
@@ -320,6 +351,11 @@ void OpenSpaceTrajectoryGenerator::RecordDebugInfo(
   }
 }
 
+void OpenSpaceTrajectoryGenerator::RecordDebugInfo() {
+  // load trajectories by optimization and partition
+  open_space_debug_.mutable_trajectories()->CopyFrom(trajectory_partition_);
+}
+
 Status OpenSpaceTrajectoryGenerator::TrajectoryPartition(
     const Eigen::MatrixXd& state_result_ds,
     const Eigen::MatrixXd& control_result_ds,

modules/planning/open_space/open_space_trajectory_generator.h

@@ -108,8 +108,11 @@ class OpenSpaceTrajectoryGenerator {
                        const Eigen::MatrixXd& n_warm_up,
                        const Eigen::MatrixXd& dual_l_result_ds,
                        const Eigen::MatrixXd& dual_n_result_ds,
+                       const Eigen::MatrixXd& state_result_ds,
+                       const Eigen::MatrixXd& control_result_ds,
                        const std::vector<double>& XYbounds,
                        ThreadSafeIndexedObstacles* obstalce_list);
+  void RecordDebugInfo();
 
  private:
   std::unique_ptr<::apollo::planning::HybridAStar> warm_start_;

modules/planning/open_space_planning.cc

@@ -323,7 +323,7 @@ void AddOpenSpaceTrajectory(const OpenSpaceDebug& open_space_debug,
 
   for (const auto& obstacle : open_space_debug.obstacles()) {
     auto* polygon = chart->add_polygon();
-    polygon->set_label(obstacle.id());
+    polygon->set_label("boundary");
     for (int vertice_index = 0;
          vertice_index < obstacle.vertices_x_coords_size(); vertice_index++) {
       auto* point_debug = polygon->add_point();
@@ -332,22 +332,35 @@ void AddOpenSpaceTrajectory(const OpenSpaceDebug& open_space_debug,
     }
   }
 
-  for (const auto& trajectory : open_space_debug.trajectories().trajectory()) {
-    auto* line = chart->add_line();
-    line->set_label(trajectory.name());
-    for (const auto& point : trajectory.trajectory_point()) {
-      auto* point_debug = line->add_point();
-      point_debug->set_x(point.path_point().x());
-      point_debug->set_y(point.path_point().y());
-    }
-
-    // Set chartJS's dataset properties
-    auto* properties = line->mutable_properties();
-    (*properties)["borderWidth"] = "2";
-    (*properties)["pointRadius"] = "0";
-    (*properties)["fill"] = "false";
-    (*properties)["showLine"] = "true";
+  auto smoothed_trajectory = open_space_debug.smoothed_trajectory();
+  auto* smoothed_line = chart->add_line();
+  smoothed_line->set_label("smoothed");
+  for (const auto& point : smoothed_trajectory.vehicle_motion_point()) {
+    auto* point_debug = smoothed_line->add_point();
+    point_debug->set_x(point.trajectory_point().path_point().x());
+    point_debug->set_y(point.trajectory_point().path_point().y());
+  }
+  // Set chartJS's dataset properties
+  auto* smoothed_properties = smoothed_line->mutable_properties();
+  (*smoothed_properties)["borderWidth"] = "2";
+  (*smoothed_properties)["pointRadius"] = "0";
+  (*smoothed_properties)["fill"] = "false";
+  (*smoothed_properties)["showLine"] = "true";
+
+  auto warm_start_trajectory = open_space_debug.warm_start_trajectory();
+  auto* warm_start_line = chart->add_line();
+  warm_start_line->set_label("warm_start");
+  for (const auto& point : warm_start_trajectory.vehicle_motion_point()) {
+    auto* point_debug = warm_start_line->add_point();
+    point_debug->set_x(point.trajectory_point().path_point().x());
+    point_debug->set_y(point.trajectory_point().path_point().y());
   }
+  // Set chartJS's dataset properties
+  auto* warm_start_properties = warm_start_line->mutable_properties();
+  (*warm_start_properties)["borderWidth"] = "2";
+  (*warm_start_properties)["pointRadius"] = "0";
+  (*warm_start_properties)["fill"] = "false";
+  (*warm_start_properties)["showLine"] = "true";
 }
 
 void OpenSpacePlanning::ExportOpenSpaceChart(
@@ -366,7 +379,7 @@ bool OpenSpacePlanning::CheckPlanningConfig(const PlanningConfig& config) {
 }
 
 void OpenSpacePlanning::FillPlanningPb(const double timestamp,
-                    ADCTrajectory* const trajectory_pb) {
+                                       ADCTrajectory* const trajectory_pb) {
   trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
   if (!local_view_.prediction_obstacles->has_header()) {
     trajectory_pb->mutable_header()->set_lidar_timestamp(

modules/planning/proto/planning_internal.proto

@@ -132,7 +132,8 @@ message Trajectories {
 
 message OpenSpaceDebug {
   optional apollo.planning_internal.Trajectories trajectories = 1;
-  optional apollo.common.VehicleMotion warm_start_trajecotry = 2;
+  optional apollo.common.VehicleMotion warm_start_trajectory = 2;
+  optional apollo.common.VehicleMotion smoothed_trajectory = 3;
   repeated double warm_start_dual_lambda = 4;
   repeated double warm_start_dual_miu = 5;
   repeated double optimized_dual_lambda = 6;