planning: add mission complete support. (#972)

modules/planning/common/decider.cc

@@ -24,7 +24,9 @@
 
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/log.h"
+#include "modules/common/util/util.h"
 #include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/common/reference_line_info.h"
 
 namespace apollo {
@@ -45,11 +47,31 @@ void Decider::MakeDecision(const ReferenceLineInfo& reference_line_info,
   if (error_code < 0) {
     MakeEStopDecision(path_decision);
   }
-  // TODO(all): check other main decisions
+  MakeMainMissionCompleteDecision(reference_line_info);
 
   SetObjectDecisions(path_decision);
 }
 
+void Decider::MakeMainMissionCompleteDecision(
+  const ReferenceLineInfo& reference_line_info) {
+  if (!decision_result_->main_decision().has_stop()) {
+    return;
+  }
+  auto main_stop = decision_result_->main_decision().stop();
+  if (main_stop.reason_code() != STOP_REASON_DESTINATION) {
+    return;
+  }
+  const auto& adc_pos = reference_line_info.init_adc_point().path_point();
+  if (common::util::DistanceXY(adc_pos, main_stop.stop_point()) >
+      FLAGS_destination_check_distance) {
+    return;
+  }
+  auto mission_complete =
+      decision_result_->mutable_main_decision()->mutable_mission_complete();
+  mission_complete->mutable_stop_point()->CopyFrom(main_stop.stop_point());
+  mission_complete->set_stop_heading(main_stop.stop_heading());
+}
+
 int Decider::MakeMainStopDecision(
     const ReferenceLineInfo& reference_line_info) {
   double min_stop_line_s = std::numeric_limits<double>::infinity();

modules/planning/common/decider.h

@@ -40,6 +40,8 @@ class Decider {
 
  private:
   int MakeMainStopDecision(const ReferenceLineInfo &reference_line_info);
+  void MakeMainMissionCompleteDecision(
+      const ReferenceLineInfo &reference_line_info);
   void MakeEStopDecision(const PathDecision &path_decision);
   void SetObjectDecisions(const PathDecision &path_decision);
 

modules/planning/common/planning_gflags.cc

@@ -127,6 +127,9 @@ DEFINE_double(stop_distance_obstacle, 10.0,
               "stop distance from in-lane obstacle (meters)");
 DEFINE_double(stop_distance_destination, 3.0,
               "stop distance from destination line");
+DEFINE_double(destination_check_distance, 5.0,
+              "if the distance between destination and ADC is less than this,"
+              " it is considered to reach destination");
 DEFINE_double(nudge_distance_obstacle, 0.3,
               "minimum distance to nudge a obstacle (meters)");
 DEFINE_double(follow_min_distance, 10,

modules/planning/common/planning_gflags.h

@@ -82,6 +82,7 @@ DECLARE_double(static_decision_nudge_l_buffer);
 DECLARE_double(lateral_ignore_buffer);
 DECLARE_double(stop_distance_obstacle);
 DECLARE_double(stop_distance_destination);
+DECLARE_double(destination_check_distance);
 DECLARE_double(nudge_distance_obstacle);
 DECLARE_double(follow_min_distance);
 DECLARE_double(follow_time_buffer);

modules/planning/common/reference_line_info.cc

@@ -102,6 +102,9 @@ PathDecision* ReferenceLineInfo::path_decision() { return &path_decision_; }
 const PathDecision& ReferenceLineInfo::path_decision() const {
   return path_decision_;
 }
+const common::TrajectoryPoint& ReferenceLineInfo::init_adc_point() const {
+  return init_adc_point_;
+}
 
 const ReferenceLine& ReferenceLineInfo::reference_line() const {
   return reference_line_;

modules/planning/common/reference_line_info.h

@@ -60,6 +60,8 @@ class ReferenceLineInfo {
   PathDecision* path_decision();
   const PathDecision& path_decision() const;
   const ReferenceLine& reference_line() const;
+  const common::TrajectoryPoint& init_adc_point() const;
+
 
   void SetTrajectory(const DiscretizedTrajectory& trajectory);
 

modules/planning/integration_tests/sunnyvale_loop_test.cc

@@ -166,5 +166,18 @@ TEST_F(SunnyvaleLoopTest, change_lane) {
   RUN_GOLDEN_TEST;
 }
 
+/*
+ * test mission complete
+ */
+TEST_F(SunnyvaleLoopTest, mission_complete) {
+  std::string seq_num = "10";
+  FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
+  FLAGS_enable_prediction = false;
+  FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
+  FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/proto/decision.proto

@@ -151,6 +151,10 @@ message MainChangeLane {
 
 message MainMissionComplete {
   // arrived at routing destination
+  // When stopped, the front center of vehicle should be at this point.
+  optional apollo.common.PointENU stop_point = 1;
+  // When stopped, the heading of the vehicle should be stop_heading.
+  optional double stop_heading = 2;
 }
 
 message MainNotReady {

modules/planning/testdata/sunnyvale_loop_test/10_chassis.pb.txt

+engine_started: true
+speed_mps: 0
+throttle_percentage: 0
+brake_percentage: 0
+driving_mode: COMPLETE_AUTO_DRIVE
+gear_location: GEAR_DRIVE
+header {
+  timestamp_sec: 1509506440.1766765
+  module_name: "SimControl"
+  sequence_num: 33425
+}

modules/planning/testdata/sunnyvale_loop_test/10_localization.pb.txt

+header {
+  timestamp_sec: 1509506440.1767445
+  module_name: "SimControl"
+  sequence_num: 33425
+}
+pose {
+  position {
+    x: 587610.56668756611
+    y: 4140980.447316783
+    z: 0
+  }
+  orientation {
+    qx: 0
+    qy: 0
+    qz: -0.12490509248506153
+    qw: 0.99216869426085919
+  }
+  linear_velocity {
+    x: 0
+    y: 0
+    z: 0
+  }
+  linear_acceleration {
+    x: 0
+    y: 0
+    z: 0
+  }
+  angular_velocity {
+    x: 0
+    y: 0
+    z: -0
+  }
+  heading: 1.3203319788735257
+  linear_acceleration_vrf {
+    x: 0
+    y: 0
+    z: 0
+  }
+  angular_velocity_vrf {
+    x: 0
+    y: 0
+    z: 0
+  }
+}

modules/planning/testdata/sunnyvale_loop_test/10_routing.pb.txt

+header {
+  timestamp_sec: 1509506244.1569996
+  module_name: "routing"
+  sequence_num: 3
+}
+road {
+  id: "57"
+  passage {
+    segment {
+      id: "57_1_-1"
+      start_s: 164.97164317920888
+      end_s: 285.05279843053006
+    }
+    can_exit: true
+    change_lane_type: FORWARD
+  }
+}
+measurement {
+  distance: 120.08115525132118
+}
+routing_request {
+  header {
+    timestamp_sec: 1509506244.1533203
+    module_name: "dreamview"
+    sequence_num: 3
+  }
+  waypoint {
+    id: "57_1_-1"
+    s: 164.97164317920888
+    pose {
+      x: 587583.393211436
+      y: 4140874.3642127877
+    }
+  }
+  waypoint {
+    id: "57_1_-1"
+    s: 285.05279843053006
+    pose {
+      x: 587613.1190959675
+      y: 4140990.6894103163
+    }
+  }
+}
+map_version: "1.000000"
+status {
+  error_code: OK
+  msg: "Success!"
+}