Planning: make sure the acceleration is correctly obtained from VehicleState.

modules/common/vehicle_state/vehicle_state.cc

@@ -70,10 +70,14 @@ void VehicleState::ConstructExceptLinearVelocity(
   }
 
   if (FLAGS_enable_map_reference_unify) {
+    CHECK(localization.pose().has_angular_velocity_vrf());
     angular_v_ = localization.pose().angular_velocity_vrf().z();
+    CHECK(localization.pose().has_linear_acceleration_vrf());
     linear_a_y_ = localization.pose().linear_acceleration_vrf().y();
   } else {
+    CHECK(localization.pose().has_angular_velocity());
     angular_v_ = localization.pose().angular_velocity().z();
+    CHECK(localization.pose().has_linear_acceleration());
     linear_a_y_ = localization.pose().linear_acceleration().y();
   }
 

modules/common/vehicle_state/vehicle_state.h

@@ -249,4 +249,4 @@ class VehicleState {
 }  // namespace common
 }  // namespace apollo
 
-#endif /* MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_H_ */
+#endif  // MODULES_COMMON_VEHICLE_STATE_VEHICLE_STATE_H_

modules/planning/planning.cc

@@ -65,7 +65,7 @@ bool Planning::InitFrame(const uint32_t sequence_num) {
     const auto& prediction =
         AdapterManager::GetPrediction()->GetLatestObserved();
     frame_->SetPrediction(prediction);
-    ADEBUG << "Get prediction";
+    ADEBUG << "Get prediction done.";
   }
 
   if (!frame_->Init(config_)) {

modules/planning/trajectory_stitcher/trajectory_stitcher.cc

@@ -31,19 +31,32 @@ namespace planning {
 
 using VehicleState = apollo::common::VehicleState;
 
+namespace {
+
 std::vector<common::TrajectoryPoint> compute_reinit_stitching_trajectory() {
   common::TrajectoryPoint init_point;
   init_point.mutable_path_point()->set_x(VehicleState::instance()->x());
   init_point.mutable_path_point()->set_y(VehicleState::instance()->y());
-  init_point.set_v(VehicleState::instance()->linear_velocity());
-  init_point.set_a(VehicleState::instance()->linear_acceleration());
+  init_point.mutable_path_point()->set_z(VehicleState::instance()->z());
   init_point.mutable_path_point()->set_theta(
       VehicleState::instance()->heading());
   init_point.mutable_path_point()->set_kappa(VehicleState::instance()->kappa());
 
+  init_point.set_v(VehicleState::instance()->linear_velocity());
+  init_point.set_a(VehicleState::instance()->linear_acceleration());
   init_point.set_relative_time(0.0);
+
+  DCHECK(!std::isnan(init_point.path_point().x()));
+  DCHECK(!std::isnan(init_point.path_point().y()));
+  DCHECK(!std::isnan(init_point.path_point().z()));
+  DCHECK(!std::isnan(init_point.path_point().theta()));
+  DCHECK(!std::isnan(init_point.path_point().kappa()));
+  DCHECK(!std::isnan(init_point.v()));
+  DCHECK(!std::isnan(init_point.a()));
+
   return std::vector<common::TrajectoryPoint>(1, init_point);
 }
+}
 
 // Planning from current vehicle state:
 // if 1. the auto-driving mode is off or

modules/planning/trajectory_stitcher/trajectory_stitcher.h

@@ -18,8 +18,8 @@
  * @file trajectory_stitcher.h
  **/
 
-#ifndef MODULES_PLANNING_TRAJECTORY_STITCHER_H_
-#define MODULES_PLANNING_TRAJECTORY_STITCHER_H_
+#ifndef MODULES_PLANNING_TRAJECTORY_STITCHER_TRAJECTORY_STITCHER_H_
+#define MODULES_PLANNING_TRAJECTORY_STITCHER_TRAJECTORY_STITCHER_H_
 
 #include <vector>
 
@@ -45,4 +45,4 @@ class TrajectoryStitcher {
 }  // namespace planning
 }  // namespace apollo
 
-#endif  // MODULES_PLANNING_TRAJECTORY_STITCHER_H_
+#endif  // MODULES_PLANNING_TRAJECTORY_STITCHER_TRAJECTORY_STITCHER_H_