diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index 676f628922b..50ca6f5fe94 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -46,6 +46,8 @@ "Tschirnhaus", "walltime", "xerces", - "xercesc" + "xercesc", + "Szymon", + "Parapura" ] } diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp new file mode 100644 index 00000000000..509e3c6e63f --- /dev/null +++ b/common/math/geometry/include/geometry/plane.hpp @@ -0,0 +1,45 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__PLANE_HPP_ +#define GEOMETRY__PLANE_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ + +/// @class Plane +/// @brief Represents a plane in 3D space, defined by a normal vector and a point on the plane. +/// +/// The plane is described using the equation: +/// Ax + By + Cz + D = 0 +/// where: +/// - A, B, C are the components of the normal vector (normal_ attribute). +/// - D is the offset from the origin, calculated using the point and normal vector (d_ attribute). +struct Plane +{ + Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); + auto offset(const geometry_msgs::msg::Point & point) const -> double; + + const geometry_msgs::msg::Vector3 normal_; + const double d_; +}; +} // namespace geometry +} // namespace math +#endif // GEOMETRY__PLANE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp new file mode 100644 index 00000000000..4e9450b2b0a --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -0,0 +1,39 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ +#define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ + +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getAngleDifference(const T & quat1, const T & quat2) -> double +{ + const Eigen::Quaterniond q1(quat1.w, quat1.x, quat1.y, quat1.z); + const Eigen::Quaterniond q2(quat2.w, quat2.x, quat2.y, quat2.z); + + const Eigen::AngleAxisd delta(q1.inverse() * q2); + + return std::abs(delta.angle()); // [rad] +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ diff --git a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp new file mode 100644 index 00000000000..318a239c16d --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp @@ -0,0 +1,41 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ +#define GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3 +{ + const Eigen::Matrix3d rotation_matrix = getRotationMatrix(orientation); + + return geometry_msgs::build() + .x(rotation_matrix(0, 2)) + .y(rotation_matrix(1, 2)) + .z(rotation_matrix(2, 2)); +} + +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp new file mode 100644 index 00000000000..6439839443c --- /dev/null +++ b/common/math/geometry/src/plane.cpp @@ -0,0 +1,39 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) +: normal_(normal), d_(-(normal.x * point.x + normal.y * point.y + normal.z * point.z)) +{ + if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { + THROW_SIMULATION_ERROR("Plane cannot be created using zero normal vector."); + } else if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { + THROW_SIMULATION_ERROR("Plane cannot be created using point with NaN value."); + } +} + +auto Plane::offset(const geometry_msgs::msg::Point & point) const -> double +{ + return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; +} +} // namespace geometry +} // namespace math diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 4bcba6c41cc..dc5d2f1a8e1 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -139,6 +139,8 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; + auto isOtherEntityAtConsideredAltitude( + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index cbe86b1c649..86db4b18d41 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -316,7 +316,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed()) { + if (status.laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -326,6 +326,13 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } +auto ActionNode::isOtherEntityAtConsideredAltitude( + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool +{ + return hdmap_utils->isAltitudeMatching( + canonicalized_entity_status->getAltitude(), entity_status.getAltitude()); +} + auto ActionNode::getDistanceToConflictingEntity( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index ec4dcf6ef18..0aff66be710 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -17,7 +17,10 @@ #include +#include +#include #include +#include #include #include #include @@ -48,6 +51,9 @@ class DetectionSensorBase const std::vector &) const -> std::vector::const_iterator; + auto isOnOrAboveEgoPlane( + const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool; + public: virtual ~DetectionSensorBase() = default; @@ -55,6 +61,10 @@ class DetectionSensorBase const double current_simulation_time, const std::vector &, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) = 0; + +private: + std::optional ego_plane_opt_{std::nullopt}; + std::optional ego_plane_pose_opt_{std::nullopt}; }; template diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index dc6c1837f0d..cf5925688ff 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -62,6 +64,51 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } } +auto DetectionSensorBase::isOnOrAboveEgoPlane( + const geometry_msgs::Pose & entity_pose, const geometry_msgs::Pose & ego_pose) -> bool +{ + /* + The threshold for detecting significant changes in ego vehicle's orientation (unit: radian). + The value determines the minimum angular difference required to consider the ego orientation + as "changed". + + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double rotation_threshold_ = 0.04; + /* + Maximum downward offset in Z-axis relative to the ego position (unit: meter). + If the NPC is lower than this offset relative to the ego position, + the NPC will be excluded from detection + + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; + + const auto hasEgoOrientationChanged = [this](const geometry_msgs::msg::Pose & ego_pose_ros) { + return math::geometry::getAngleDifference( + ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_; + }; + + // if other entity is at the same altitude as Ego or within max_downward_z_offset_ below Ego + if (entity_pose.position().z() >= (ego_pose.position().z() - max_downward_z_offset_)) { + return true; + // otherwise check if other entity is above ego plane + } else { + // update ego plane if needed + geometry_msgs::msg::Pose ego_pose_ros; + simulation_interface::toMsg(ego_pose, ego_pose_ros); + if (!ego_plane_opt_ || !ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) { + ego_plane_opt_.emplace( + ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation)); + ego_plane_pose_opt_ = ego_pose_ros; + } + + geometry_msgs::msg::Pose entity_pose_ros; + simulation_interface::toMsg(entity_pose, entity_pose_ros); + return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0; + } +} + template auto make(From &&...) -> To; @@ -321,6 +368,7 @@ auto DetectionSensor::update( auto is_in_range = [&](const auto & status) { return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and + isOnOrAboveEgoPlane(status.pose(), ego_entity_status->pose()) and (configuration_.detect_all_objects_in_range() or std::find( lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) != diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 68a781808a3..b1d07af341f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -54,6 +54,8 @@ class CanonicalizedEntityStatus auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &; auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void; + auto getAltitude() const -> double; + auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &; auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; auto setLinearVelocity(double linear_velocity) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 0f34b2c7c97..9be623a2cab 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -160,7 +160,7 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; + auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, @@ -380,6 +380,13 @@ class HdMapUtils auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true) const -> geometry_msgs::msg::PoseStamped; + auto isAltitudeMatching(const double current_altitude, const double target_altitude) const + -> bool; + + auto getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance = 1.0) const -> std::optional; + private: /** @defgroup cache * Declared mutable for caching diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp index cb2b70636cd..c9daa49273e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_source.hpp @@ -117,7 +117,8 @@ class TrafficSource : public TrafficModuleBase const std::size_t id; private: - auto makeRandomPose(const bool random_orientation = false) -> geometry_msgs::msg::Pose; + auto makeRandomPose(const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose; auto isPoseValid(const VehicleOrPedestrianParameter &, const geometry_msgs::msg::Pose &) -> std::pair>; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..758d10b6abf 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -122,6 +122,11 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms return entity_status_.pose; } +auto CanonicalizedEntityStatus::getAltitude() const -> double +{ + return entity_status_.pose.position.z; +} + auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & { if (canonicalized_lanelet_pose_) { diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 650fb0fd802..eab04aaf2f2 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -333,7 +333,7 @@ auto HdMapUtils::getNearbyLaneletIds( return lanelet_ids; } -auto HdMapUtils::getHeight(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const +auto HdMapUtils::getAltitude(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const -> double { return toMapPose(lanelet_pose).pose.position.z; @@ -594,6 +594,11 @@ auto HdMapUtils::toLaneletPose( return std::nullopt; } auto pose_on_centerline = spline->getPose(s.value()); + + if (!isAltitudeMatching(pose.position.z, pose_on_centerline.position.z)) { + return std::nullopt; + } + auto rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); @@ -617,6 +622,25 @@ auto HdMapUtils::toLaneletPose( return lanelet_pose; } +auto HdMapUtils::isAltitudeMatching( + const double current_altitude, const double target_altitude) const -> bool +{ + /* + Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + entity's Z-position is always relative to its base. This eliminates the + need to dynamically adjust the threshold based on the entity's dimensions, + ensuring consistent altitude matching regardless of the entity type. + + The position of the entity is defined relative to its base, typically + the center of the rear axle projected onto the ground in the case of vehicles. + + There is no technical basis for this value; it was determined based on + experiments. + */ + static constexpr double altitude_threshold = 1.0; + return std::abs(current_altitude - target_altitude) <= altitude_threshold; +} + auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, const double matching_distance) const -> std::optional @@ -2166,6 +2190,18 @@ auto HdMapUtils::toPolygon(const lanelet::ConstLineString3d & line_string) const return ret; } +auto HdMapUtils::getLaneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) const -> std::optional +{ + if (const auto spline = getCenterPointsSpline(lanelet_id)) { + if (const auto s = spline->getSValue(pose, matching_distance)) { + return spline->getPoint(s.value()).z; + } + } + return std::nullopt; +} + HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_map) { vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create( diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 350920a42d1..68f82fdce61 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -72,7 +72,9 @@ auto TrafficSource::Validator::operator()( }); } -auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_msgs::msg::Pose +auto TrafficSource::makeRandomPose( + const bool random_orientation, const VehicleOrPedestrianParameter & parameter) + -> geometry_msgs::msg::Pose { const double angle = angle_distribution_(engine_); @@ -83,6 +85,17 @@ auto TrafficSource::makeRandomPose(const bool random_orientation) -> geometry_ms random_pose.position.x += radius * std::cos(angle); random_pose.position.y += radius * std::sin(angle); + if (const auto nearby_lanelets = hdmap_utils_->getNearbyLaneletIds( + random_pose.position, radius, std::holds_alternative(parameter)); + !nearby_lanelets.empty()) { + // Get the altitude of the first nearby lanelet + if ( + const auto altitude = + hdmap_utils_->getLaneletAltitude(nearby_lanelets.front(), random_pose, radius)) { + random_pose.position.z = altitude.value(); + } + } + if (random_orientation) { random_pose.orientation = math::geometry::convertEulerAngleToQuaternion( traffic_simulator::helper::constructRPY(0.0, 0.0, angle_distribution_(engine_))); @@ -106,7 +119,7 @@ void TrafficSource::execute( static constexpr auto max_randomization_attempts = 10000; for (auto tries = 0; tries < max_randomization_attempts; ++tries) { - auto candidate_pose = makeRandomPose(configuration_.use_random_orientation); + auto candidate_pose = makeRandomPose(configuration_.use_random_orientation, parameter); if (auto [valid, lanelet_pose] = isPoseValid(parameter, candidate_pose); valid) { return std::make_pair(candidate_pose, lanelet_pose); } diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index eaa36c849a4..dd0dda7330f 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -2098,9 +2098,11 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLaneletNoRoute) { const auto pose_to = hdmap_utils.toLaneletPose( - makePose(makePoint(81590.79, 50067.66), makeQuaternionFromYaw(90.0)), lanelet::Id{3002185}); + makePose(makePoint(81590.79, 50067.66, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002185}); const auto pose_from = hdmap_utils.toLaneletPose( - makePose(makePoint(81596.20, 50068.04), makeQuaternionFromYaw(90.0)), lanelet::Id{3002166}); + makePose(makePoint(81596.20, 50068.04, 35.0), makeQuaternionFromYaw(90.0)), + lanelet::Id{3002166}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index a12b5e33328..c10cc1d4931 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -63,13 +63,14 @@ auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } -auto makePose(const double x, const double y, const double yaw_deg) -> geometry_msgs::msg::Pose +auto makePose(const double x, const double y, const double z, const double yaw_deg) + -> geometry_msgs::msg::Pose { /** * @note +x axis is 0 degrees; +y axis is 90 degrees */ return geometry_msgs::build() - .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .position(geometry_msgs::build().x(x).y(y).z(z)) .orientation(math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build().x(0.0).y(0.0).z( convertDegToRad(yaw_deg)))); diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index d288117c96b..8dc5b5cdee8 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -238,10 +238,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + makePose(81595.44, 50006.09, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + makePose(81584.48, 50084.76, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -260,10 +260,10 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + makePose(3800.05, 73715.77, 0.5, 30.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + makePose(3841.26, 73748.80, 0.5, 110.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -283,10 +283,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + makePose(81585.79, 50042.62, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + makePose(81588.34, 50083.23, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -305,10 +305,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + makePose(81599.02, 50065.76, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + makePose(81599.61, 50045.16, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -328,10 +328,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + makePose(81595.47, 49982.80, 36.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + makePose(81599.34, 50022.34, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -343,10 +343,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + makePose(81612.35, 50015.63, 35.0, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + makePose(81612.95, 49991.30, 35.5, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -367,10 +367,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + makePose(81592.96, 49997.94, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + makePose(81570.56, 50141.75, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -383,10 +383,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + makePose(81587.31, 50165.57, 35.0, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + makePose(81610.25, 49988.59, 35.5, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -406,10 +406,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + makePose(86627.71, 44972.06, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -421,10 +421,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + makePose(86555.38, 45000.88, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -436,10 +436,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); + makePose(86553.48, 44990.56, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -451,10 +451,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); + makePose(86579.91, 44979.00, 3.0, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -475,10 +475,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + makePose(86736.13, 44969.63, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + makePose(86642.95, 44958.78, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -490,10 +490,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + makePose(86732.06, 44976.58, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + makePose(86704.59, 44927.32, 3.0, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -514,9 +514,9 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); + makePose(86637.19, 44967.35, 3.0, 340.0), false, hdmap_utils_ptr); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + makePose(86648.82, 44886.19, 3.0, 240.0), false, hdmap_utils_ptr); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -528,10 +528,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + makePose(86719.94, 44957.20, 3.0, 210.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + makePose(86599.32, 44975.01, 3.0, 180.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -550,8 +550,8 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch */ TEST(distance, boundingBoxDistance_intersection) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(30.0, 1.0); const auto bounding_box_to = makeCustom2DBoundingBox(1.0, 30.0); @@ -569,8 +569,8 @@ TEST(distance, boundingBoxDistance_intersection) */ TEST(distance, boundingBoxDistance_disjoint) { - const auto pose_from = makePose(100.0, 100.0, 0.0); - const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto pose_from = makePose(100.0, 100.0, 0.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 0.0, 90.0); const auto bounding_box_from = makeCustom2DBoundingBox(1.0, 30.0); const auto bounding_box_to = makeCustom2DBoundingBox(30.0, 1.0); @@ -592,56 +592,56 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) constexpr lanelet::Id lanelet_id = 34741L; constexpr double tolerance = 0.1; { - const auto pose = makePose(3818.33, 73726.18, 30.0); + const auto pose = makePose(3818.33, 73726.18, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.5, tolerance); } { - const auto pose = makePose(3816.89, 73723.09, 30.0); + const auto pose = makePose(3816.89, 73723.09, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.6, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 30.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.7, tolerance); } { - const auto pose = makePose(3813.42, 73721.11, 120.0); + const auto pose = makePose(3813.42, 73721.11, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.3, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.4, tolerance); } { - const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto pose = makePose(3810.99, 73721.40, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.4, tolerance); } { - const auto pose = makePose(3680.81, 73757.27, 30.0); + const auto pose = makePose(3680.81, 73757.27, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); EXPECT_NEAR(result, 5.1, tolerance); } { - const auto pose = makePose(3692.79, 73753.00, 30.0); + const auto pose = makePose(3692.79, 73753.00, 0.0, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, 34684L, hdmap_utils_ptr); @@ -656,7 +656,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{34603L, 34600L, 34621L, 34741L}; - const auto pose = makePose(3836.16, 73757.99, 120.0); + const auto pose = makePose(3836.16, 73757.99, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -678,7 +678,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 34426L; - const auto pose = makePose(3693.34, 73738.37, 300.0); + const auto pose = makePose(3693.34, 73738.37, 0.0, 300.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -693,7 +693,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) */ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToLeftLaneBound( @@ -709,56 +709,56 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) constexpr lanelet::Id lanelet_id = 660L; constexpr double tolerance = 0.1; { - const auto pose = makePose(86651.84, 44941.47, 135.0); + const auto pose = makePose(86651.84, 44941.47, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.1, tolerance); } { - const auto pose = makePose(86653.05, 44946.74, 135.0); + const auto pose = makePose(86653.05, 44946.74, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 0.6, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 120.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 4.3, tolerance); } { - const auto pose = makePose(86651.47, 44941.07, 210.0); + const auto pose = makePose(86651.47, 44941.07, 0.0, 210.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 3.1, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 2.0, tolerance); } { - const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto pose = makePose(86644.10, 44951.86, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 1.1, tolerance); } { - const auto pose = makePose(86644.11, 44941.21, 0.0); + const auto pose = makePose(86644.11, 44941.21, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); EXPECT_NEAR(result, 11.2, tolerance); } { - const auto pose = makePose(86656.83, 44946.96, 0.0); + const auto pose = makePose(86656.83, 44946.96, 0.0, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double result = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -773,7 +773,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) { const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; - const auto pose = makePose(86642.05, 44902.61, 60.0); + const auto pose = makePose(86642.05, 44902.61, 0.0, 60.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = std::transform_reduce( lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), @@ -795,7 +795,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) { constexpr lanelet::Id lanelet_id = 654L; - const auto pose = makePose(86702.79, 44929.05, 150.0); + const auto pose = makePose(86702.79, 44929.05, 0.0, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( pose, bounding_box, lanelet_id, hdmap_utils_ptr); @@ -810,7 +810,7 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) */ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) { - const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto pose = makePose(3825.87, 73773.08, 0.0, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( traffic_simulator::distance::distanceToRightLaneBound(