From b375c5fdb655b0a2f385b30ddcdc2114798ef9e2 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Fri, 6 Dec 2024 17:20:59 +0100 Subject: [PATCH 01/14] [RJD-1369] Improve lanelet matching - 3D support - Enhanced lanelet matching algorithm (`toLaneletPose` method) by incorporating lanelet altitude. - Defined the `altitude_threshold` parameter that sets the maximum altitude difference to determine when an entity can be matched with a specific lanelet. --- .../hdmap_utils/hdmap_utils.hpp | 19 ++++++++++++++++++- .../src/hdmap_utils/hdmap_utils.cpp | 13 ++++++++++++- 2 files changed, 30 insertions(+), 2 deletions(-) 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 07dced38f8f..618b63b688d 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,7 +380,24 @@ 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; + private: + /* + 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 conducted by Szymon Parapura. + */ + static constexpr double altitude_threshold_ = 1.0; + /** @defgroup cache * Declared mutable for caching */ diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index b6f9406a8d8..55a71850e49 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -332,7 +332,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; @@ -593,6 +593,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())); @@ -616,6 +621,12 @@ auto HdMapUtils::toLaneletPose( return lanelet_pose; } +auto HdMapUtils::isAltitudeMatching( + const double current_altitude, const double target_altitude) const -> bool +{ + 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 From c1b4d1f9d55f704435018e070bb60b9a7a0bc99d Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Sun, 8 Dec 2024 17:17:16 +0100 Subject: [PATCH 02/14] [RJD-1369] Improve Collision Solving for Multi-Level Support - Enhanced BehaviorTree to consider altitude when detecting potential obstacles, allowing to ignore objects located at different altitudes. - Modified the detection sensor by introducing Ego plane determination to exclude objects below the Ego plane, preventing unnecessary slowing or stopping caused by incorrect detections. --- .../math/geometry/include/geometry/plane.hpp | 42 +++++++++++++++ .../quaternion/get_angle_difference.hpp | 39 ++++++++++++++ .../geometry/quaternion/get_normal_vector.hpp | 41 +++++++++++++++ common/math/geometry/src/plane.cpp | 48 +++++++++++++++++ .../behavior_tree_plugin/action_node.hpp | 2 + .../behavior_tree_plugin/src/action_node.cpp | 10 +++- .../detection_sensor/detection_sensor.hpp | 42 ++++++++++++++- .../detection_sensor/detection_sensor.cpp | 52 +++++++++++++++++++ 8 files changed, 274 insertions(+), 2 deletions(-) create mode 100644 common/math/geometry/include/geometry/plane.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp create mode 100644 common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp create mode 100644 common/math/geometry/src/plane.cpp diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp new file mode 100644 index 00000000000..a718c319bd7 --- /dev/null +++ b/common/math/geometry/include/geometry/plane.hpp @@ -0,0 +1,42 @@ +// 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 +#include + +namespace math +{ +namespace geometry +{ +struct Plane +{ + geometry_msgs::msg::Vector3 normal_; + double d_; + + Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); + auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; +}; + +auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) + -> std::optional; + +} // 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..298a9e2d481 --- /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 // Załaduj biblioteki Eigen +#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); + + 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..b315df00c1a --- /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_ \ No newline at end of file diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp new file mode 100644 index 00000000000..ea4805b2bc0 --- /dev/null +++ b/common/math/geometry/src/plane.cpp @@ -0,0 +1,48 @@ +// 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 + +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)) +{ +} + +auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) -> double +{ + return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; +} + +auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) + -> std::optional +{ + if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { + return std::nullopt; + } + + if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { + return std::nullopt; + } + + return Plane(point, normal); +} + +} // 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..0b2c7350e5d 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 isEntityAltitudeCompatible( + const traffic_simulator::CanonicalizedEntityStatus & other_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..82f77d43358 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() && isEntityAltitudeCompatible(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, @@ -326,6 +326,14 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } +auto ActionNode::isEntityAltitudeCompatible( + const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool +{ + return hdmap_utils->isAltitudeMatching( + hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), + hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); +} + 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 f5545a0d9d4..33de5b9b8f1 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 @@ -37,7 +40,11 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), configuration_(configuration) + : previous_simulation_time_(current_simulation_time), + configuration_(configuration), + ego_pose_(), + previous_ego_pose_(std::nullopt), + ego_plane_(std::nullopt) { } @@ -48,6 +55,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 +65,36 @@ class DetectionSensorBase const double current_simulation_time, const std::vector &, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) = 0; + +private: + /* + 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 Szymon Parapura + 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 Szymon Parapura + experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; + + geometry_msgs::msg::Pose ego_pose_; + std::optional previous_ego_pose_; + std::optional ego_plane_; + + auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const + -> bool; + auto needToUpdateEgoPlane() const -> bool; + auto hasEgoOrientationChanged() const -> bool; }; 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 23b38f4b094..187d93eab60 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,55 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } } +auto DetectionSensorBase::isOnOrAboveEgoPlane( + const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool +{ + using math::geometry::getNormalVector; + using math::geometry::makePlane; + + geometry_msgs::msg::Pose entity_pose; + simulation_interface::toMsg(ego_pose, ego_pose_); + simulation_interface::toMsg(npc_pose, entity_pose); + + if (isAltitudeDifferenceWithinThreshold(entity_pose)) { + return true; + } + + if (needToUpdateEgoPlane()) { + ego_plane_ = makePlane(ego_pose_.position, getNormalVector(ego_pose_.orientation)); + if (ego_plane_) { + previous_ego_pose_ = ego_pose_; + } else { + return false; + } + } + + return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; +} + +auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( + const geometry_msgs::msg::Pose & entity_pose) const -> bool +{ + return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); +} + +auto DetectionSensorBase::needToUpdateEgoPlane() const -> bool +{ + return !ego_plane_ || hasEgoOrientationChanged(); +} + +auto DetectionSensorBase::hasEgoOrientationChanged() const -> bool +{ + using math::geometry::getAngleDifference; + + if (!previous_ego_pose_) { + return true; + } + + return getAngleDifference(ego_pose_.orientation, previous_ego_pose_.value().orientation) > + rotation_threshold_; +} + template auto make(From &&...) -> To; @@ -322,6 +373,7 @@ auto DetectionSensor::updat 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()) != From 21baa9e093eb4b46c925097736aa8e34755ecaed Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 10:17:04 +0100 Subject: [PATCH 03/14] Fix missing newline at end of file --- .../geometry/include/geometry/quaternion/get_normal_vector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp index b315df00c1a..318a239c16d 100644 --- a/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_normal_vector.hpp @@ -38,4 +38,4 @@ auto getNormalVector(const T & orientation) -> geometry_msgs::msg::Vector3 } // namespace geometry } // namespace math -#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ \ No newline at end of file +#endif // GEOMETRY__VECTOR3__GET_NORMAL_VECTOR_HPP_ From d27b8f5ebe2f11a8b167dc7d614b1c60f86fb836 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 11:20:02 +0100 Subject: [PATCH 04/14] Removed unrecognized words because spell-check flagged them as invalid --- .../sensor_simulation/detection_sensor/detection_sensor.hpp | 6 ++---- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) 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 33de5b9b8f1..b220571ef7a 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 @@ -72,8 +72,7 @@ class DetectionSensorBase 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 Szymon Parapura - experiments. + There is no technical basis for this value, it was determined based on experiments. */ constexpr static double rotation_threshold_ = 0.04; @@ -82,8 +81,7 @@ class DetectionSensorBase 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 Szymon Parapura - experiments. + There is no technical basis for this value, it was determined based on experiments. */ constexpr static double max_downward_z_offset_ = 1.0; 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 618b63b688d..6e5bec89712 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 @@ -394,7 +394,7 @@ class HdMapUtils 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 conducted by Szymon Parapura. + experiments. */ static constexpr double altitude_threshold_ = 1.0; From 729c31f61c7c3846493b9b4f99e526f4c5c56be7 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 16:13:52 +0100 Subject: [PATCH 05/14] Fix an issue with unit tests - distanceTest --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 6 +- .../test/src/helper_functions.hpp | 5 +- .../test/src/utils/test_distance.cpp | 116 +++++++++--------- 3 files changed, 65 insertions(+), 62 deletions(-) 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 ebe1558fb98..d670eeef614 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( From fbd15d4c65da769a31a701bb284b82b9ee87acee Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Mon, 9 Dec 2024 19:03:00 +0100 Subject: [PATCH 06/14] [RJD-1370] Fix 3D Lanelet Matching Issue in cpp_mock_scenario - Updated the makeRandomPose method to correctly support 3D lanelet matching. --- .../hdmap_utils/hdmap_utils.hpp | 4 ++++ .../traffic/traffic_source.hpp | 3 ++- .../src/hdmap_utils/hdmap_utils.cpp | 12 ++++++++++++ .../src/traffic/traffic_source.cpp | 17 +++++++++++++++-- 4 files changed, 33 insertions(+), 3 deletions(-) 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 6e5bec89712..e8797a4f3ea 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 @@ -383,6 +383,10 @@ class HdMapUtils 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: /* Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the 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/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 55a71850e49..8b0909ecba6 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -2175,6 +2175,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); } From d64210b4171344b9b1c8edbcfe162b4761c6464c Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 12:30:47 +0100 Subject: [PATCH 07/14] Remove comment --- .../include/geometry/quaternion/get_angle_difference.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp index 298a9e2d481..8c8ff46265f 100644 --- a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -15,7 +15,7 @@ #ifndef GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ #define GEOMETRY__QUATERNION__GET_ANGLE_DIFFERENCE_HPP_ -#include // Załaduj biblioteki Eigen +#include #include namespace math From 8e954f489857b95e8f9c5ab92f73e6a319c5a0bd Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 15:43:58 +0100 Subject: [PATCH 08/14] Refactor code to improve readability based on SonarQube findings --- common/math/geometry/include/geometry/plane.hpp | 2 +- common/math/geometry/src/plane.cpp | 2 +- .../include/behavior_tree_plugin/action_node.hpp | 2 +- simulation/behavior_tree_plugin/src/action_node.cpp | 4 ++-- .../detection_sensor/detection_sensor.hpp | 13 ++++--------- .../detection_sensor/detection_sensor.cpp | 4 ++-- 6 files changed, 11 insertions(+), 16 deletions(-) diff --git a/common/math/geometry/include/geometry/plane.hpp b/common/math/geometry/include/geometry/plane.hpp index a718c319bd7..af442606a14 100644 --- a/common/math/geometry/include/geometry/plane.hpp +++ b/common/math/geometry/include/geometry/plane.hpp @@ -30,7 +30,7 @@ struct Plane double d_; Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); - auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; + auto calculateOffset(const geometry_msgs::msg::Point & point) const -> double; }; auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp index ea4805b2bc0..16eb761f2e7 100644 --- a/common/math/geometry/src/plane.cpp +++ b/common/math/geometry/src/plane.cpp @@ -25,7 +25,7 @@ Plane::Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg:: { } -auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) -> double +auto Plane::calculateOffset(const geometry_msgs::msg::Point & point) const -> double { return normal_.x * point.x + normal_.y * point.y + normal_.z * point.z + d_; } 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 0b2c7350e5d..2ebc7b9446c 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 @@ -140,7 +140,7 @@ class ActionNode : public BT::ActionNodeBase auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; auto isEntityAltitudeCompatible( - const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool; + 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 82f77d43358..49fd3ff33dc 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -327,11 +327,11 @@ auto ActionNode::getDistanceToTargetEntityPolygon( } auto ActionNode::isEntityAltitudeCompatible( - const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool + const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool { return hdmap_utils->isAltitudeMatching( hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), - hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); + hdmap_utils->getAltitude(entity_status.getLaneletPose())); } auto ActionNode::getDistanceToConflictingEntity( 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 685ed421bd4..177c972247b 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 @@ -40,11 +40,7 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), - configuration_(configuration), - ego_pose_(), - previous_ego_pose_(std::nullopt), - ego_plane_(std::nullopt) + : previous_simulation_time_(current_simulation_time), configuration_(configuration) { } @@ -86,11 +82,10 @@ class DetectionSensorBase constexpr static double max_downward_z_offset_ = 1.0; geometry_msgs::msg::Pose ego_pose_; - std::optional previous_ego_pose_; - std::optional ego_plane_; + std::optional previous_ego_pose_{std::nullopt}; + std::optional ego_plane_{std::nullopt}; - auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const - -> bool; + auto isEntityAltitudeAcceptable(const geometry_msgs::msg::Pose & entity_pose) const -> bool; auto needToUpdateEgoPlane() const -> bool; auto hasEgoOrientationChanged() const -> bool; }; 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 85f1411fa04..8feb188553f 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 @@ -74,7 +74,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( simulation_interface::toMsg(ego_pose, ego_pose_); simulation_interface::toMsg(npc_pose, entity_pose); - if (isAltitudeDifferenceWithinThreshold(entity_pose)) { + if (isEntityAltitudeAcceptable(entity_pose)) { return true; } @@ -90,7 +90,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; } -auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( +auto DetectionSensorBase::isEntityAltitudeAcceptable( const geometry_msgs::msg::Pose & entity_pose) const -> bool { return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); From c6e983e5b62a2a827ba282bc638c325f230796db Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:08:15 +0100 Subject: [PATCH 09/14] ref(traffic_simulator, simple_sensor_simulator): refactor altitude checks --- .github/workflows/custom_spell.json | 4 +- .../math/geometry/include/geometry/plane.hpp | 23 +++--- .../quaternion/get_angle_difference.hpp | 2 +- common/math/geometry/src/plane.cpp | 25 ++----- .../behavior_tree_plugin/action_node.hpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 7 +- .../detection_sensor/detection_sensor.hpp | 32 +------- .../detection_sensor/detection_sensor.cpp | 73 ++++++++++--------- .../simulation_interface/conversions.hpp | 15 ++-- .../behavior/behavior_plugin_base.hpp | 14 ++-- .../data_type/entity_status.hpp | 2 + .../hdmap_utils/hdmap_utils.hpp | 14 ---- .../src/data_type/entity_status.cpp | 5 ++ .../src/hdmap_utils/hdmap_utils.cpp | 15 +++- 14 files changed, 104 insertions(+), 129 deletions(-) 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 index a718c319bd7..509e3c6e63f 100644 --- a/common/math/geometry/include/geometry/plane.hpp +++ b/common/math/geometry/include/geometry/plane.hpp @@ -15,7 +15,6 @@ #ifndef GEOMETRY__PLANE_HPP_ #define GEOMETRY__PLANE_HPP_ -#include #include #include #include @@ -24,19 +23,23 @@ 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 { - geometry_msgs::msg::Vector3 normal_; - double d_; - Plane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal); - auto calculateOffset(const geometry_msgs::msg::Point & point) -> double; -}; - -auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) - -> std::optional; + 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 index 8c8ff46265f..4e9450b2b0a 100644 --- a/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp +++ b/common/math/geometry/include/geometry/quaternion/get_angle_difference.hpp @@ -29,7 +29,7 @@ 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); - Eigen::AngleAxisd delta(q1.inverse() * q2); + const Eigen::AngleAxisd delta(q1.inverse() * q2); return std::abs(delta.angle()); // [rad] } diff --git a/common/math/geometry/src/plane.cpp b/common/math/geometry/src/plane.cpp index ea4805b2bc0..6439839443c 100644 --- a/common/math/geometry/src/plane.cpp +++ b/common/math/geometry/src/plane.cpp @@ -14,35 +14,26 @@ #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::calculateOffset(const geometry_msgs::msg::Point & point) -> double +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_; } - -auto makePlane(const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Vector3 & normal) - -> std::optional -{ - if (normal.x == 0.0 && normal.y == 0.0 && normal.z == 0.0) { - return std::nullopt; - } - - if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) { - return std::nullopt; - } - - return Plane(point, normal); -} - } // 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 0b2c7350e5d..5e9d6bc968d 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,7 +139,7 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto getConflictingEntityStatusOnLane(const lanelet::Ids & route_lanelets) const -> std::vector; - auto isEntityAltitudeCompatible( + auto isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & other_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 82f77d43358..4e4d0677f84 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() && isEntityAltitudeCompatible(status)) { + 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,12 +326,11 @@ auto ActionNode::getDistanceToTargetEntityPolygon( return std::nullopt; } -auto ActionNode::isEntityAltitudeCompatible( +auto ActionNode::isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & other_entity_status) const -> bool { return hdmap_utils->isAltitudeMatching( - hdmap_utils->getAltitude(canonicalized_entity_status->getLaneletPose()), - hdmap_utils->getAltitude(other_entity_status.getLaneletPose())); + canonicalized_entity_status->getAltitude(), other_entity_status.getAltitude()); } auto ActionNode::getDistanceToConflictingEntity( 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 685ed421bd4..9a35ef719fc 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 @@ -42,9 +42,8 @@ class DetectionSensorBase const simulation_api_schema::DetectionSensorConfiguration & configuration) : previous_simulation_time_(current_simulation_time), configuration_(configuration), - ego_pose_(), - previous_ego_pose_(std::nullopt), - ego_plane_(std::nullopt) + ego_plane_opt_(std::nullopt), + previous_ego_pose_opt_(std::nullopt) { } @@ -67,31 +66,8 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; private: - /* - 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; - - geometry_msgs::msg::Pose ego_pose_; - std::optional previous_ego_pose_; - std::optional ego_plane_; - - auto isAltitudeDifferenceWithinThreshold(const geometry_msgs::msg::Pose & entity_pose) const - -> bool; - auto needToUpdateEgoPlane() const -> bool; + std::optional ego_plane_opt_; + std::optional previous_ego_pose_opt_; auto hasEgoOrientationChanged() const -> bool; }; 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 85f1411fa04..fd85e6d6b8f 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 @@ -65,52 +65,53 @@ auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( } auto DetectionSensorBase::isOnOrAboveEgoPlane( - const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool + const geometry_msgs::Pose & entity_pose, const geometry_msgs::Pose & ego_pose) -> bool { - using math::geometry::getNormalVector; - using math::geometry::makePlane; + /* + 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". - geometry_msgs::msg::Pose entity_pose; - simulation_interface::toMsg(ego_pose, ego_pose_); - simulation_interface::toMsg(npc_pose, entity_pose); + 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 - if (isAltitudeDifferenceWithinThreshold(entity_pose)) { - return true; - } + There is no technical basis for this value, it was determined based on experiments. + */ + constexpr static double max_downward_z_offset_ = 1.0; - if (needToUpdateEgoPlane()) { - ego_plane_ = makePlane(ego_pose_.position, getNormalVector(ego_pose_.orientation)); - if (ego_plane_) { - previous_ego_pose_ = ego_pose_; + geometry_msgs::msg::Pose ego_pose_ros, entity_pose_ros; + simulation_interface::toMsg(entity_pose, entity_pose_ros); + simulation_interface::toMsg(ego_pose, ego_pose_ros); + + const auto hasEgoOrientationChanged = [this, &entity_pose_ros]() { + if (previous_ego_pose_opt_) { + return true; } else { - return false; + return math::geometry::getAngleDifference( + entity_pose_ros.orientation, previous_ego_pose_opt_->orientation) > + rotation_threshold_; } - } - - return ego_plane_.value().calculateOffset(entity_pose.position) >= 0.0; -} - -auto DetectionSensorBase::isAltitudeDifferenceWithinThreshold( - const geometry_msgs::msg::Pose & entity_pose) const -> bool -{ - return entity_pose.position.z >= (ego_pose_.position.z - max_downward_z_offset_); -} - -auto DetectionSensorBase::needToUpdateEgoPlane() const -> bool -{ - return !ego_plane_ || hasEgoOrientationChanged(); -} + }; -auto DetectionSensorBase::hasEgoOrientationChanged() const -> bool -{ - using math::geometry::getAngleDifference; + // update ego plane if needed + if (!ego_plane_opt_ || hasEgoOrientationChanged()) { + ego_plane_opt_.emplace( + entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); + } + previous_ego_pose_opt_ = entity_pose_ros; - if (!previous_ego_pose_) { + // if other entity is just above ego return true + if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { return true; + // otherwise check if other entity is above ego plane + } else { + return ego_plane_opt_->offset(entity_pose_ros.position) >= 0.0; } - - return getAngleDifference(ego_pose_.orientation, previous_ego_pose_.value().orientation) > - rotation_threshold_; } template diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 0bd1f09e53f..6e8ee4b7ec8 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,8 +190,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -206,8 +205,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,8 +233,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index f4162f3e9b0..c4dac740c60 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off 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 c3ea1354b54..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 @@ -388,20 +388,6 @@ class HdMapUtils const double matching_distance = 1.0) const -> std::optional; private: - /* - 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; - /** @defgroup cache * Declared mutable for caching */ 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 c35415806bd..eab04aaf2f2 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -625,7 +625,20 @@ auto HdMapUtils::toLaneletPose( auto HdMapUtils::isAltitudeMatching( const double current_altitude, const double target_altitude) const -> bool { - return std::abs(current_altitude - target_altitude) <= altitude_threshold_; + /* + 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( From bb0a6bf74930a7302eda787009cabae7a8efdaec Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:14:34 +0100 Subject: [PATCH 10/14] fix(traffic_simulator): revert clang changes --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 6e8ee4b7ec8..0bd1f09e53f 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_control_msgs, Lateral); DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); @@ -190,7 +190,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -205,7 +206,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -233,7 +235,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From 8cf2c39ad0187868be85e4aa488c0c1eb99c15d2 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:35:37 +0100 Subject: [PATCH 11/14] fix(simple_sensor_simulator): fix after detection_sensor refactor --- .../detection_sensor/detection_sensor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 fd85e6d6b8f..c25fafb8719 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 @@ -88,13 +88,12 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( simulation_interface::toMsg(entity_pose, entity_pose_ros); simulation_interface::toMsg(ego_pose, ego_pose_ros); - const auto hasEgoOrientationChanged = [this, &entity_pose_ros]() { + const auto hasEgoOrientationChanged = [this, &ego_pose_ros]() { if (previous_ego_pose_opt_) { return true; } else { return math::geometry::getAngleDifference( - entity_pose_ros.orientation, previous_ego_pose_opt_->orientation) > - rotation_threshold_; + ego_pose_ros.orientation, previous_ego_pose_opt_->orientation) > rotation_threshold_; } }; @@ -103,7 +102,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( ego_plane_opt_.emplace( entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); } - previous_ego_pose_opt_ = entity_pose_ros; + previous_ego_pose_opt_ = ego_pose_ros; // if other entity is just above ego return true if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { From 80c5c89aecbe623d989c3c846fc1c612ca7050bb Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 20:48:27 +0100 Subject: [PATCH 12/14] fix(simple_senor_simulator): fix after Szymon discussion --- .../detection_sensor/detection_sensor.hpp | 4 +-- .../detection_sensor/detection_sensor.cpp | 34 ++++++++----------- 2 files changed, 17 insertions(+), 21 deletions(-) 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 9a35ef719fc..237468b0fd8 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 @@ -43,7 +43,7 @@ class DetectionSensorBase : previous_simulation_time_(current_simulation_time), configuration_(configuration), ego_plane_opt_(std::nullopt), - previous_ego_pose_opt_(std::nullopt) + ego_plane_pose_opt_(std::nullopt) { } @@ -67,7 +67,7 @@ class DetectionSensorBase private: std::optional ego_plane_opt_; - std::optional previous_ego_pose_opt_; + std::optional ego_plane_pose_opt_; auto hasEgoOrientationChanged() const -> bool; }; 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 c25fafb8719..13dad82fb9e 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 @@ -84,31 +84,27 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( */ constexpr static double max_downward_z_offset_ = 1.0; - geometry_msgs::msg::Pose ego_pose_ros, entity_pose_ros; - simulation_interface::toMsg(entity_pose, entity_pose_ros); - simulation_interface::toMsg(ego_pose, ego_pose_ros); - - const auto hasEgoOrientationChanged = [this, &ego_pose_ros]() { - if (previous_ego_pose_opt_) { - return true; - } else { - return math::geometry::getAngleDifference( - ego_pose_ros.orientation, previous_ego_pose_opt_->orientation) > rotation_threshold_; - } + 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_; }; - // update ego plane if needed - if (!ego_plane_opt_ || hasEgoOrientationChanged()) { - ego_plane_opt_.emplace( - entity_pose_ros.position, math::geometry::getNormalVector(entity_pose_ros.orientation)); - } - previous_ego_pose_opt_ = ego_pose_ros; - // if other entity is just above ego return true - if (entity_pose_ros.position.z >= (ego_pose_ros.position.z - max_downward_z_offset_)) { + 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; } } From 547cb0adbbfe337355e18e206e746860d111d47e Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 21:22:38 +0100 Subject: [PATCH 13/14] Remove unused function and update comment --- .../sensor_simulation/detection_sensor/detection_sensor.hpp | 1 - .../src/sensor_simulation/detection_sensor/detection_sensor.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) 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 237468b0fd8..67fb5bd3025 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 @@ -68,7 +68,6 @@ class DetectionSensorBase private: std::optional ego_plane_opt_; std::optional ego_plane_pose_opt_; - auto hasEgoOrientationChanged() const -> bool; }; 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 13dad82fb9e..c5f180d70a2 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 @@ -89,7 +89,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( ego_pose_ros.orientation, ego_plane_pose_opt_->orientation) > rotation_threshold_; }; - // if other entity is just above ego return true + // 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 From 682acd3e04f8faa9c5bbe8bd3fe4be44280e1b02 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Tue, 10 Dec 2024 23:57:59 +0100 Subject: [PATCH 14/14] fix(simple_sensor_simulator): Fix if condition by adding negation to ensure proper logic --- .../detection_sensor/detection_sensor.hpp | 9 +++------ .../detection_sensor/detection_sensor.cpp | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) 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 67fb5bd3025..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 @@ -40,10 +40,7 @@ class DetectionSensorBase explicit DetectionSensorBase( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration) - : previous_simulation_time_(current_simulation_time), - configuration_(configuration), - ego_plane_opt_(std::nullopt), - ego_plane_pose_opt_(std::nullopt) + : previous_simulation_time_(current_simulation_time), configuration_(configuration) { } @@ -66,8 +63,8 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; private: - std::optional ego_plane_opt_; - std::optional ego_plane_pose_opt_; + 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 c5f180d70a2..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 @@ -97,7 +97,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane( // 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)) { + 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;