planning: fix navigation mode planning trajectory stitching problem

modules/common/math/quaternion.h

@@ -71,7 +71,7 @@ inline double QuaternionToHeading(const double qw, const double qx,
  * @return Heading encoded by given quaternion
  */
 template <typename T>
-inline double QuaternionToHeading(const Eigen::Quaternion<T> &q) {
+inline T QuaternionToHeading(const Eigen::Quaternion<T> &q) {
   return QuaternionToHeading(q.w(), q.x(), q.y(), q.z());
 }
 
@@ -86,7 +86,7 @@ inline double QuaternionToHeading(const Eigen::Quaternion<T> &q) {
  * @return Quaternion encoding rotation by given heading
  */
 template <typename T>
-inline Eigen::Quaternion<T> HeadingToQuaternion(const double heading) {
+inline Eigen::Quaternion<T> HeadingToQuaternion(T heading) {
   // Note that heading is zero at East and yaw is zero at North.
   EulerAnglesZXY<T> euler_angles(heading - M_PI_2);
   return euler_angles.ToQuaternion();

modules/common/math/quaternion_test.cc

@@ -36,7 +36,7 @@ TEST(QuaternionTest, QuaternionToHeading) {
                    QuaternionToHeading(v, 0.0, 0.0, v));  // Pointing to west.
 
   Eigen::Quaternionf q(1.0, 0.0, 0.0, 0.0);
-  EXPECT_DOUBLE_EQ(M_PI_2, QuaternionToHeading(q));  // Pointing to north.
+  EXPECT_FLOAT_EQ(M_PI_2, QuaternionToHeading(q));  // Pointing to north.
 
   const double headings[] = {-3.3, -2.2, -1.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7};
   for (double heading : headings) {

modules/map/hdmap/hdmap.cc

@@ -26,8 +26,8 @@ int HDMap::LoadMapFromFile(const std::string& map_filename) {
 }
 
 int HDMap::LoadMapFromProto(const Map& map_proto) {
-  AINFO << "Loading HDMap with header: "
-        << map_proto.header().ShortDebugString();
+  ADEBUG << "Loading HDMap with header: "
+         << map_proto.header().ShortDebugString();
   return impl_.LoadMapFromProto(map_proto);
 }
 

modules/planning/common/trajectory/discretized_trajectory.cc

@@ -162,6 +162,11 @@ const std::vector<TrajectoryPoint>& DiscretizedTrajectory::trajectory_points()
   return trajectory_points_;
 }
 
+void DiscretizedTrajectory::SetTrajectoryPoints(
+    const std::vector<common::TrajectoryPoint>& trajectory_points) {
+  trajectory_points_ = trajectory_points;
+}
+
 void DiscretizedTrajectory::Clear() { trajectory_points_.clear(); }
 
 }  // namespace planning

modules/planning/common/trajectory/discretized_trajectory.h

@@ -43,6 +43,9 @@ class DiscretizedTrajectory : public Trajectory {
   explicit DiscretizedTrajectory(
       const std::vector<common::TrajectoryPoint>& trajectory_points);
 
+  void SetTrajectoryPoints(
+      const std::vector<common::TrajectoryPoint>& trajectory_points);
+
   virtual ~DiscretizedTrajectory() = default;
 
   common::TrajectoryPoint Evaluate(const double relative_time) const override;

modules/planning/common/trajectory/trajectory_stitcher.cc

@@ -25,6 +25,7 @@
 
 #include "modules/common/configs/config_gflags.h"
 #include "modules/common/log.h"
+#include "modules/common/math/quaternion.h"
 #include "modules/common/util/util.h"
 #include "modules/planning/common/planning_gflags.h"
 
@@ -52,6 +53,44 @@ TrajectoryStitcher::ComputeReinitStitchingTrajectory(
   return std::vector<TrajectoryPoint>(1, init_point);
 }
 
+void TrajectoryStitcher::TransformLastPublishedTrajectory(
+    const double current_time, PublishableTrajectory* prev_trajectory) {
+  if (!prev_trajectory) {
+    return;
+  }
+  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();
+  if (prev_trajectory_size <= 1) {
+    return;
+  }
+  const double time_diff = current_time - prev_trajectory->header_time();
+  auto matched_point =
+      prev_trajectory->EvaluateUsingLinearApproximation(time_diff);
+  if (!matched_point.has_path_point()) {
+    return;
+  }
+  const double cos_theta = std::cos(-matched_point.path_point().theta());
+  const double sin_theta = std::sin(-matched_point.path_point().theta());
+  std::vector<TrajectoryPoint> transformed_points;
+  for (const auto& old_point : prev_trajectory->trajectory_points()) {
+    TrajectoryPoint point = old_point;
+    Eigen::Vector3d before_rotate(
+        old_point.path_point().x() - matched_point.path_point().x(),
+        old_point.path_point().y() - matched_point.path_point().y(),
+        old_point.path_point().z() - matched_point.path_point().z());
+    const double after_rotate_x =
+        before_rotate.x() * cos_theta - before_rotate.y() * sin_theta;
+    const double after_rotate_y =
+        before_rotate.x() * sin_theta + before_rotate.y() * cos_theta;
+    point.mutable_path_point()->set_x(after_rotate_x);
+    point.mutable_path_point()->set_y(after_rotate_y);
+    point.mutable_path_point()->set_z(before_rotate.z());
+    point.mutable_path_point()->set_theta(common::math::WrapAngle(
+        old_point.path_point().theta() - matched_point.path_point().theta()));
+    transformed_points.emplace_back(point);
+  }
+  prev_trajectory->SetTrajectoryPoints(transformed_points);
+}
+
 // only used in navigation mode
 std::vector<TrajectoryPoint> TrajectoryStitcher::CalculateInitPoint(
     const VehicleState& vehicle_state, const ReferenceLine& reference_line,
@@ -134,8 +173,6 @@ std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
       Vec2d(vehicle_state.x(), vehicle_state.y()));
   auto nearest_point = prev_trajectory->TrajectoryPointAt(nearest_point_index);
 
-  DCHECK(nearest_point.has_path_point());
-  DCHECK(matched_point.has_path_point());
   const double lat_diff =
       std::hypot(nearest_point.path_point().x() - vehicle_state.x(),
                  nearest_point.path_point().y() - vehicle_state.y());

modules/planning/common/trajectory/trajectory_stitcher.h

@@ -45,6 +45,9 @@ class TrajectoryStitcher {
       const common::VehicleState& vehicle_state,
       const ReferenceLine& reference_line, bool* is_replan);
 
+  static void TransformLastPublishedTrajectory(
+      const double planning_cycle_time, PublishableTrajectory* prev_trajectory);
+
   static std::vector<common::TrajectoryPoint> ComputeStitchingTrajectory(
       const common::VehicleState& vehicle_state, const double current_timestamp,
       const double planning_cycle_time,

modules/planning/planning.cc

@@ -276,28 +276,15 @@ void Planning::RunOnce() {
   }
 
   const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
-  bool is_replan = false;
-  std::vector<TrajectoryPoint> stitching_trajectory;
   if (FLAGS_use_navigation_mode) {
-    std::list<ReferenceLine> reference_lines;
-    std::list<hdmap::RouteSegments> segments;
-    if (!reference_line_provider_->GetReferenceLines(&reference_lines,
-                                                     &segments) ||
-        reference_lines.empty()) {
-      std::string msg("Reference line is not ready");
-      AERROR << msg;
-      not_ready->set_reason(msg);
-      status.Save(not_ready_pb.mutable_header()->mutable_status());
-      PublishPlanningPb(&not_ready_pb, start_timestamp);
-      return;
-    }
-    stitching_trajectory = TrajectoryStitcher::CalculateInitPoint(
-        vehicle_state, reference_lines.front(), &is_replan);
-  } else {
-    stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
-        vehicle_state, start_timestamp, planning_cycle_time,
-        last_publishable_trajectory_.get(), &is_replan);
+    TrajectoryStitcher::TransformLastPublishedTrajectory(
+        planning_cycle_time, last_publishable_trajectory_.get());
   }
+  bool is_replan = false;
+  std::vector<TrajectoryPoint> stitching_trajectory;
+  stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
+      vehicle_state, start_timestamp, planning_cycle_time,
+      last_publishable_trajectory_.get(), &is_replan);
 
   const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
   status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp,

modules/planning/tasks/traffic_decider/traffic_decider.cc

@@ -151,7 +151,7 @@ Status TrafficDecider::Execute(Frame *frame,
 
   for (const auto &rule_config : rule_configs_.config()) {
     if (!rule_config.enabled()) {
-      AINFO << "Rule " << rule_config.rule_id() << " not enabled";
+      ADEBUG << "Rule " << rule_config.rule_id() << " not enabled";
       continue;
     }
     auto rule = s_rule_factory.CreateObject(rule_config.rule_id(), rule_config);

scripts/filter_planning.sh

@@ -41,7 +41,9 @@ perfect_control_topic="$perception_topic  \
 
 planning_deps="$perfect_control_topic \
     or topic == '/apollo/canbus/chassis' \
-    or topic == '/apollo/localization/pose'"
+    or topic == '/apollo/localization/pose' \
+    or topic == '/apollo/navigation' \
+    or topic == '/apollo/relative_map'"
 
 planning_topic="topic == '/apollo/planning'"
 prediction_topic="topic == '/apollo/prediction'"