Autoware.Auto
motion_testing.hpp
Go to the documentation of this file.
1 // Copyright 2019 Christopher Ho
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_TESTING__MOTION_TESTING_HPP_
15 #define MOTION_TESTING__MOTION_TESTING_HPP_
16 
18 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
19 #include <autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp>
20 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
21 
22 #include <chrono>
23 #include <random>
24 
25 namespace motion
26 {
27 namespace motion_testing
28 {
29 using Generator = std::mt19937;
33 using Index = decltype(Trajectory::points)::size_type;
34 using Real = decltype(Point::longitudinal_velocity_mps);
35 // TODO(c.ho) Make these more modular
36 
38 MOTION_TESTING_PUBLIC State make_state(
39  Real x0,
40  Real y0,
41  Real heading,
42  Real v0,
43  Real a0,
44  Real turn_rate,
45  std::chrono::system_clock::time_point t);
46 
49 MOTION_TESTING_PUBLIC State generate_state(Generator & gen);
50 
54 MOTION_TESTING_PUBLIC Trajectory generate_trajectory(const State & start_state, Generator & gen);
55 
60 MOTION_TESTING_PUBLIC
61 Trajectory constant_trajectory(const State & start_state, std::chrono::nanoseconds dt);
63 
64 MOTION_TESTING_PUBLIC
65 Trajectory bad_heading_trajectory(const State & start_state, std::chrono::nanoseconds dt);
67 
68 MOTION_TESTING_PUBLIC Trajectory constant_velocity_trajectory(
69  float x0,
70  float y0,
71  float heading,
72  float v0,
73  std::chrono::nanoseconds dt);
75 MOTION_TESTING_PUBLIC Trajectory constant_acceleration_trajectory(
76  float x0,
77  float y0,
78  float heading,
79  float v0,
80  float a0,
81  std::chrono::nanoseconds dt);
84  float x0,
85  float y0,
86  float heading,
87  float v0,
88  float turn_rate,
89  std::chrono::nanoseconds dt);
92  float x0,
93  float y0,
94  float heading,
95  float v0,
96  float a0,
97  float turn_rate,
98  std::chrono::nanoseconds dt);
99 
103 MOTION_TESTING_PUBLIC void next_state(
104  const Trajectory & trajectory,
105  State & state,
106  uint32_t hint,
107  Generator * gen = nullptr); // TODO(c.ho) std::optional NOLINT
108 // TODO(c.ho) version that takes control commands
109 
113 MOTION_TESTING_PUBLIC
115  const Trajectory & trajectory,
116  const Point & target,
117  Real heading_tolerance = Real{0.006F});
118 
122 MOTION_TESTING_PUBLIC
123 Index dynamically_feasible(const Trajectory & trajectory, Real tolerance = 0.05F);
124 } // namespace motion_testing
125 } // namespace motion
126 
127 #endif // MOTION_TESTING__MOTION_TESTING_HPP_
motion::motion_testing::constant_acceleration_turn_rate_trajectory
MOTION_TESTING_PUBLIC Trajectory constant_acceleration_turn_rate_trajectory(float x0, float y0, float heading, float v0, float a0, float turn_rate, std::chrono::nanoseconds dt)
Generates a constant acceleration and constant turn rate trajectory.
motion::motion_testing::bad_heading_trajectory
MOTION_TESTING_PUBLIC Trajectory bad_heading_trajectory(const State &start_state, std::chrono::nanoseconds dt)
Generates a constant velocity trajectory.
Definition: motion_testing.cpp:124
motion::motion_testing::progresses_towards_target
MOTION_TESTING_PUBLIC Index progresses_towards_target(const Trajectory &trajectory, const Point &target, Real heading_tolerance=Real{0.006F})
Definition: motion_testing.cpp:219
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
motion::motion_testing::Generator
std::mt19937 Generator
Definition: motion_testing.hpp:29
motion::motion_testing::Point
autoware_auto_planning_msgs::msg::TrajectoryPoint Point
Definition: motion_testing.hpp:31
motion::motion_testing::next_state
MOTION_TESTING_PUBLIC void next_state(const Trajectory &trajectory, State &state, uint32_t hint, Generator *gen=nullptr)
Definition: motion_testing.cpp:206
motion::motion_testing::constant_velocity_trajectory
MOTION_TESTING_PUBLIC Trajectory constant_velocity_trajectory(float x0, float y0, float heading, float v0, std::chrono::nanoseconds dt)
Generates a constant velocity trajectory with invalid heading values.
motion::motion_testing::make_state
MOTION_TESTING_PUBLIC State make_state(Real x0, Real y0, Real heading, Real v0, Real a0, Real turn_rate, std::chrono::system_clock::time_point t)
Makes a state, intended to make message generation more terse.
Definition: motion_testing.cpp:28
motion::motion_testing::constant_velocity_turn_rate_trajectory
MOTION_TESTING_PUBLIC Trajectory constant_velocity_turn_rate_trajectory(float x0, float y0, float heading, float v0, float turn_rate, std::chrono::nanoseconds dt)
Generates a constant velocity and constant turn rate trajectory.
motion::motion_testing::Index
decltype(Trajectory::points)::size_type Index
Definition: motion_testing.hpp:33
motion::motion_testing::Real
decltype(Point::longitudinal_velocity_mps) Real
Definition: motion_testing.hpp:34
motion::motion_testing::constant_trajectory
MOTION_TESTING_PUBLIC Trajectory constant_trajectory(const State &start_state, std::chrono::nanoseconds dt)
Generate a trajectory given the start state, assuming the highest derivatives are held constant....
Definition: motion_testing.cpp:86
motion::motion_testing::dynamically_feasible
MOTION_TESTING_PUBLIC Index dynamically_feasible(const Trajectory &trajectory, Real tolerance=0.05F)
Definition: motion_testing.cpp:251
motion::motion_testing::constant_acceleration_trajectory
MOTION_TESTING_PUBLIC Trajectory constant_acceleration_trajectory(float x0, float y0, float heading, float v0, float a0, std::chrono::nanoseconds dt)
Generates a constant acceleration trajectory.
motion::motion_testing::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: motion_testing.hpp:30
motion
Definition: controller_base.hpp:31
motion::motion_testing::generate_trajectory
MOTION_TESTING_PUBLIC Trajectory generate_trajectory(const State &start_state, Generator &gen)
Generates a trajectory assuming the starting state, a bicycle model, and additive noise applied to XX...
Definition: motion_testing.cpp:77
motion::motion_testing::generate_state
MOTION_TESTING_PUBLIC State generate_state(Generator &gen)
Generates a state from a normal distribution with the following bounds: TODO(c.ho)
Definition: motion_testing.cpp:54
motion::motion_testing::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_testing.hpp:32
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
visibility_control.hpp