trajectory_analyzer.h

浏览该文件的文档.

/******************************************************************************
 * 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/common_msgs/basic_msgs/pnc_point.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
 
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.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;
 
 protected:
  std::vector<common::TrajectoryPoint> trajectory_points_;
 
  double header_time_ = 0.0;
  unsigned int seq_num_ = 0;
 
 private:
  common::PathPoint FindMinDistancePoint(const common::TrajectoryPoint &p0,
                                         const common::TrajectoryPoint &p1,
                                         const double x, const double y) const;
};
 
}  // namespace control
}  // namespace apollo