planning: open space add configurations to debug parallel trajectory

modules/planning/conf/planning_config.pb.txt

@@ -197,6 +197,7 @@ default_task_config: {
         max_position_error_to_end_point: 0.05
         max_theta_error_to_end_point: 0.05
         is_near_destination_threshold: 0.05
+        enable_check_parallel_trajectory: true
       }
     }
   }

modules/planning/proto/planner_open_space_config.proto

@@ -32,6 +32,7 @@ message PlannerOpenSpaceConfig {
   optional double max_position_error_to_end_point = 7 [default = 0.5];
   optional double max_theta_error_to_end_point = 8 [default = 0.2];
   optional double is_near_destination_threshold = 9 [default = 0.001];
+  optional bool enable_check_parallel_trajectory = 10 [default = false];
 }
 
 message ROIConfig {

modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_optimizer.cc

@@ -142,6 +142,16 @@ common::Status OpenSpaceTrajectoryOptimizer::Plan(
     for (size_t i = 0; i < size; ++i) {
       LoadHybridAstarResultInEigen(&partition_trajectories[i], &xWS_vec[i],
                                    &uWS_vec[i]);
+      // checking initial and ending points
+      if (config_.
+          planner_open_space_config().enable_check_parallel_trajectory()) {
+        AINFO << "trajectory id: " << i;
+        AINFO << "trajectory partitioned size: " << xWS_vec[i].cols();
+        AINFO << "initial point: " << xWS_vec[i].col(0).transpose();
+        AINFO << "ending point: "
+            << xWS_vec[i].col(xWS_vec[i].cols() - 1).transpose();
+      }
+
       Eigen::MatrixXd last_time_u(2, 1);
       double init_v = 0.0;
       // Stitching point control and velocity is set for first piece of