smooth_stop.hpp

Go to the documentation of this file.

// Copyright 2021 Tier IV, Inc.
//
// 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.
 
#ifndef TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_
#define TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_
 
#include <algorithm>
#include <cmath>
#include <experimental/optional>  // NOLINT
#include <limits>
#include <utility>
#include <vector>
 
#include "common/types.hpp"
#include "rclcpp/rclcpp.hpp"
#include "trajectory_follower/visibility_control.hpp"
 
 
namespace autoware
{
namespace motion
{
namespace control
{
namespace trajectory_follower
{
using autoware::common::types::float64_t;
using autoware::common::types::bool8_t;
class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop
{
public:
  void init(const float64_t pred_vel_in_target, const float64_t pred_stop_dist);
 
  void setParams(
    float64_t max_strong_acc, float64_t min_strong_acc, float64_t weak_acc, float64_t weak_stop_acc,
    float64_t strong_stop_acc, float64_t min_fast_vel, float64_t min_running_vel,
    float64_t min_running_acc,
    float64_t weak_stop_time, float64_t weak_stop_dist, float64_t strong_stop_dist);
 
  std::experimental::optional<float64_t> calcTimeToStop(
    const std::vector<std::pair<rclcpp::Time, float64_t>> & vel_hist) const;
 
  float64_t calculate(
    const float64_t stop_dist, const float64_t current_vel, const float64_t current_acc,
    const std::vector<std::pair<rclcpp::Time, float64_t>> & vel_hist, const float64_t delay_time);
 
private:
  struct Params
  {
    float64_t max_strong_acc;
    float64_t min_strong_acc;
    float64_t weak_acc;
    float64_t weak_stop_acc;
    float64_t strong_stop_acc;
 
    float64_t min_fast_vel;
    float64_t min_running_vel;
    float64_t min_running_acc;
    float64_t weak_stop_time;
 
    float64_t weak_stop_dist;
    float64_t strong_stop_dist;
  };
  Params m_params;
 
  float64_t m_strong_acc;
  rclcpp::Time m_weak_acc_time;
  bool8_t m_is_set_params = false;
};
}  // namespace trajectory_follower
}  // namespace control
}  // namespace motion
}  // namespace autoware
 
#endif  // TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_