Autoware.Auto
trajectory_common.hpp
Go to the documentation of this file.
1 // Copyright 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 MOTION_COMMON__TRAJECTORY_COMMON_HPP_
16 #define MOTION_COMMON__TRAJECTORY_COMMON_HPP_
17 
18 #include <experimental/optional>
19 #include <limits>
20 #include <stdexcept>
21 #include <vector>
22 
23 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
24 #include "common/types.hpp"
25 #include "eigen3/Eigen/Core"
26 #include "geometry/common_2d.hpp"
27 #include "geometry_msgs/msg/pose.hpp"
28 #include "geometry_msgs/msg/point.hpp"
31 #include "tf2/utils.h"
32 
33 namespace autoware
34 {
35 namespace motion
36 {
37 namespace motion_common
38 {
40 typedef decltype (autoware_auto_planning_msgs::msg::Trajectory::points) Points;
42 typedef Eigen::Matrix<float64_t, 3, 1> Vector3f;
43 
48 MOTION_COMMON_PUBLIC void validateNonEmpty(const Points & points);
49 
56 MOTION_COMMON_PUBLIC float64_t calcYawDeviation(
57  const float64_t & base_yaw,
58  const float64_t & target_yaw);
59 
68 MOTION_COMMON_PUBLIC std::experimental::optional<size_t> searchZeroVelocityIndex(
69  const Points & points, const size_t src_idx, const size_t dst_idx,
70  const float64_t epsilon = 1e-3);
71 
76 MOTION_COMMON_PUBLIC std::experimental::optional<size_t> searchZeroVelocityIndex(
77  const Points & points);
78 
85 MOTION_COMMON_PUBLIC size_t findNearestIndex(
86  const Points & points,
87  const geometry_msgs::msg::Point & point);
88 
97 MOTION_COMMON_PUBLIC std::experimental::optional<size_t> findNearestIndex(
98  const Points & points, const geometry_msgs::msg::Pose & pose,
99  const float64_t max_dist = std::numeric_limits<float64_t>::max(),
100  const float64_t max_yaw = std::numeric_limits<float64_t>::max());
101 
110 MOTION_COMMON_PUBLIC float64_t calcLongitudinalOffsetToSegment(
111  const Points & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target);
112 
121 MOTION_COMMON_PUBLIC size_t findNearestSegmentIndex(
122  const Points & points,
123  const geometry_msgs::msg::Point & point);
124 
132 MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
133  const Points & points, const size_t src_idx,
134  const size_t dst_idx);
135 
143 MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
144  const Points & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx);
145 
153 MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(
154  const Points & points, const geometry_msgs::msg::Point & src_point,
155  const geometry_msgs::msg::Point & dst_point);
156 
163 MOTION_COMMON_PUBLIC float64_t calcLongitudinalDeviation(
164  const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point);
165 
166 } // namespace motion_common
167 } // namespace motion
168 } // namespace autoware
169 
170 #endif // MOTION_COMMON__TRAJECTORY_COMMON_HPP_
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
autoware::motion::motion_common::findNearestSegmentIndex
MOTION_COMMON_PUBLIC size_t findNearestSegmentIndex(const Points &points, const geometry_msgs::msg::Point &point)
find nearest segment index to point segment is straight path between two continuous points of traject...
Definition: trajectory_common.cpp:137
autoware::motion::motion_common::calcYawDeviation
MOTION_COMMON_PUBLIC float64_t calcYawDeviation(const float64_t &base_yaw, const float64_t &target_yaw)
calculate the yaw deviation between two angles
Definition: trajectory_common.cpp:36
autoware::motion::motion_common::searchZeroVelocityIndex
MOTION_COMMON_PUBLIC std::experimental::optional< size_t > searchZeroVelocityIndex(const Points &points, const size_t src_idx, const size_t dst_idx, const float64_t epsilon=1e-3)
search first index with a velocity of zero in the given range of points
Definition: trajectory_common.cpp:41
types.hpp
This file includes common type definition.
autoware::motion::motion_common
Definition: trajectory_common.hpp:37
common_2d.hpp
This file includes common functionality for 2D geometry, such as dot products.
angle_utils.hpp
autoware::motion::motion_common::calcLongitudinalOffsetToSegment
MOTION_COMMON_PUBLIC float64_t calcLongitudinalOffsetToSegment(const Points &points, const size_t seg_idx, const geometry_msgs::msg::Point &p_target)
calculate length along trajectory from seg_idx point to nearest point to p_target on trajectory If se...
Definition: trajectory_common.cpp:114
autoware::motion::motion_common::calcSignedArcLength
MOTION_COMMON_PUBLIC float64_t calcSignedArcLength(const Points &points, const size_t src_idx, const size_t dst_idx)
calculate arc length along points
Definition: trajectory_common.cpp:156
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::motion::motion_common::findNearestIndex
MOTION_COMMON_PUBLIC size_t findNearestIndex(const Points &points, const geometry_msgs::msg::Point &point)
search the index of the point nearest to the given target
Definition: trajectory_common.cpp:60
autoware::motion::motion_common::Points
decltype(autoware_auto_planning_msgs::msg::Trajectory::points) typedef Points
Definition: trajectory_common.hpp:40
autoware::motion::motion_common::calcLongitudinalDeviation
MOTION_COMMON_PUBLIC float64_t calcLongitudinalDeviation(const geometry_msgs::msg::Pose &base_pose, const geometry_msgs::msg::Point &target_point)
calculate longitudinal deviation of a point relative to a pose
Definition: trajectory_common.cpp:203
motion_common.hpp
autoware::motion::motion_common::Point
autoware_auto_planning_msgs::msg::TrajectoryPoint Point
Definition: trajectory_common.hpp:39
motion
Definition: controller_base.hpp:31
Point
geometry_msgs::msg::Point32 Point
Definition: cluster2d.cpp:28
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
autoware::motion::motion_common::Vector3f
Eigen::Matrix< float64_t, 3, 1 > Vector3f
Definition: trajectory_common.hpp:42
autoware::motion::motion_common::validateNonEmpty
MOTION_COMMON_PUBLIC void validateNonEmpty(const Points &points)
throws an exception if the given list of points is empty
Definition: trajectory_common.cpp:29