planning: added engage advice

modules/planning/common/reference_line_info.cc

@@ -38,12 +38,15 @@
 namespace apollo {
 namespace planning {
 
-using apollo::common::math::Vec2d;
-using apollo::common::math::Box2d;
+using apollo::common::EngageAdvice;
 using apollo::common::SLPoint;
 using apollo::common::TrajectoryPoint;
 using apollo::common::VehicleConfigHelper;
 using apollo::common::VehicleSignal;
+using apollo::common::math::Box2d;
+using apollo::common::math::Vec2d;
+using apollo::canbus::Chassis;
+using apollo::common::util::Dropbox;
 
 ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state,
                                      const TrajectoryPoint& adc_planning_point,
@@ -535,6 +538,67 @@ void ReferenceLineInfo::SetObjectDecisions(
   }
 }
 
+void ReferenceLineInfo::ExportEngageAdvice(EngageAdvice* engage_advice) const {
+  constexpr char kPrevAdvice[] = "PlanningPreviousAdvice";
+  constexpr double kMaxAngleDiff = M_PI / 6.0;
+  auto* prev_advice = Dropbox<EngageAdvice>::Open()->Get(kPrevAdvice);
+  if (!prev_advice) {
+    EngageAdvice advice;
+    Dropbox<EngageAdvice>::Open()->Set(kPrevAdvice, advice);
+    prev_advice = Dropbox<EngageAdvice>::Open()->Get(kPrevAdvice);
+  }
+  if (!IsDrivable()) {
+    if (prev_advice->advice() == EngageAdvice::DISALLOW_ENGAGE) {
+      engage_advice->set_advice(EngageAdvice::DISALLOW_ENGAGE);
+    } else {
+      engage_advice->set_advice(EngageAdvice::PREPARE_DISENGAGE);
+    }
+    engage_advice->set_reason("Reference line not drivable");
+  } else if (!is_on_reference_line_) {
+    if (prev_advice->advice() == EngageAdvice::DISALLOW_ENGAGE) {
+      engage_advice->set_advice(EngageAdvice::DISALLOW_ENGAGE);
+    } else {
+      engage_advice->set_advice(EngageAdvice::PREPARE_DISENGAGE);
+    }
+    engage_advice->set_reason("Not on reference line");
+  } else {
+    // check heading
+    auto ref_point =
+        reference_line_.GetReferencePoint(adc_sl_boundary_.end_s());
+    if (common::math::AngleDiff(vehicle_state_.heading(), ref_point.heading()) >
+        kMaxAngleDiff) {
+      if (prev_advice->advice() == EngageAdvice::DISALLOW_ENGAGE) {
+        engage_advice->set_advice(EngageAdvice::DISALLOW_ENGAGE);
+      } else {
+        engage_advice->set_advice(EngageAdvice::PREPARE_DISENGAGE);
+      }
+      engage_advice->set_reason("Vehicle heading is not aligned");
+    } else {
+      if (prev_advice->advice() == EngageAdvice::DISALLOW_ENGAGE ||
+          prev_advice->advice() == EngageAdvice::UNKNOWN) {
+        engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
+      } else if (prev_advice->advice() == EngageAdvice::READY_TO_ENGAGE) {
+        if (vehicle_state_.driving_mode() !=
+            Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
+          engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
+        } else {
+          engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
+        }
+      } else if (prev_advice->advice() == EngageAdvice::PREPARE_DISENGAGE) {
+        if (vehicle_state_.driving_mode() !=
+            Chassis::DrivingMode::Chassis_DrivingMode_COMPLETE_AUTO_DRIVE) {
+          engage_advice->set_advice(EngageAdvice::READY_TO_ENGAGE);
+        } else {
+          engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
+        }
+      } else if (prev_advice->advice() == EngageAdvice::KEEP_ENGAGED) {
+        engage_advice->set_advice(EngageAdvice::KEEP_ENGAGED);
+      }
+    }
+  }
+  *prev_advice = *engage_advice;
+}
+
 void ReferenceLineInfo::MakeEStopDecision(
     DecisionResult* decision_result) const {
   decision_result->Clear();

modules/planning/common/reference_line_info.h

@@ -28,6 +28,7 @@
 #include <unordered_set>
 #include <vector>
 
+#include "modules/common/proto/drive_state.pb.h"
 #include "modules/common/proto/pnc_point.pb.h"
 #include "modules/common/proto/vehicle_state.pb.h"
 #include "modules/planning/proto/planning.pb.h"
@@ -113,6 +114,8 @@ class ReferenceLineInfo {
   void SetDrivable(bool drivable);
   bool IsDrivable() const;
 
+  void ExportEngageAdvice(common::EngageAdvice* engage_advice) const;
+
   const hdmap::RouteSegments& Lanes() const;
   const std::list<hdmap::Id> TargetLaneId() const;
 

modules/planning/planning.cc

@@ -452,6 +452,10 @@ Status Planning::Plan(const double current_time_stamp,
   }
 
   last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);
+
+  best_reference_line->ExportEngageAdvice(
+      trajectory_pb->mutable_engage_advice());
+
   return status;
 }
 

modules/planning/proto/BUILD

@@ -19,6 +19,7 @@ proto_library(
         ":planning_internal_proto_lib",
         "//modules/canbus/proto:canbus_proto_lib",
         "//modules/common/proto:common_proto_lib",
+        "//modules/common/proto:drive_state_proto_lib",
         "//modules/common/proto:error_code_proto_lib",
         "//modules/common/proto:header_proto_lib",
         "//modules/common/proto:pnc_point_proto_lib",

modules/planning/proto/planning.proto

@@ -4,6 +4,7 @@ package apollo.planning;
 
 import "modules/common/proto/header.proto";
 import "modules/common/proto/vehicle_signal.proto";
+import "modules/common/proto/drive_state.proto";
 import "modules/common/proto/pnc_point.proto";
 import "modules/canbus/proto/chassis.proto";
 import "modules/map/proto/map_id.proto";
@@ -71,7 +72,7 @@ message LatencyStats {
   optional double init_frame_time_ms = 3;
 }
 
-// next id: 19
+// next id: 20
 message ADCTrajectory {
   optional apollo.common.Header header = 1;
 
@@ -113,4 +114,8 @@ message ADCTrajectory {
 
   // lane id along reference line
   repeated apollo.hdmap.Id lane_id = 18;
+
+  // set the engage advice for based on current planning result.
+  optional apollo.common.EngageAdvice engage_advice = 19;
+
 }

modules/planning/testdata/garage_test/result_follow_0.pb.txt

@@ -1744,3 +1744,6 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "1_-1"
 }
+engage_advice {
+  advice: READY_TO_ENGAGE
+}

modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt

@@ -1744,3 +1744,6 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "1_-1"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt

@@ -1758,3 +1758,7 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "1_-1"
 }
+engage_advice {
+  advice: DISALLOW_ENGAGE
+  reason: "Not on reference line"
+}

modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt

@@ -906,3 +906,6 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "1_-1"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt

@@ -1580,3 +1580,7 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "40_1_-2"
 }
+engage_advice {
+  advice: PREPARE_DISENGAGE
+  reason: "Not on reference line"
+}

modules/planning/testdata/sunnyvale_loop_test/result_change_lane_0.pb.txt

@@ -1721,3 +1721,7 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "57_1_-1"
 }
+engage_advice {
+  advice: PREPARE_DISENGAGE
+  reason: "Not on reference line"
+}

modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt

@@ -1362,3 +1362,6 @@ lane_id {
 lane_id {
   id: "6_1_-2"
 }
+engage_advice {
+  advice: READY_TO_ENGAGE
+}

modules/planning/testdata/sunnyvale_loop_test/result_follow_01_0.pb.txt

@@ -1747,3 +1747,6 @@ lane_id {
 lane_id {
   id: "9_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt

@@ -1323,3 +1323,6 @@ lane_id {
 lane_id {
   id: "58_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt

@@ -1751,3 +1751,6 @@ lane_id {
 lane_id {
   id: "9_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt

@@ -1743,3 +1743,6 @@ right_of_way_status: UNPROTECTED
 lane_id {
   id: "57_1_-1"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt

@@ -1595,3 +1595,6 @@ lane_id {
 lane_id {
   id: "140_1_-1"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt

@@ -1690,3 +1690,6 @@ lane_id {
 lane_id {
   id: "22_1_-1"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt

@@ -1747,3 +1747,6 @@ lane_id {
 lane_id {
   id: "58_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_rightturn_with_red_light_0.pb.txt

@@ -1747,3 +1747,6 @@ lane_id {
 lane_id {
   id: "58_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt

@@ -1748,3 +1748,6 @@ lane_id {
 lane_id {
   id: "9_1_-2"
 }
+engage_advice {
+  advice: KEEP_ENGAGED
+}

modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt

@@ -1887,3 +1887,6 @@ lane_id {
 lane_id {
   id: "160_1_-3"
 }
+engage_advice {
+  advice: READY_TO_ENGAGE
+}