pure_pursuit.hpp

Go to the documentation of this file.

// Copyright 2019 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef PURE_PURSUIT__PURE_PURSUIT_HPP_
#define PURE_PURSUIT__PURE_PURSUIT_HPP_

#include <autoware_auto_msgs/msg/trajectory.hpp>
#include <autoware_auto_msgs/msg/trajectory_point.hpp>
#include <autoware_auto_msgs/msg/vehicle_kinematic_state.hpp>
#include <autoware_auto_msgs/msg/vehicle_control_command.hpp>
#include <controller_common/controller_base.hpp>
#include <utility>
#include "pure_pursuit/config.hpp"

namespace autoware
{
namespace motion
{
namespace control
{
namespace pure_pursuit
{
using Trajectory = autoware_auto_msgs::msg::Trajectory;
using TrajectoryPoint = autoware_auto_msgs::msg::TrajectoryPoint;
using TrajectoryPointStamped = autoware_auto_msgs::msg::VehicleKinematicState;
using ControllerDiagnostic = autoware_auto_msgs::msg::ControlDiagnostic;
using VehicleControlCommand = autoware_auto_msgs::msg::VehicleControlCommand;

class PURE_PURSUIT_PUBLIC PurePursuit
  : public ::motion::control::controller_common::ControllerBase
{
public:
  explicit PurePursuit(const Config & cfg);

protected:
  VehicleControlCommand compute_command_impl(const TrajectoryPointStamped & state) override;

private:
  PURE_PURSUIT_LOCAL void compute_errors(const TrajectoryPoint & current_point);
  PURE_PURSUIT_LOCAL void compute_lookahead_distance(const float32_t current_velocity);
  PURE_PURSUIT_LOCAL bool8_t in_traveling_direction(
    const TrajectoryPoint & current_point,
    const TrajectoryPoint & point) const;
  PURE_PURSUIT_LOCAL void compute_interpolate_target_point(
    const TrajectoryPoint & current_point,
    const TrajectoryPoint & target_point,
    const uint32_t idx);
  PURE_PURSUIT_LOCAL bool8_t compute_target_point(const TrajectoryPoint & current_point);
  PURE_PURSUIT_LOCAL static float32_t compute_points_distance_squared(
    const TrajectoryPoint & point1,
    const TrajectoryPoint & point2);
  PURE_PURSUIT_LOCAL std::pair<float32_t, float32_t> compute_relative_xy_offset(
    const TrajectoryPoint & current,
    const TrajectoryPoint & target) const;
  PURE_PURSUIT_LOCAL float32_t compute_steering_rad(const TrajectoryPoint & current_point);
  PURE_PURSUIT_LOCAL float32_t compute_command_accel_mps(
    const TrajectoryPoint & current_point,
    const bool8_t is_emergency) const;

  float32_t m_lookahead_distance;
  TrajectoryPoint m_target_point;
  VehicleControlCommand m_command;
  Config m_config;

  bool8_t m_is_traj_update;
  uint32_t m_reference_idx;
  uint32_t m_iterations;
};  // class PurePursuit
}  // namespace pure_pursuit
}  // namespace control
}  // namespace motion
}  // namespace autoware

#endif  // PURE_PURSUIT__PURE_PURSUIT_HPP_