trajectory_analyzer.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <vector>

#include "modules/planning/proto/planning.pb.h"

#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"

namespace apollo {
namespace control {

class TrajectoryAnalyzer {
 public:
  TrajectoryAnalyzer() = default;

  TrajectoryAnalyzer(
      const planning::ADCTrajectory *planning_published_trajectory);

  ~TrajectoryAnalyzer() = default;

  unsigned int seq_num() { return seq_num_; }

  common::TrajectoryPoint QueryNearestPointByAbsoluteTime(const double t) const;

  common::TrajectoryPoint QueryNearestPointByRelativeTime(const double t) const;

  common::TrajectoryPoint QueryNearestPointByPosition(const double x,
                                                      const double y) const;

  common::PathPoint QueryMatchedPathPoint(const double x, const double y) const;

  void ToTrajectoryFrame(const double x, const double y, const double theta,
                         const double v, const common::PathPoint &matched_point,
                         double *ptr_s, double *ptr_s_dot, double *ptr_d,
                         double *ptr_d_dot) const;

  void TrajectoryTransformToCOM(const double rear_to_com_distance);

  common::math::Vec2d ComputeCOMPosition(
      const double rear_to_com_distance,
      const common::PathPoint &path_point) const;

  const std::vector<common::TrajectoryPoint> &trajectory_points() const;

 private:
  common::PathPoint FindMinDistancePoint(const common::TrajectoryPoint &p0,
                                         const common::TrajectoryPoint &p1,
                                         const double x, const double y) const;

  std::vector<common::TrajectoryPoint> trajectory_points_;

  double header_time_ = 0.0;
  unsigned int seq_num_ = 0;
};

}  // namespace control
}  // namespace apollo