Planning: OpenSpace: fix wrong control_state_output and wrong initial heading

modules/planning/conf/planner_open_space_config.pb.txt

 delta_t : 0.5
-max_position_error_to_end_point : 0.5
-max_theta_error_to_end_point : 0.2
+max_position_error_to_end_point : 0.05
+max_theta_error_to_end_point : 0.05
 
 warm_start_config : {
   xy_grid_resolution : 0.2

modules/planning/conf/planning.conf

@@ -24,12 +24,12 @@
 # --default_qp_smoothing_eps_num=-1e-3
 # --default_qp_smoothing_eps_den=1e-3
 
-#--enable_perception_obstacles=true
-#--parking_longitudinal_range=15
-#--parking_start_range=10
-#--parking_inwards=false
-#--use_dual_variable_warm_start=true
-#--open_space_planner_switchable=true
-#--enable_open_space_planner_thread=false
-#--enable_record_debug=true
-#--export_chart=true
+# --enable_perception_obstacles=false
+# --parking_longitudinal_range=15
+# --parking_start_range=10
+# --parking_inwards=false
+# --use_dual_variable_warm_start=true
+# --open_space_planner_switchable=true
+# --enable_open_space_planner_thread=false
+# --enable_record_debug=true
+# --export_chart=true

modules/planning/open_space/distance_approach_ipopt_interface.cc

@@ -1326,7 +1326,7 @@ void DistanceApproachIPOPTInterface::finalize_solution(
     state_result_(2, i) = x[state_index + 2];
     state_result_(3, i) = x[state_index + 3];
     control_result_(0, i) = x[control_index];
-    control_result_(1, i) = x[control_index];
+    control_result_(1, i) = x[control_index + 1];
     time_result_(0, i) = x[time_index];
     for (int j = 0; j < obstacles_edges_sum_; j++) {
       dual_l_result_(j, i) = x[dual_l_index + j];
@@ -1347,6 +1347,7 @@ void DistanceApproachIPOPTInterface::finalize_solution(
   state_result_(2, horizon_) = x[state_index + 2];
   state_result_(3, horizon_) = x[state_index + 3];
   time_result_(0, horizon_) = x[time_index];
+  time_result_ = ts_ * time_result_;
   for (int j = 0; j < obstacles_edges_sum_; j++) {
     dual_l_result_(j, horizon_) = x[dual_l_index + j];
   }

modules/planning/open_space/distance_approach_problem.cc

@@ -54,22 +54,15 @@ bool DistanceApproachProblem::Solve(
   // Create an instance of the IpoptApplication
   Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
 
-  // app->Options()->SetStringValue("hessian_approximation", "limited-memory");
-  // app->Options()->SetStringValue("jacobian_approximation",
-  // "finite-difference-values");
-  // app->Options()->SetStringValue("derivative_test", "first-order");
-  // app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
-  // TODO(QiL) : Change IPOPT settings to flag or configs
   app->Options()->SetIntegerValue("print_level", 0);
-  app->Options()->SetIntegerValue("mumps_mem_percent", 10000);
+  app->Options()->SetIntegerValue("mumps_mem_percent", 6000);
   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", "yes");
-  app->Options()->SetStringValue("print_timing_statistics", "yes");
+  app->Options()->SetStringValue("print_timing_statistics", "no");
   app->Options()->SetStringValue("alpha_for_y", "min");
   app->Options()->SetStringValue("recalc_y", "yes");
 

modules/planning/open_space/open_space_ROI.cc

@@ -507,7 +507,7 @@ bool OpenSpaceROI::GetMapInfo(ParkingSpaceInfoConstPtr *target_parking_spot,
   double vehicle_lane_s = 0.0;
   double vehicle_lane_l = 0.0;
   int status = HDMapUtil::BaseMap().GetNearestLaneWithHeading(
-      point, 5.0, vehicle_state_.heading(), M_PI / 3.0, &nearest_lane,
+      point, 10.0, vehicle_state_.heading(), M_PI / 2.0, &nearest_lane,
       &vehicle_lane_s, &vehicle_lane_l);
   if (status != 0) {
     AERROR << "Getlane failed at OpenSpaceROI::GetOpenSpaceROI()";

modules/planning/open_space/open_space_trajectory_generator.cc

@@ -88,13 +88,13 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
 
   // rotate and scale the state according to the origin point defined in
   // frame
-  init_x_ = init_x_ - translate_origin.x();
-  init_y_ = init_y_ - translate_origin.y();
+  init_x_ -= translate_origin.x();
+  init_y_ -= translate_origin.y();
   double tmp_x = init_x_;
   init_x_ =
       init_x_ * std::cos(-rotate_angle) - init_y_ * std::sin(-rotate_angle);
   init_y_ = tmp_x * std::sin(-rotate_angle) + init_y_ * std::cos(-rotate_angle);
-
+  init_phi_ = common::math::NormalizeAngle(init_phi_ - rotate_angle);
   // TODO(Jinyun) how to initial input not decided yet
   init_steer_ = 0;
   init_a_ = 0;
@@ -204,7 +204,6 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
     RecordDebugInfo(xWS, uWS, l_warm_up, n_warm_up, dual_l_result_ds,
                     dual_n_result_ds, XYbounds_, obstalce_list);
   }
-
   // rescale the states to the world frame
   for (std::size_t i = 0; i < horizon_ + 1; i++) {
     double tmp_x = state_result_ds(0, i);
@@ -214,7 +213,8 @@ apollo::common::Status OpenSpaceTrajectoryGenerator::Plan(
                             state_result_ds(1, i) * std::cos(rotate_angle);
     state_result_ds(0, i) += translate_origin.x();
     state_result_ds(1, i) += translate_origin.y();
-    state_result_ds(2, i) += rotate_angle;
+    state_result_ds(2, i) =
+        common::math::NormalizeAngle(state_result_ds(2, i) + rotate_angle);
   }
 
   // Step 9 : Trajectory Partition and  Publish TrajectoryPoint
@@ -230,6 +230,7 @@ void OpenSpaceTrajectoryGenerator::UpdateTrajectory(
     Trajectories* adc_trajectories,
     std::vector<canbus::Chassis::GearPosition>* gear_positions) {
   adc_trajectories->CopyFrom(trajectory_partition_);
+  *gear_positions = gear_positions_;
 }
 
 void OpenSpaceTrajectoryGenerator::UpdateDebugInfo(
@@ -338,20 +339,19 @@ Status OpenSpaceTrajectoryGenerator::TrajectoryPartition(
   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) {
+    gear_positions_.push_back(canbus::Chassis::GEAR_REVERSE);
   } else {
-    if (state_result_ds(3, 0) < 1e-3 && state_result_ds(3, 1) < 1e-3 &&
-        state_result_ds(3, 2) < 1e-3) {
-      gear_positions_.push_back(canbus::Chassis::GEAR_REVERSE);
-    } else {
-      return Status(ErrorCode::PLANNING_ERROR, "Invalid trajectory start!");
-    }
+    return Status(ErrorCode::PLANNING_ERROR,
+                  "Invalid trajectory start! initial speed too small");
   }
 
   // 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 &&
+    if (state_result_ds(3, i) < -1e-16 &&
         gear_positions_.back() == canbus::Chassis::GEAR_DRIVE) {
       current_trajectory = trajectory_partition.add_trajectory();
       gear_positions_.push_back(canbus::Chassis::GEAR_REVERSE);
@@ -360,20 +360,18 @@ Status OpenSpaceTrajectoryGenerator::TrajectoryPartition(
     }
     // shift from GEAR_REVERSE to GEAR_DRIVE if v > 0
     // then add a new trajectory with GEAR_DRIVE
-<<<<<<< HEAD
-    if (state_result_ds(3, i) > 1e-3 &&
-=======
     if (state_result_ds(3, i) > 1e-16 &&
->>>>>>> 0d5506d... Planning: OpenSpace: fix wrong relative time adjustment in FillplanningPb
+        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;
     }
 
     auto* point = current_trajectory->add_trajectory_point();
-    relative_time += time_result_ds(0, i);
     point->set_relative_time(relative_time);
+    relative_time += time_result_ds(0, i);
     point->mutable_path_point()->set_x(state_result_ds(0, i));
     point->mutable_path_point()->set_y(state_result_ds(1, i));
-    point->mutable_path_point()->set_z(0);
     point->mutable_path_point()->set_theta(state_result_ds(2, i));
     if (i > 0) {
       distance_s +=

modules/planning/open_space_planning.cc

@@ -289,26 +289,23 @@ Status OpenSpacePlanning::Plan(
   trajectory_pb->CopyFrom(frame_->trajectory());
 
   auto* ptr_debug = trajectory_pb->mutable_debug();
-  if (FLAGS_enable_record_debug) {
-    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
-        stitching_trajectory.back());
-    ADEBUG << "Open space init point added!";
-  }
-
-  if (status.ok() && FLAGS_enable_record_debug) {
-    ptr_debug->mutable_planning_data()->mutable_open_space()->CopyFrom(
-        frame_->open_space_debug());
-    ADEBUG << "Open space debug information added!";
-  }
-
-  if (FLAGS_export_chart) {
-    ExportOpenSpaceChart(frame_->open_space_debug(), ptr_debug);
-    ADEBUG << "Open Space Planning debug from frame is : "
-           << frame_->open_space_debug().ShortDebugString();
-    ADEBUG << "Open Space Planning export chart with : "
-           << trajectory_pb->ShortDebugString();
+  if (status.ok()) {
+    if (FLAGS_enable_record_debug) {
+      ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
+          stitching_trajectory.back());
+      ADEBUG << "Open space init point added!";
+      ptr_debug->mutable_planning_data()->mutable_open_space()->CopyFrom(
+          frame_->open_space_debug());
+      ADEBUG << "Open space debug information added!";
+    }
+    if (FLAGS_enable_record_debug && FLAGS_export_chart) {
+      ExportOpenSpaceChart(frame_->open_space_debug(), ptr_debug);
+      ADEBUG << "Open Space Planning debug from frame is : "
+             << frame_->open_space_debug().ShortDebugString();
+      ADEBUG << "Open Space Planning export chart with : "
+             << trajectory_pb->ShortDebugString();
+    }
   }
-
   return status;
 }
 

modules/planning/planner/open_space/open_space_planner.cc

@@ -92,6 +92,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
       trajectory_partition_.Clear();
       gear_positions_.clear();
       open_space_debug_.Clear();
+      current_trajectory_index_ = 0;
       open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                          &gear_positions_);
       open_space_trajectory_generator_->UpdateDebugInfo(&open_space_debug_);
@@ -113,8 +114,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
     TrajectoryPoint end_point = current_trajectory_.trajectory_point(
         current_trajectory_.trajectory_point_size() - 1);
 
-    if (vehicle_state_.linear_velocity() <= 1e-3 &&
-        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
+    if (std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                       (vehicle_state_.x() - end_point.path_point().x()) +
                   (vehicle_state_.y() - end_point.path_point().y()) *
                       (vehicle_state_.y() - end_point.path_point().y())) <
@@ -166,12 +166,12 @@ apollo::common::Status OpenSpacePlanner::Plan(
         vehicle_state_, XYbounds_, rotate_angle_, translate_origin_, end_pose_,
         obstacles_num_, obstacles_edges_num_, obstacles_A_, obstacles_b_,
         obstalce_list_);
-
     // 3. If status is OK, update vehicle trajectory;
     if (status == Status::OK()) {
       trajectory_partition_.Clear();
       gear_positions_.clear();
       open_space_debug_.Clear();
+      current_trajectory_index_ = 0;
       open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                          &gear_positions_);
       open_space_trajectory_generator_->UpdateDebugInfo(&open_space_debug_);
@@ -181,15 +181,11 @@ apollo::common::Status OpenSpacePlanner::Plan(
       return Status(ErrorCode::PLANNING_ERROR,
                     "Planning failed to generate open space trajectory");
     }
-
     current_trajectory_ =
         trajectory_partition_.trajectory(current_trajectory_index_);
-
     TrajectoryPoint end_point = current_trajectory_.trajectory_point(
         current_trajectory_.trajectory_point_size() - 1);
-
-    if (vehicle_state_.linear_velocity() <= 1e-3 &&
-        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
+    if (std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                       (vehicle_state_.x() - end_point.path_point().x()) +
                   (vehicle_state_.y() - end_point.path_point().y()) *
                       (vehicle_state_.y() - end_point.path_point().y())) <
@@ -198,7 +194,7 @@ apollo::common::Status OpenSpacePlanner::Plan(
             planner_open_space_config_.max_theta_error_to_end_point() &&
         (current_trajectory_index_ <
          trajectory_partition_.trajectory_size() - 1)) {
-      current_trajectory_index_ += 1;
+      current_trajectory_index_++;
     }
 
     // 4. Collision check for updated trajectory, if pass, update frame, else,
@@ -208,6 +204,8 @@ apollo::common::Status OpenSpacePlanner::Plan(
       publishable_trajectory_.Clear();
       publishable_trajectory_.mutable_trajectory_point()->CopyFrom(
           *(current_trajectory_.mutable_trajectory_point()));
+      publishable_trajectory_.set_gear(
+          gear_positions_[current_trajectory_index_]);
       frame->mutable_trajectory()->CopyFrom(publishable_trajectory_);
       frame->mutable_open_space_debug()->CopyFrom(open_space_debug_);
       return Status::OK();