scenario_simulator_v2 C++ API
lanelet_pose.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 TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
16 #define TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
17 
19 
20 namespace traffic_simulator
21 {
23 
24 inline namespace lanelet_pose
25 {
27 {
28 public:
29  explicit CanonicalizedLaneletPose(
30  const LaneletPose & maybe_non_canonicalized_lanelet_pose,
31  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils);
32  explicit CanonicalizedLaneletPose(
33  const LaneletPose & maybe_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets,
34  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils);
38  explicit operator LaneletPose() const noexcept { return lanelet_pose_; }
39  explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; }
40  auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; }
41  auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; }
43  LaneletPose from, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
44  const traffic_simulator::RoutingConfiguration & routing_configuration =
46  static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void
47  {
48  consider_pose_by_road_slope_ = consider_pose_by_road_slope;
49  }
50  static auto getConsiderPoseByRoadSlope() -> bool { return consider_pose_by_road_slope_; }
51 
57 #define DEFINE_COMPARISON_OPERATOR(OPERATOR) \
58  bool operator OPERATOR(const CanonicalizedLaneletPose & rhs) const \
59  { \
60  if ( \
61  static_cast<LaneletPose>(*this).lanelet_id == static_cast<LaneletPose>(rhs).lanelet_id && \
62  static_cast<LaneletPose>(*this).s OPERATOR static_cast<LaneletPose>(rhs).s) { \
63  return true; \
64  } \
65  return false; \
66  }
67 
72 #undef DEFINE_COMPARISON_OPERATOR
73 
74 private:
75  auto adjustOrientationAndOzPosition(const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils)
76  -> void;
77  auto canonicalize(
78  const LaneletPose & may_non_canonicalized_lanelet_pose,
79  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils) -> LaneletPose;
80  auto canonicalize(
81  const LaneletPose & may_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets,
82  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils) -> LaneletPose;
84  std::vector<LaneletPose> lanelet_poses_;
85  geometry_msgs::msg::Pose map_pose_;
86  inline static bool consider_pose_by_road_slope_{false};
87 };
88 } // namespace lanelet_pose
89 
91 auto isSameLaneletId(const CanonicalizedLaneletPose &, const lanelet::Id lanelet_id) -> bool;
92 } // namespace traffic_simulator
93 
94 #endif // TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
Definition: lanelet_pose.hpp:27
LaneletPose lanelet_pose_
Definition: lanelet_pose.hpp:83
static bool consider_pose_by_road_slope_
Definition: lanelet_pose.hpp:86
CanonicalizedLaneletPose(const LaneletPose &maybe_non_canonicalized_lanelet_pose, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils)
Definition: lanelet_pose.cpp:27
auto getAlternativeLaneletPoseBaseOnShortestRouteFrom(LaneletPose from, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils, const traffic_simulator::RoutingConfiguration &routing_configuration=traffic_simulator::RoutingConfiguration()) const -> std::optional< LaneletPose >
Definition: lanelet_pose.cpp:112
auto hasAlternativeLaneletPose() const -> bool
Definition: lanelet_pose.hpp:41
static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void
Definition: lanelet_pose.hpp:46
auto canonicalize(const LaneletPose &may_non_canonicalized_lanelet_pose, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils) -> LaneletPose
Definition: lanelet_pose.cpp:72
std::vector< LaneletPose > lanelet_poses_
Definition: lanelet_pose.hpp:84
geometry_msgs::msg::Pose map_pose_
Definition: lanelet_pose.hpp:85
CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose &obj)
Definition: lanelet_pose.cpp:63
DEFINE_COMPARISON_OPERATOR(<=) DEFINE_COMPARISON_OPERATOR(<) DEFINE_COMPARISON_OPERATOR(>
auto getLaneletPose() const -> const LaneletPose &
Definition: lanelet_pose.hpp:40
static auto getConsiderPoseByRoadSlope() -> bool
Definition: lanelet_pose.hpp:50
Definition: cache.hpp:46
Definition: cache.hpp:27
Definition: api.hpp:48
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:22
auto isSameLaneletId(const CanonicalizedEntityStatus &first_status, const CanonicalizedEntityStatus &second_status) -> bool
Definition: entity_status.cpp:200
Definition: routing_configuration.hpp:24