Planning: OpenSpace: fix wrong relative time adjustment in FillplanningPb

modules/planning/conf/planner_open_space_config.pb.txt

@@ -6,7 +6,7 @@ warm_start_config : {
   xy_grid_resolution : 0.2
   phi_grid_resolution : 0.05
   max_steering : 0.6
-  next_node_num : 10
+  next_node_num : 20
   step_size : 0.5
   back_penalty : 0.0
   gear_switch_penalty : 10.0

modules/planning/open_space/distance_approach_ipopt_interface.cc

@@ -1269,7 +1269,7 @@ bool DistanceApproachIPOPTInterface::eval_jac_g(int n, const double* x,
 
   ADEBUG << "eval_jac_g done";
   return true;
-} // NOLINT
+}  // NOLINT
 
 bool DistanceApproachIPOPTInterface::eval_h(int n, const double* x, bool new_x,
                                             double obj_factor, int m,

modules/planning/open_space/distance_approach_problem.cc

@@ -59,6 +59,7 @@ bool DistanceApproachProblem::Solve(
   app->Options()->SetNumericValue("mumps_pivtol", 1e-6);
   app->Options()->SetIntegerValue("max_iter", 1000);
   app->Options()->SetNumericValue("tol", 1e-4);
+  app->Options()->SetNumericValue("acceptable_constr_viol_tol", 1e-1);
   app->Options()->SetNumericValue("min_hessian_perturbation", 1e-12);
   app->Options()->SetNumericValue("jacobian_regularization_value", 1e-7);
   app->Options()->SetStringValue("print_timing_statistics", "no");

modules/planning/open_space/open_space_trajectory_generator.cc

@@ -330,8 +330,8 @@ Status OpenSpaceTrajectoryGenerator::TrajectoryPartition(
   if (horizon_ < 3)
     return Status(ErrorCode::PLANNING_ERROR, "Invalid trajectory length!");
 
-  if (state_result_ds(3, 0) > -1e-16 && state_result_ds(3, 1) > -1e-16 &&
-      state_result_ds(3, 2) > -1e-16) {
+  if (state_result_ds(3, 0) > 1e-16 && state_result_ds(3, 1) > 1e-16 &&
+      state_result_ds(3, 2) > 1e-16) {
     gear_positions_.push_back(canbus::Chassis::GEAR_DRIVE);
   } else if (state_result_ds(3, 0) < -1e-16 && state_result_ds(3, 1) < -1e-16 &&
              state_result_ds(3, 2) < -1e-16) {
@@ -354,7 +354,7 @@ Status OpenSpaceTrajectoryGenerator::TrajectoryPartition(
     }
     // 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-16 &&
+    if (state_result_ds(3, i) > 1e-16 &&
         gear_positions_.back() == canbus::Chassis::GEAR_REVERSE) {
       current_trajectory = trajectory_partition.add_trajectory();
       gear_positions_.push_back(canbus::Chassis::GEAR_DRIVE);

modules/planning/open_space_planning.cc

@@ -311,7 +311,7 @@ void AddOpenSpaceTrajectory(const OpenSpaceDebug& open_space_debug,
                             Chart* chart) {
   chart->set_title("Open Space Trajectory Visualization");
   auto* options = chart->mutable_options();
-  CHECK(open_space_debug.xy_boundary_size() == 4);
+  CHECK_EQ(open_space_debug.xy_boundary_size(), 4);
   options->mutable_x()->set_min(open_space_debug.xy_boundary(0));
   options->mutable_x()->set_max(open_space_debug.xy_boundary(1));
   options->mutable_x()->set_label_string("x (meter)");
@@ -363,5 +363,24 @@ bool OpenSpacePlanning::CheckPlanningConfig(const PlanningConfig& config) {
   return true;
 }
 
+void OpenSpacePlanning::FillPlanningPb(const double timestamp,
+                    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(
+        local_view_.prediction_obstacles->header().lidar_timestamp());
+    trajectory_pb->mutable_header()->set_camera_timestamp(
+        local_view_.prediction_obstacles->header().camera_timestamp());
+    trajectory_pb->mutable_header()->set_radar_timestamp(
+        local_view_.prediction_obstacles->header().radar_timestamp());
+  }
+  trajectory_pb->mutable_routing_header()->CopyFrom(
+      local_view_.routing->header());
+
+  if (FLAGS_use_planning_fallback &&
+      trajectory_pb->trajectory_point_size() == 0) {
+    SetFallbackTrajectory(trajectory_pb);
+  }
+}
 }  // namespace planning
 }  // namespace apollo

modules/planning/open_space_planning.h

@@ -69,6 +69,8 @@ class OpenSpacePlanning : public PlanningBase {
 
   void ExportOpenSpaceChart(const planning_internal::OpenSpaceDebug& debug_info,
                             planning_internal::Debug* debug_chart);
+  void FillPlanningPb(const double timestamp,
+                      ADCTrajectory* const trajectory_pb) override;
 
  private:
   common::Status InitFrame(const uint32_t sequence_num,

modules/planning/planning_base.h

@@ -72,8 +72,8 @@ class PlanningBase {
       ADCTrajectory* const trajectory) = 0;
 
  protected:
-  void FillPlanningPb(const double timestamp,
-                      ADCTrajectory* const trajectory_pb);
+  virtual void FillPlanningPb(const double timestamp,
+                              ADCTrajectory* const trajectory_pb);
   void SetFallbackTrajectory(ADCTrajectory* const trajectory_pb);
 
   virtual void ExportChart(const planning_internal::Debug& debug_info,

modules/tools/open_space_visualization/distance_approach_visualizer.py

@@ -30,8 +30,8 @@ OpenSpacePlanner = DistancePlanner()
 
 # parameter(except max, min and car size is defined in proto)
 num_output_buffer = 10000
-sx = -10.0
-sy = 3
+sx = -8
+sy = 4
 sphi = 0.0
 
 scenario = "backward"
@@ -110,7 +110,7 @@ start = time.time()
 print("planning start")
 if not OpenSpacePlanner.DistancePlan(sx, sy, sphi, ex, ey, ephi, XYbounds_ctype):
     print("planning fail")
-    exit()
+ #   exit()
 end = time.time()
 print("planning time is " + str(end - start))