Autoware.Auto
motion_common.hpp
Go to the documentation of this file.
1 // Copyright 2019-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 #ifndef MOTION_COMMON__MOTION_COMMON_HPP_
15 #define MOTION_COMMON__MOTION_COMMON_HPP_
16 
18 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
19 #include <autoware_auto_system_msgs/msg/control_diagnostic.hpp>
20 #include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
21 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
22 #include <geometry_msgs/msg/transform_stamped.hpp>
23 
24 #include <tf2/LinearMath/Quaternion.h>
25 #include <tf2/utils.h>
26 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
28 
29 #include <algorithm>
30 #include <cmath>
31 
32 namespace motion
33 {
34 namespace motion_common
35 {
36 // Use same representation as message type
37 using Real = decltype(autoware_auto_planning_msgs::msg::TrajectoryPoint::longitudinal_velocity_mps);
39 using Diagnostic = autoware_auto_system_msgs::msg::ControlDiagnostic;
42 using Orientation = geometry_msgs::msg::Quaternion;
43 using Double = decltype(Orientation::x);
44 using Index = decltype(Trajectory::points)::size_type;
45 using Point = decltype(Trajectory::points)::value_type;
46 
48 MOTION_COMMON_PUBLIC bool is_past_point(const Point & state, const Point & pt) noexcept;
51 MOTION_COMMON_PUBLIC bool is_past_point(
52  const Point & state, const Point & current_pt, const Point & next_pt) noexcept;
54 MOTION_COMMON_PUBLIC
55 bool is_past_point(const Point & state, const Point & pt, Double nx, Double ny) noexcept;
56 
58 template<typename IsPastPointF>
60  const Trajectory & traj,
61  const State & state,
62  Index start_idx,
63  IsPastPointF is_past_point)
64 {
65  // Invariant: m_reference_trajectory.points.size > 0U
66  if (traj.points.empty()) {
67  return 0U;
68  }
69  auto next_idx = start_idx;
70  for (auto idx = start_idx; idx < traj.points.size() - 1U; ++idx) {
71  const auto current_pt = traj.points[idx];
72  const auto next_pt = traj.points[idx + 1U];
73 
74  // If I'm not past the next point, then I'm done
75  if (!is_past_point(state, current_pt, next_pt, traj)) {
76  break;
77  }
78  // Otherwise, update
79  next_idx = idx + 1U;
80  }
81  return next_idx;
82 }
83 
85 MOTION_COMMON_PUBLIC bool heading_ok(const Trajectory & traj);
86 
88 // Operations relating to algebraic manipulation of VehicleKinematicState and TrajectoryPoint
89 
91 MOTION_COMMON_PUBLIC void doTransform(
92  const Point & t_in,
93  Point & t_out,
94  const geometry_msgs::msg::TransformStamped & transform) noexcept;
96 MOTION_COMMON_PUBLIC void doTransform(
97  const State & t_in,
98  State & t_out,
99  const geometry_msgs::msg::TransformStamped & transform) noexcept;
100 
102 MOTION_COMMON_PUBLIC Double to_angle(Orientation orientation) noexcept;
103 
110 template<typename RealT>
111 Orientation from_angles(RealT roll, RealT pitch, RealT yaw) noexcept
112 {
113  static_assert(std::is_floating_point<RealT>::value, "angle must be floating point");
114  tf2::Quaternion quat;
115  quat.setRPY(roll, pitch, yaw);
116  return tf2::toMsg(quat);
117 }
118 
123 template<typename RealT>
124 Orientation from_angle(RealT angle) noexcept
125 {
126  return from_angles(RealT{}, RealT{}, angle);
127 }
128 
130 template<typename T>
131 T clamp(T val, T min, T max)
132 {
133  if (max < min) {
134  throw std::domain_error{"max < min"};
135  }
136  return std::min(max, std::max(min, val));
137 }
138 
140 template<typename T, typename RealT = double>
141 T interpolate(T a, T b, RealT t)
142 {
143  static_assert(std::is_floating_point<RealT>::value, "t must be floating point");
144  t = clamp(t, RealT{0.0}, RealT{1.0});
145  const auto del = b - a;
146  return static_cast<T>(t * static_cast<RealT>(del)) + a;
147 }
148 
150 MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t);
151 
153 template<typename SlerpF>
154 Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn)
155 {
156  Point ret{rosidl_runtime_cpp::MessageInitialization::ALL};
157  {
158  const auto dt0 = time_utils::from_message(a.time_from_start);
159  const auto dt1 = time_utils::from_message(b.time_from_start);
160  ret.time_from_start = time_utils::to_message(time_utils::interpolate(dt0, dt1, t));
161  }
162  ret.pose.position.x = interpolate(a.pose.position.x, b.pose.position.x, t);
163  ret.pose.position.y = interpolate(a.pose.position.y, b.pose.position.y, t);
164  ret.pose.orientation = slerp_fn(a.pose.orientation, b.pose.orientation, t);
165  ret.longitudinal_velocity_mps =
166  interpolate(a.longitudinal_velocity_mps, b.longitudinal_velocity_mps, t);
167  ret.lateral_velocity_mps = interpolate(a.lateral_velocity_mps, b.lateral_velocity_mps, t);
168  ret.acceleration_mps2 = interpolate(a.acceleration_mps2, b.acceleration_mps2, t);
169  ret.heading_rate_rps = interpolate(a.heading_rate_rps, b.heading_rate_rps, t);
170  ret.front_wheel_angle_rad = interpolate(a.front_wheel_angle_rad, b.front_wheel_angle_rad, t);
171  ret.rear_wheel_angle_rad = interpolate(a.rear_wheel_angle_rad, b.rear_wheel_angle_rad, t);
172 
173  return ret;
174 }
175 
177 MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t);
178 
180 template<typename SlerpF>
181 void sample(
182  const Trajectory & in,
183  Trajectory & out,
184  std::chrono::nanoseconds period,
185  SlerpF slerp_fn)
186 {
187  out.points.clear();
188  out.header = in.header;
189  if (in.points.empty()) {
190  return;
191  }
192  // Determine number of points
194  const auto last_time = from_message(in.points.back().time_from_start);
195  auto count_ = last_time / period;
196  // should round down
197  using SizeT = typename decltype(in.points)::size_type;
198  const auto count =
199  static_cast<SizeT>(std::min(count_, static_cast<decltype(count_)>(in.CAPACITY)));
200  out.points.reserve(count);
201  // Insert first point
202  out.points.push_back(in.points.front());
203  // Add remaining points
204  auto ref_duration = period;
205  auto after_current_ref_idx = SizeT{1};
206  for (auto idx = SizeT{1}; idx < count; ++idx) {
207  // Determine next reference index
208  for (auto jdx = after_current_ref_idx; jdx < in.points.size(); ++jdx) {
209  const auto & pt = in.points[jdx];
210  if (from_message(pt.time_from_start) > ref_duration) {
211  after_current_ref_idx = jdx;
212  break;
213  }
214  }
215  // Interpolate
216  {
217  const auto & curr_pt = in.points[after_current_ref_idx - 1U];
218  const auto & next_pt = in.points[after_current_ref_idx];
219  const auto dt = std::chrono::duration_cast<std::chrono::duration<Real>>(
220  from_message(next_pt.time_from_start) - from_message(curr_pt.time_from_start));
221  const auto dt_ = std::chrono::duration_cast<std::chrono::duration<Real>>(
222  ref_duration - from_message(curr_pt.time_from_start));
223  const auto t = dt_.count() / dt.count();
224  out.points.push_back(interpolate(curr_pt, next_pt, t, slerp_fn));
225  }
226  ref_duration += period;
227  }
228 }
229 
231 MOTION_COMMON_PUBLIC void sample(
232  const Trajectory & in,
233  Trajectory & out,
234  std::chrono::nanoseconds period);
235 
237 MOTION_COMMON_PUBLIC
238 void error(const Point & state, const Point & ref, Diagnostic & out) noexcept;
239 } // namespace motion_common
240 } // namespace motion
241 
242 namespace geometry_msgs
243 {
244 namespace msg
245 {
247 MOTION_COMMON_PUBLIC Quaternion operator+(Quaternion a, Quaternion b) noexcept;
249 MOTION_COMMON_PUBLIC Quaternion operator-(Quaternion a) noexcept;
251 MOTION_COMMON_PUBLIC Quaternion operator-(Quaternion a, Quaternion b) noexcept;
252 } // namespace msg
253 } // namespace geometry_msgs
254 #endif // MOTION_COMMON__MOTION_COMMON_HPP_
geometry_msgs::msg::operator+
MOTION_COMMON_PUBLIC Quaternion operator+(Quaternion a, Quaternion b) noexcept
Addition operator.
Definition: motion_common.cpp:164
motion::motion_common::Real
decltype(autoware_auto_planning_msgs::msg::TrajectoryPoint::longitudinal_velocity_mps) Real
Definition: motion_common.hpp:37
time_utils::to_message
TIME_UTILS_PUBLIC builtin_interfaces::msg::Time to_message(std::chrono::system_clock::time_point t)
Convert from std::chrono::time_point to time message.
Definition: time_utils.cpp:67
visibility_control.hpp
motion::motion_common::error
MOTION_COMMON_PUBLIC void error(const Point &state, const Point &ref, Diagnostic &out) noexcept
Diagnostic header stuff.
Definition: motion_common.cpp:135
autoware::motion::control::pure_pursuit::VehicleControlCommand
autoware_auto_vehicle_msgs::msg::VehicleControlCommand VehicleControlCommand
Definition: pure_pursuit.hpp:41
motion::motion_common::from_angles
Orientation from_angles(RealT roll, RealT pitch, RealT yaw) noexcept
Converts angles into a corresponding Orientation.
Definition: motion_common.hpp:111
motion::motion_common::clamp
T clamp(T val, T min, T max)
Standard clamp implementation.
Definition: motion_common.hpp:131
geometry_msgs
Definition: motion_common.hpp:242
motion::motion_common::Double
decltype(Orientation::x) Double
Definition: motion_common.hpp:43
autoware::motion::motion_common
Definition: trajectory_common.hpp:37
motion::motion_common::Command
autoware_auto_vehicle_msgs::msg::VehicleControlCommand Command
Definition: motion_common.hpp:38
motion::motion_common::from_angle
Orientation from_angle(RealT angle) noexcept
Converts a heading angle into a corresponding Orientation.
Definition: motion_common.hpp:124
motion::motion_common::slerp
MOTION_COMMON_PUBLIC Orientation slerp(const Orientation &a, const Orientation &b, const Real t)
Spherical linear interpolation.
Definition: motion_common.cpp:112
motion::motion_common::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: motion_common.hpp:40
motion::motion_common::sample
void sample(const Trajectory &in, Trajectory &out, std::chrono::nanoseconds period, SlerpF slerp_fn)
Sample a trajectory using interpolation; does not extrapolate.
Definition: motion_common.hpp:181
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
motion::motion_common::interpolate
T interpolate(T a, T b, RealT t)
Standard linear interpolation.
Definition: motion_common.hpp:141
motion::motion_common::update_reference_index
Index update_reference_index(const Trajectory &traj, const State &state, Index start_idx, IsPastPointF is_past_point)
Advance to the first trajectory point past state according to criterion is_past_point.
Definition: motion_common.hpp:59
geometry_msgs::msg::operator-
MOTION_COMMON_PUBLIC Quaternion operator-(Quaternion a) noexcept
Unary minus.
Definition: motion_common.cpp:172
motion::motion_common::heading_ok
MOTION_COMMON_PUBLIC bool heading_ok(const Trajectory &traj)
Check that all heading values in a trajectory are normalized 2D quaternions.
Definition: motion_common.cpp:68
motion::motion_testing_nodes::TransformStamped
geometry_msgs::msg::TransformStamped TransformStamped
Definition: motion_testing_publisher.hpp:37
time_utils::from_message
TIME_UTILS_PUBLIC std::chrono::system_clock::time_point from_message(builtin_interfaces::msg::Time t) noexcept
Convert from std::chrono::time_point from time message.
Definition: time_utils.cpp:80
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
motion::motion_common::Point
decltype(Trajectory::points)::value_type Point
Definition: motion_common.hpp:45
motion::motion_common::Diagnostic
autoware_auto_system_msgs::msg::ControlDiagnostic Diagnostic
Definition: motion_common.hpp:39
motion::motion_common::to_angle
MOTION_COMMON_PUBLIC Double to_angle(Orientation orientation) noexcept
Converts 3D quaternion to simple heading representation.
Definition: motion_common.cpp:106
motion::motion_common::Index
decltype(Trajectory::points)::size_type Index
Definition: motion_common.hpp:44
time_utils.hpp
motion
Definition: controller_base.hpp:31
motion::motion_common::Orientation
geometry_msgs::msg::Quaternion Orientation
Definition: motion_common.hpp:42
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
Point
geometry_msgs::msg::Point32 Point
Definition: cluster2d.cpp:28
motion::motion_common::doTransform
MOTION_COMMON_PUBLIC void doTransform(const Point &t_in, Point &t_out, const geometry_msgs::msg::TransformStamped &transform) noexcept
Apply transform to TrajectoryPoint.
Definition: motion_common.cpp:82
yaw
DifferentialState yaw
Definition: kinematic_bicycle.snippet.hpp:28
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
motion::motion_common::is_past_point
MOTION_COMMON_PUBLIC bool is_past_point(const Point &state, const Point &pt) noexcept
Check if a state is past a given trajectory point, assuming heading is correct.
Definition: motion_common.cpp:30