planning: output planning topic msg when something goes wrong. (#2098)

modules/planning/integration_tests/planning_test_base.cc

@@ -186,11 +186,6 @@ bool PlanningTestBase::RunPlanning(const std::string& test_case_name,
 }
 
 bool PlanningTestBase::IsValidTrajectory(const ADCTrajectory& trajectory) {
-  if (trajectory.trajectory_point().empty()) {
-    AERROR << "trajectory has NO point.";
-    return false;
-  }
-
   for (int i = 0; i < trajectory.trajectory_point_size(); ++i) {
     const auto& point = trajectory.trajectory_point(i);
 

modules/planning/planning.cc

@@ -219,8 +219,9 @@ void Planning::RunOnce() {
   }
 
   if (!status.ok() || !IsVehicleStateValid(vehicle_state)) {
-    AERROR << "Update VehicleStateProvider failed.";
-    not_ready->set_reason("Update VehicleStateProvider failed.");
+    std::string msg("Update VehicleStateProvider failed");
+    AERROR << msg;
+    not_ready->set_reason(msg);
     status.Save(not_ready_pb.mutable_header()->mutable_status());
     PublishPlanningPb(&not_ready_pb, start_timestamp);
     return;
@@ -228,7 +229,11 @@ void Planning::RunOnce() {
 
   if (!ReferenceLineProvider::instance()->UpdateRoutingResponse(
           AdapterManager::GetRoutingResponse()->GetLatestObserved())) {
-    AERROR << "Failed to update routing in reference line provider";
+    std::string msg("Failed to update routing in reference line provider");
+    AERROR << msg;
+    not_ready->set_reason(msg);
+    status.Save(not_ready_pb.mutable_header()->mutable_status());
+    PublishPlanningPb(&not_ready_pb, start_timestamp);
     return;
   }
 
@@ -250,7 +255,11 @@ void Planning::RunOnce() {
   status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp,
                      vehicle_state);
   if (!frame_) {
-    AERROR << "Failed to init frame";
+    std::string msg("Failed to init frame");
+    AERROR << msg;
+    not_ready->set_reason(msg);
+    status.Save(not_ready_pb.mutable_header()->mutable_status());
+    PublishPlanningPb(&not_ready_pb, start_timestamp);
     return;
   }
   auto* trajectory_pb = frame_->mutable_trajectory();
@@ -260,15 +269,22 @@ void Planning::RunOnce() {
   trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(
       Clock::NowInSeconds() - start_timestamp);
   if (!status.ok()) {
-    AERROR << "Init frame failed";
+    std::string msg("Failed to init frame");
+    AERROR << msg;
     if (FLAGS_publish_estop) {
       ADCTrajectory estop;
       estop.mutable_estop();
       status.Save(estop.mutable_header()->mutable_status());
       PublishPlanningPb(&estop, start_timestamp);
+    } else {
+      not_ready->set_reason(msg);
+      status.Save(not_ready_pb.mutable_header()->mutable_status());
+      PublishPlanningPb(&not_ready_pb, start_timestamp);
     }
+
     auto seq_num = frame_->SequenceNum();
     FrameHistory::instance()->Add(seq_num, std::move(frame_));
+
     return;
   }