node3d.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2018 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.
 *****************************************************************************/
 
/*
* @file
*/
 
#pragma once
 
#include <memory>
#include <string>
#include <vector>
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/planning/planning_open_space/proto/planner_open_space_config.pb.h"
 
#include "modules/common/math/box2d.h"
 
namespace apollo {
namespace planning {
 
class Node3d {
 public:
  Node3d(const double x, const double y, const double phi);
  Node3d(const double x,
          const double y,
          const double phi,
          const std::vector<double>& XYbounds,
          const PlannerOpenSpaceConfig& open_space_conf);
  Node3d(const std::vector<double>& traversed_x,
          const std::vector<double>& traversed_y,
          const std::vector<double>& traversed_phi,
          const std::vector<double>& XYbounds,
          const PlannerOpenSpaceConfig& open_space_conf);
  virtual ~Node3d() = default;
  static apollo::common::math::Box2d GetBoundingBox(
          const common::VehicleParam& vehicle_param_,
          const double x,
          const double y,
          const double phi);
  double GetCost() const {
      return traj_cost_ + heuristic_cost_;
  }
  double GetTrajCost() const {
      return traj_cost_;
  }
  double GetHeuCost() const {
      return heuristic_cost_;
  }
  int GetGridX() const {
      return x_grid_;
  }
  int GetGridY() const {
      return y_grid_;
  }
  double GetX() const {
      return x_;
  }
  double GetY() const {
      return y_;
  }
  double GetPhi() const {
      return phi_;
  }
  bool operator==(const Node3d& right) const;
  const std::string& GetIndex() const {
      return index_;
  }
  size_t GetStepSize() const {
      return step_size_;
  }
  bool GetDirec() const {
      return direction_;
  }
  double GetSteer() const {
      return steering_;
  }
  std::shared_ptr<Node3d> GetPreNode() const {
      return pre_node_;
  }
  const std::vector<double>& GetXs() const {
      return traversed_x_;
  }
  const std::vector<double>& GetYs() const {
      return traversed_y_;
  }
  const std::vector<double>& GetPhis() const {
      return traversed_phi_;
  }
  const double& GetTravelDist() const {
      return travel_distance_;
  }
  void SetPre(std::shared_ptr<Node3d> pre_node) {
      pre_node_ = pre_node;
  }
  void SetDirec(bool direction) {
      direction_ = direction;
  }
  void SetTrajCost(double cost) {
      traj_cost_ = cost;
  }
  void SetHeuCost(double cost) {
      heuristic_cost_ = cost;
  }
  void SetSteer(double steering) {
      steering_ = steering;
  }
  void SetTravelDist(double dist) {
      travel_distance_ = dist;
  }
 
 private:
  static std::string ComputeStringIndex(
          int x_grid, int y_grid, int phi_grid);
 
 private:
  double x_ = 0.0;
  double y_ = 0.0;
  double phi_ = 0.0;
  size_t step_size_ = 1;
  std::vector<double> traversed_x_;
  std::vector<double> traversed_y_;
  std::vector<double> traversed_phi_;
  int x_grid_ = 0;
  int y_grid_ = 0;
  int phi_grid_ = 0;
  std::string index_;
  double traj_cost_ = 0.0;
  double heuristic_cost_ = 0.0;
  double cost_ = 0.0;
  std::shared_ptr<Node3d> pre_node_ = nullptr;
  double steering_ = 0.0;
  // true for moving forward and false for moving backward
  bool direction_ = true;
  // travel distance in direction_
  double travel_distance_ = 0.0;
 
  // park generic
 public:
  Node3d(const double x,
          const double y,
          const double phi,
          const std::vector<double>& XYbounds,
          const WarmStartConfig& warm_start_conf);
 
  Node3d(const std::vector<double>& traversed_x,
          const std::vector<double>& traversed_y,
          const std::vector<double>& traversed_phi,
          const std::vector<double>& XYbounds,
          const WarmStartConfig& warm_start_conf);
};
 
}  // namespace planning
}  // namespace apollo