Autoware.Auto
interpolate.hpp
Go to the documentation of this file.
1 // Copyright 2018-2021 The Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
16 #define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
17 
18 #include <cmath>
19 #include <iostream>
20 #include <vector>
21 
22 #include "common/types.hpp"
24 
25 namespace autoware
26 {
27 namespace motion
28 {
29 namespace control
30 {
31 namespace trajectory_follower
32 {
42 TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate(
43  const std::vector<float64_t> & base_index, const std::vector<float64_t> & base_value,
44  const std::vector<float64_t> & return_index, std::vector<float64_t> & return_value);
52 TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate(
53  const std::vector<float64_t> & base_index, const std::vector<float64_t> & base_value,
54  const float64_t & return_index, float64_t & return_value);
55 
59 class TRAJECTORY_FOLLOWER_PUBLIC SplineInterpolate
60 {
61  bool8_t initialized_ = false;
62  std::vector<float64_t> m_a;
63  std::vector<float64_t> m_b;
64  std::vector<float64_t> m_c;
65  std::vector<float64_t> m_d;
66 
67 public:
71  SplineInterpolate() = default;
76  explicit SplineInterpolate(const std::vector<float64_t> & x);
81  void generateSpline(const std::vector<float64_t> & x);
87  float64_t getValue(const float64_t & s);
96  const std::vector<float64_t> & base_index, const std::vector<float64_t> & base_value,
97  const std::vector<float64_t> & return_index, std::vector<float64_t> & return_value);
98  void getValueVector(const std::vector<float64_t> & s_v, std::vector<float64_t> & value_v);
99 };
100 } // namespace trajectory_follower
101 } // namespace control
102 } // namespace motion
103 } // namespace autoware
104 #endif // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP__
visibility_control.hpp
types.hpp
This file includes common type definition.
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
time_utils::interpolate
TIME_UTILS_PUBLIC std::chrono::nanoseconds interpolate(std::chrono::nanoseconds a, std::chrono::nanoseconds b, float t) noexcept
Standard interpolation.
Definition: time_utils.cpp:96
autoware::motion::control::trajectory_follower::linearInterpolate
TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate(const std::vector< float64_t > &base_index, const std::vector< float64_t > &base_value, const std::vector< float64_t > &return_index, std::vector< float64_t > &return_value)
linearly interpolate the given values assuming a base indexing and a new desired indexing
Definition: interpolate.cpp:80
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::motion::control::trajectory_follower
Definition: debug_values.hpp:29
autoware::motion::control::trajectory_follower::SplineInterpolate
Class for spline interpolation.
Definition: interpolate.hpp:59
motion
Definition: controller_base.hpp:31
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47