15 #ifndef TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
16 #define TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
25 inline namespace lanelet_pose
32 const LaneletPose & non_canonicalized_lanelet_pose,
const lanelet::Ids & route_lanelets);
60 #define DEFINE_COMPARISON_OPERATOR(OPERATOR) \
61 bool operator OPERATOR(const CanonicalizedLaneletPose & rhs) const \
64 static_cast<LaneletPose>(*this).lanelet_id == static_cast<LaneletPose>(rhs).lanelet_id && \
65 static_cast<LaneletPose>(*this).s OPERATOR static_cast<LaneletPose>(rhs).s) { \
75 #undef DEFINE_COMPARISON_OPERATOR
79 os <<
"CanonicalizedLaneletPose(\n";
82 os <<
" alternative from lanelet_poses_: " << obj.
lanelet_poses_.front() <<
"\n";
84 os <<
" lanelet_poses_: [\n";
86 os <<
" - " << pose <<
"\n";
90 os <<
" map_pose_: " << obj.
map_pose_ <<
"\n";
91 os <<
" consider_pose_by_road_slope_: "
98 auto adjustOrientationAndOzPosition() -> void;
Definition: lanelet_pose.hpp:28
LaneletPose lanelet_pose_
Definition: lanelet_pose.hpp:99
static bool consider_pose_by_road_slope_
Definition: lanelet_pose.hpp:102
auto hasAlternativeLaneletPose() const -> bool
Definition: lanelet_pose.hpp:43
static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void
Definition: lanelet_pose.hpp:49
auto getAlternativeLaneletPoseBaseOnShortestRouteFrom(LaneletPose from, const RoutingConfiguration &routing_configuration=RoutingConfiguration()) const -> std::optional< LaneletPose >
Definition: lanelet_pose.cpp:70
auto getLaneletId() const noexcept -> lanelet::Id
Definition: lanelet_pose.hpp:41
auto alignOrientationToLanelet() -> void
Definition: lanelet_pose.cpp:90
std::vector< LaneletPose > lanelet_poses_
Definition: lanelet_pose.hpp:100
geometry_msgs::msg::Pose map_pose_
Definition: lanelet_pose.hpp:101
auto getAltitude() const -> double
Definition: lanelet_pose.hpp:40
CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose &obj)
Definition: lanelet_pose.cpp:61
DEFINE_COMPARISON_OPERATOR(<=) DEFINE_COMPARISON_OPERATOR(<) DEFINE_COMPARISON_OPERATOR(>
CanonicalizedLaneletPose(const LaneletPose &non_canonicalized_lanelet_pose)
Definition: lanelet_pose.cpp:29
auto getLaneletPose() const -> const LaneletPose &
Definition: lanelet_pose.hpp:39
static auto getConsiderPoseByRoadSlope() -> bool
Definition: lanelet_pose.hpp:53
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
auto isSameLaneletId(const CanonicalizedEntityStatus &first_status, const CanonicalizedEntityStatus &second_status) -> bool
Definition: entity_status.cpp:198
std::ostream & operator<<(std::ostream &os, const geometry_msgs::msg::Point &point)
Definition: ostream_helpers.cpp:20
Definition: routing_configuration.hpp:24