Autoware.Auto
trajectory_manager.hpp
Go to the documentation of this file.
1 // Copyright 2020 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 
18 
19 #ifndef BEHAVIOR_PLANNER__TRAJECTORY_MANAGER_HPP_
20 #define BEHAVIOR_PLANNER__TRAJECTORY_MANAGER_HPP_
21 
23 
24 #include <common/types.hpp>
25 #include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
26 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
27 #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
28 
29 #include <iostream>
30 #include <vector>
31 
32 namespace autoware
33 {
35 namespace behavior_planner
36 {
37 
41 
44 
45 struct BEHAVIOR_PLANNER_PUBLIC PlannerConfig
46 {
53 };
54 
55 
56 // TODO(mitsudome-r): remove this class after making controller
57 // able to handle whole trajectory with turning points
60 class BEHAVIOR_PLANNER_PUBLIC TrajectoryManager
61 {
62 public:
63  explicit TrajectoryManager(const PlannerConfig & config);
64 
65  void clear_trajectory();
66  void set_trajectory(const Trajectory & trajectory);
67  Trajectory get_trajectory(const State & state);
68  size_t get_remaining_length(const State & state);
69  bool8_t is_trajectory_ready();
70  bool8_t has_arrived_subgoal(const State & state);
71 
72 private:
73  void set_sub_trajectories();
74  std::size_t get_closest_state(const State & state, const Trajectory & trajectory);
75  Trajectory crop_from_current_state(const Trajectory & trajectory, const State & state);
76  void set_time_from_start(Trajectory * trajectory);
77 
78  // parameters
79  const PlannerConfig m_config;
80  Trajectory m_trajectory;
81  std::vector<Trajectory> m_sub_trajectories;
82  size_t m_selected_trajectory;
83 };
84 
85 } // namespace behavior_planner
86 } // namespace autoware
87 
88 #endif // BEHAVIOR_PLANNER__TRAJECTORY_MANAGER_HPP_
autoware::behavior_planner::PlannerConfig::subroute_goal_offset_lane2parking
float32_t subroute_goal_offset_lane2parking
Definition: trajectory_manager.hpp:50
motion::motion_testing_nodes::TrajectoryPoint
autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint
Definition: motion_testing_publisher.hpp:36
types.hpp
This file includes common type definition.
behavior_planner
Definition: behavior_planner.launch.py:1
autoware::behavior_planner::PlannerConfig::cg_to_vehicle_center
float32_t cg_to_vehicle_center
Definition: trajectory_manager.hpp:52
autoware::behavior_planner::PlannerConfig::subroute_goal_offset_parking2lane
float32_t subroute_goal_offset_parking2lane
Definition: trajectory_manager.hpp:51
autoware::behavior_planner::PlannerConfig::heading_weight
float32_t heading_weight
Definition: trajectory_manager.hpp:49
autoware::behavior_planner::TrajectoryManager
Class that splits trajectory at gear changing points and publish them separately.
Definition: trajectory_manager.hpp:60
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::behavior_planner::PlannerConfig::goal_distance_thresh
float32_t goal_distance_thresh
Definition: trajectory_manager.hpp:47
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::behavior_planner::PlannerConfig
Definition: trajectory_manager.hpp:45
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
motion::motion_common::Trajectory
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
visibility_control.hpp
autoware::behavior_planner::State
autoware_auto_vehicle_msgs::msg::VehicleKinematicState State
Definition: behavior_planner.hpp:57
autoware::trajectory_spoofer::VehicleKinematicState
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
autoware::behavior_planner::PlannerConfig::stop_velocity_thresh
float32_t stop_velocity_thresh
Definition: trajectory_manager.hpp:48