scenario_simulator_v2 C++ API
route_planner.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
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 BEHAVIOR_TREE_PLUGIN__ROUTE_PLANNER_HPP_
16 #define BEHAVIOR_TREE_PLUGIN__ROUTE_PLANNER_HPP_
17 
18 #include <deque>
19 #include <memory>
22 #include <vector>
23 
24 namespace traffic_simulator
25 {
27 {
28 public:
29  explicit RoutePlanner(
30  const traffic_simulator::RoutingGraphType &, const std::shared_ptr<hdmap_utils::HdMapUtils> &);
31 
32  auto getWholeRouteLanelets() const -> lanelet::Ids { return whole_lanelet_ids_; }
33  auto getRouteLanelets(const CanonicalizedLaneletPose & entity_lanelet_pose, double horizon = 100)
34  -> lanelet::Ids;
35 
36  auto setWaypoints(const std::vector<CanonicalizedLaneletPose> & waypoints) -> void;
37 
38  auto cancelRoute() -> void;
39  auto getGoalPoses() const -> std::vector<CanonicalizedLaneletPose>;
40  auto getGoalPosesInWorldFrame() const -> std::vector<geometry_msgs::msg::Pose>;
41 
42 private:
43  auto cancelWaypoint(const CanonicalizedLaneletPose & entity_lanelet_pose) -> void;
44 
45  auto updateRoute(const CanonicalizedLaneletPose & entity_lanelet_pose) -> void;
46 
47  std::optional<lanelet::Ids> route_;
48  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
49  const traffic_simulator::RoutingGraphType routing_graph_type_;
50 
51  /*
52  What we need is a queue, but we need to be able to iterate over the
53  elements for getGoalPoses, so we use std::deque instead of std::queue
54  which is not iterable.
55  */
56  std::deque<traffic_simulator::CanonicalizedLaneletPose> waypoint_queue_;
57  lanelet::Ids whole_lanelet_ids_;
58 };
59 } // namespace traffic_simulator
60 
61 #endif // BEHAVIOR_TREE_PLUGIN__ROUTE_PLANNER_HPP_
Definition: route_planner.hpp:27
auto getGoalPoses() const -> std::vector< CanonicalizedLaneletPose >
Definition: route_planner.cpp:96
auto getGoalPosesInWorldFrame() const -> std::vector< geometry_msgs::msg::Pose >
Definition: route_planner.cpp:87
auto getRouteLanelets(const CanonicalizedLaneletPose &entity_lanelet_pose, double horizon=100) -> lanelet::Ids
Definition: route_planner.cpp:41
auto cancelRoute() -> void
Definition: route_planner.cpp:65
auto setWaypoints(const std::vector< CanonicalizedLaneletPose > &waypoints) -> void
Definition: route_planner.cpp:26
auto getWholeRouteLanelets() const -> lanelet::Ids
Definition: route_planner.hpp:32
RoutePlanner(const traffic_simulator::RoutingGraphType &, const std::shared_ptr< hdmap_utils::HdMapUtils > &)
Definition: route_planner.cpp:19
Definition: lanelet_pose.hpp:28
Definition: cache.hpp:28
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_map.hpp:30
RoutingGraphType
Definition: routing_graph_type.hpp:24
Definition: api.hpp:32