From 90075c7bd3cbbf892ca92040516e8b267be1c000 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Feb 2025 17:50:56 +0100 Subject: [PATCH 1/3] Reserve memory when possible Signed-off-by: Mateusz Palczuk --- simulation/behavior_tree_plugin/src/action_node.cpp | 1 + simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 327231ddfb8..3e10b00c81a 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -110,6 +110,7 @@ auto ActionNode::getOtherEntitiesCanonicalizedEntityStatuses() const -> std::vector { std::vector other_status; + other_status.reserve(other_entity_status.size()); for (const auto & [entity_name, entity_status] : other_entity_status) { other_status.push_back(entity_status); } diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 38e8b6d5e03..22fa3856947 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -72,6 +72,7 @@ auto filterLaneletIds(const lanelet::Ids & lanelet_ids, const char subtype[]) -> { const auto convertToLanelets = [](const lanelet::Ids & lanelet_ids) -> lanelet::Lanelets { lanelet::Lanelets lanelets; + lanelets.reserve(lanelet_ids.size()); for (const auto & id : lanelet_ids) { lanelets.emplace_back(LaneletWrapper::map()->laneletLayer.get(id)); } From cda0ed88439e1153d9a6f7b15f308beddd8762fa Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Feb 2025 17:52:18 +0100 Subject: [PATCH 2/3] Use push_back instead of emplace_back when we do not want to construct in place, but just pass a value Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 22fa3856947..11a7d949067 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -74,7 +74,7 @@ auto filterLaneletIds(const lanelet::Ids & lanelet_ids, const char subtype[]) -> lanelet::Lanelets lanelets; lanelets.reserve(lanelet_ids.size()); for (const auto & id : lanelet_ids) { - lanelets.emplace_back(LaneletWrapper::map()->laneletLayer.get(id)); + lanelets.push_back(LaneletWrapper::map()->laneletLayer.get(id)); } return lanelets; }; @@ -349,7 +349,7 @@ auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids const auto & conflicting_crosswalks = graphs_container.conflictingInGraph( LaneletWrapper::map()->laneletLayer.get(lanelet_id), routing_graph_id, height_clearance); for (const auto & conflicting_crosswalk : conflicting_crosswalks) { - conflicting_crosswalk_ids.emplace_back(conflicting_crosswalk.id()); + conflicting_crosswalk_ids.push_back(conflicting_crosswalk.id()); } } return conflicting_crosswalk_ids; From e477993c321bacdb4bf0ef9be687ca03de5aaa96 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 19 Feb 2025 17:53:12 +0100 Subject: [PATCH 3/3] Do not store all elements when only the smallest is needed Signed-off-by: Mateusz Palczuk --- .../behavior_tree_plugin/src/action_node.cpp | 20 +++++++++---------- .../traffic_lights/traffic_lights_base.cpp | 13 ++++++------ 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 3e10b00c81a..9b29f7f7954 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -212,7 +212,14 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf constexpr double front_entity_angle_threshold{boost::math::constants::half_pi()}; constexpr double front_entity_horizon{40.0}; - std::vector> entities_with_distances; + std::optional front_entity_name; + auto try_front_entity = [&front_entity_name, min_distance = std::numeric_limits::max()]( + const std::string & name, const double distance) mutable { + if (distance < min_distance) { + min_distance = distance; + front_entity_name = name; + } + }; for (const auto & [other_name, other_status] : other_entity_status) { if ( const auto & other_canonicalized_lanelet_pose = other_status.getCanonicalizedLaneletPose()) { @@ -226,20 +233,13 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf other_status.getMapPose().orientation); const auto yaw = math::geometry::convertQuaternionToEulerAngle(quaternion).z; if (std::fabs(yaw) <= front_entity_angle_threshold) { - entities_with_distances.emplace_back(other_name, distance.value()); + try_front_entity(other_name, distance.value()); } } } } - if (!entities_with_distances.empty()) { - auto min_entity_with_distance_it = std::min_element( - entities_with_distances.begin(), entities_with_distances.end(), - [](const auto & lhs, const auto & rhs) { return lhs.second < rhs.second; }); - return min_entity_with_distance_it->first; - } else { - return std::nullopt; - } + return front_entity_name; } auto ActionNode::getEntityStatus(const std::string & target_name) const diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp index fcc7a21bb9f..415694e35bd 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp @@ -161,7 +161,7 @@ auto TrafficLightsBase::getDistanceToActiveTrafficLightStopLine( if (traffic_light_ids.empty()) { return std::nullopt; } - std::set collision_points = {}; + std::optional min_distance{std::nullopt}; for (const auto id : traffic_light_ids) { using Color = traffic_simulator::TrafficLight::Color; using Status = traffic_simulator::TrafficLight::Status; @@ -171,14 +171,13 @@ auto TrafficLightsBase::getDistanceToActiveTrafficLightStopLine( traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)) { const auto collision_point = traffic_simulator::distance::distanceToTrafficLightStopLine(spline, id); - if (collision_point) { - collision_points.insert(collision_point.value()); + if ( + collision_point.has_value() and + (not min_distance.has_value() or collision_point.value() < min_distance.value())) { + min_distance = collision_point; } } } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); + return min_distance; } } // namespace traffic_simulator