From dcaef8d4a98c29f4f875a78130f29fa4cd7d8685 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Fri, 4 Apr 2025 10:13:29 +0200 Subject: [PATCH 1/2] RJD-1509 Filter nearest NPCs to calculate distance --- .../behavior_tree_plugin/action_node.hpp | 2 + .../pedestrian/behavior_tree.hpp | 1 + .../vehicle/behavior_tree.hpp | 1 + .../behavior_tree_plugin/src/action_node.cpp | 39 +++++++++++-------- .../include/do_nothing_plugin/plugin.hpp | 1 + .../behavior/behavior_plugin_base.hpp | 17 ++++---- .../traffic_simulator/entity/entity_base.hpp | 5 +++ .../entity/entity_manager.hpp | 5 ++- .../src/entity/entity_base.cpp | 5 +++ .../src/entity/entity_manager.cpp | 20 ++++++++-- .../src/entity/vehicle_entity.cpp | 1 + 11 files changed, 69 insertions(+), 28 deletions(-) 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 7257c7b6ab5..d839ab4f1a2 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 @@ -86,6 +86,7 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort>("canonicalized_entity_status"), BT::InputPort>("traffic_lights"), BT::InputPort("request"), + BT::InputPort>("distances_map"), BT::OutputPort>("obstacle"), BT::OutputPort("waypoints"), BT::OutputPort("request"), @@ -116,6 +117,7 @@ class ActionNode : public BT::ActionNodeBase std::optional target_speed; EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; + std::shared_ptr distances_map; auto getDistanceToTargetEntity( const math::geometry::CatmullRomSplineInterface & spline, diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index 86d925e8a49..d575109a89b 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -70,6 +70,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index 8333e5ae027..1b5f7649260 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -74,6 +74,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index c970daf30d2..4a397a9356d 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -91,6 +91,9 @@ auto ActionNode::getBlackBoardValues() -> void if (!getInput("route_lanelets", route_lanelets)) { THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode"); } + if (!getInput>("distances_map", distances_map)) { + THROW_SIMULATION_ERROR("failed to get input distances_map in ActionNode"); + } } auto ActionNode::getHorizon() const -> double @@ -237,10 +240,21 @@ auto ActionNode::getDistanceToFrontEntity( auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - std::vector distances; - std::vector entities; - for (const auto & [name, status] : other_entity_status) { - const auto distance = getDistanceToTargetEntity(spline, status); + if (distances_map == nullptr) { + return std::nullopt; + } + std::map rough_distances; + for (const auto & [name_pair, distance] : *distances_map) { + if ( + name_pair.first == canonicalized_entity_status->getName() && distance < spline.getLength()) { + rough_distances.emplace(distance, name_pair.second); + } else if ( + name_pair.second == canonicalized_entity_status->getName() && distance < spline.getLength()) { + rough_distances.emplace(distance, name_pair.first); + } + } + + for (const auto & [rough_distance, name] : rough_distances) { const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(name).getMapPose().orientation); @@ -250,21 +264,14 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf if ( std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <= boost::math::constants::half_pi()) { - if (distance && distance.value() < 40) { - entities.emplace_back(name); - distances.emplace_back(distance.value()); + const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name)); + + if (distance && distance.value() < spline.getLength()) { + return name; } } } - if (entities.size() != distances.size()) { - THROW_SIMULATION_ERROR("size of entities and distances vector does not match."); - } - if (distances.empty()) { - return std::nullopt; - } - std::vector::iterator iter = std::min_element(distances.begin(), distances.end()); - size_t index = std::distance(distances.begin(), iter); - return entities[index]; + return std::nullopt; } auto ActionNode::getDistanceToTargetEntityOnCrosswalk( diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 1804932d741..29475738a35 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -58,6 +58,7 @@ public: \ DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER 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..d8a03be35e6 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 @@ -36,6 +36,8 @@ namespace entity_behavior using EntityStatusDict = std::unordered_map; +using DistancesMap = std::unordered_map, double>; + class BehaviorPluginBase { public: @@ -44,13 +46,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 @@ -74,6 +76,7 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(DistancesMap, "distances_map", std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index bb4b7aa738c..68d2a011b1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -47,6 +47,7 @@ namespace traffic_simulator { namespace entity { +using DistancesMap = std::unordered_map, double>; class EntityBase : public std::enable_shared_from_this { public: @@ -312,6 +313,8 @@ class EntityBase : public std::enable_shared_from_this bool verbose; + void setDistances(const std::shared_ptr & distances); + protected: std::shared_ptr status_; @@ -333,6 +336,8 @@ class EntityBase : public std::enable_shared_from_this std::unique_ptr speed_planner_; + std::shared_ptr distances_map_; + private: virtual auto requestSpeedChangeWithConstantAcceleration( const double target_speed, const speed_change::Transition, const double acceleration, diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index bf0c348024b..2ceb43197c3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -93,8 +93,9 @@ class EntityManager // update auto update(const double current_time, const double step_time) -> void; - auto updateNpcLogic(const std::string & name, const double current_time, const double step_time) - -> const CanonicalizedEntityStatus &; + auto updateNpcLogic( + const std::string & name, const double current_time, const double step_time, + const std::shared_ptr distances) -> const CanonicalizedEntityStatus &; auto updateHdmapMarker() const -> void; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 77ff802745c..ead546366cd 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -851,5 +851,10 @@ auto EntityBase::requestSynchronize( return false; } +void EntityBase::setDistances(const std::shared_ptr & distances) +{ + distances_map_ = distances; +} + } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 72d9aa63efb..aedf978956e 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -75,8 +76,20 @@ auto EntityManager::update(const double current_time, const double step_time) -> entity_ptr->setOtherStatus(all_status); } all_status.clear(); + + std::shared_ptr distances = std::make_shared(); + for (auto && [name_from, entity_from] : entities_) { + for (auto && [name_to, entity_to] : entities_) { + if (const auto pair = std::minmax(name_from, name_to); + distances->find(pair) == distances->end() && name_from != name_to) { + distances->emplace( + pair, math::geometry::getDistance(entity_from->getMapPose(), entity_to->getMapPose())); + } + } + } + for (const auto & [name, entity_ptr] : entities_) { - all_status.try_emplace(name, updateNpcLogic(name, current_time, step_time)); + all_status.try_emplace(name, updateNpcLogic(name, current_time, step_time, distances)); } for (const auto & [name, entity_ptr] : entities_) { entity_ptr->setOtherStatus(all_status); @@ -108,8 +121,8 @@ auto EntityManager::update(const double current_time, const double step_time) -> } auto EntityManager::updateNpcLogic( - const std::string & name, const double current_time, const double step_time) - -> const CanonicalizedEntityStatus & + const std::string & name, const double current_time, const double step_time, + const std::shared_ptr distances) -> const CanonicalizedEntityStatus & { if (configuration_.verbose) { std::cout << "update " << name << " behavior" << std::endl; @@ -117,6 +130,7 @@ auto EntityManager::updateNpcLogic( auto & entity = getEntity(name); // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego if (npc_logic_started_) { + entity.setDistances(distances); entity.onUpdate(current_time, step_time); } else if (entity.is()) { getEgoEntity(name).updateFieldOperatorApplication(); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 75f462e7361..b715924ddb0 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -140,6 +140,7 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) behavior_plugin_ptr_->setTargetSpeed(target_speed_); auto route_lanelets = getRouteLanelets(); behavior_plugin_ptr_->setRouteLanelets(route_lanelets); + behavior_plugin_ptr_->setDistancesMap(distances_map_); // recalculate spline only when input data changes if (previous_route_lanelets_ != route_lanelets) { From 4beca2048e0e912568625297869caa8ff5ab1314 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Tue, 8 Apr 2025 13:35:20 +0200 Subject: [PATCH 2/2] Review changes --- .../behavior_tree_plugin/action_node.hpp | 7 ++- .../pedestrian/behavior_tree.hpp | 2 +- .../pedestrian/pedestrian_action_node.hpp | 4 -- .../vehicle/behavior_tree.hpp | 2 +- .../vehicle/vehicle_action_node.hpp | 2 - .../behavior_tree_plugin/src/action_node.cpp | 63 ++++++++++--------- .../src/pedestrian/pedestrian_action_node.cpp | 4 -- .../src/vehicle/vehicle_action_node.cpp | 4 -- .../include/do_nothing_plugin/plugin.hpp | 2 +- .../behavior/behavior_plugin_base.hpp | 18 +++--- .../traffic_simulator/entity/entity_base.hpp | 6 +- .../entity/entity_manager.hpp | 4 +- .../src/entity/entity_base.cpp | 4 +- .../src/entity/entity_manager.cpp | 30 +++++---- .../src/entity/vehicle_entity.cpp | 2 +- 15 files changed, 79 insertions(+), 75 deletions(-) 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 d839ab4f1a2..5fc46fd05a0 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 @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -86,7 +87,8 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort>("canonicalized_entity_status"), BT::InputPort>("traffic_lights"), BT::InputPort("request"), - BT::InputPort>("distances_map"), + BT::InputPort>("euclidean_distances_map"), + BT::InputPort("behavior_parameter"), BT::OutputPort>("obstacle"), BT::OutputPort("waypoints"), BT::OutputPort("request"), @@ -117,7 +119,8 @@ class ActionNode : public BT::ActionNodeBase std::optional target_speed; EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; - std::shared_ptr distances_map; + std::shared_ptr euclidean_distances_map; + traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; auto getDistanceToTargetEntity( const math::geometry::CatmullRomSplineInterface & spline, diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index d575109a89b..c726d6df948 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -70,7 +70,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) - DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) + DEFINE_GETTER_SETTER(EuclideanDistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp index b7de2b568b7..a1804be6047 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp @@ -33,7 +33,6 @@ class PedestrianActionNode : public ActionNode { // clang-format off return BT::PortsList({ - BT::InputPort("behavior_parameter"), BT::InputPort("pedestrian_parameters"), }) + entity_behavior::ActionNode::providedPorts(); // clang-format on @@ -42,9 +41,6 @@ class PedestrianActionNode : public ActionNode auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus; - -protected: - traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index 1b5f7649260..611a08715d6 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -74,7 +74,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) - DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) + DEFINE_GETTER_SETTER(EuclideanDistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp index 43403ba3c65..8f575f69189 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp @@ -43,7 +43,6 @@ class VehicleActionNode : public ActionNode // clang-format off return BT::PortsList({ BT::InputPort>("reference_trajectory"), - BT::InputPort("behavior_parameter"), BT::InputPort("vehicle_parameters")}) + entity_behavior::ActionNode::providedPorts(); // clang-format on @@ -56,7 +55,6 @@ class VehicleActionNode : public ActionNode const traffic_simulator_msgs::msg::WaypointsArray & waypoints) = 0; protected: - traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; std::shared_ptr reference_trajectory; std::unique_ptr trajectory; diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 4a397a9356d..adc90ebaf7f 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -91,8 +91,13 @@ auto ActionNode::getBlackBoardValues() -> void if (!getInput("route_lanelets", route_lanelets)) { THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode"); } - if (!getInput>("distances_map", distances_map)) { - THROW_SIMULATION_ERROR("failed to get input distances_map in ActionNode"); + if (!getInput>( + "euclidean_distances_map", euclidean_distances_map)) { + THROW_SIMULATION_ERROR("failed to get input euclidean_distances_map in ActionNode"); + } + if (!getInput( + "behavior_parameter", behavior_parameter)) { + behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter(); } } @@ -240,34 +245,36 @@ auto ActionNode::getDistanceToFrontEntity( auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - if (distances_map == nullptr) { - return std::nullopt; - } - std::map rough_distances; - for (const auto & [name_pair, distance] : *distances_map) { - if ( - name_pair.first == canonicalized_entity_status->getName() && distance < spline.getLength()) { - rough_distances.emplace(distance, name_pair.second); - } else if ( - name_pair.second == canonicalized_entity_status->getName() && distance < spline.getLength()) { - rough_distances.emplace(distance, name_pair.first); + if (euclidean_distances_map != nullptr) { + std::map local_euclidean_distances_map; + const double stop_distance = calculateStopDistance(behavior_parameter.dynamic_constraints); + const double horizon = spline.getLength() > stop_distance ? spline.getLength() : stop_distance; + for (const auto & [name_pair, euclidean_distance] : *euclidean_distances_map) { + if (euclidean_distance < horizon) { + if (name_pair.first == canonicalized_entity_status->getName()) { + local_euclidean_distances_map.emplace(euclidean_distance, name_pair.second); + } else if (name_pair.second == canonicalized_entity_status->getName()) { + local_euclidean_distances_map.emplace(euclidean_distance, name_pair.first); + } + } } - } - for (const auto & [rough_distance, name] : rough_distances) { - const auto quat = math::geometry::getRotation( - canonicalized_entity_status->getMapPose().orientation, - other_entity_status.at(name).getMapPose().orientation); - /** - * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity. - */ - if ( - std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <= - boost::math::constants::half_pi()) { - const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name)); - - if (distance && distance.value() < spline.getLength()) { - return name; + for (const auto & [euclidean_distance, name] : local_euclidean_distances_map) { + const auto quaternion = math::geometry::getRotation( + canonicalized_entity_status->getMapPose().orientation, + other_entity_status.at(name).getMapPose().orientation); + /** + * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity. + */ + if ( + std::fabs(math::geometry::convertQuaternionToEulerAngle(quaternion).z) <= + boost::math::constants::half_pi()) { + const auto longitudinal_distance = + getDistanceToTargetEntity(spline, other_entity_status.at(name)); + + if (longitudinal_distance && longitudinal_distance.value() < horizon) { + return name; + } } } } diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index 1443370e8a0..6330e3df1b1 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -30,10 +30,6 @@ PedestrianActionNode::PedestrianActionNode( void PedestrianActionNode::getBlackBoardValues() { ActionNode::getBlackBoardValues(); - if (!getInput( - "behavior_parameter", behavior_parameter)) { - behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter(); - } if (!getInput( "pedestrian_parameters", pedestrian_parameters)) { THROW_SIMULATION_ERROR("failed to get input pedestrian_parameters in PedestrianActionNode"); diff --git a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp index 549ba448d97..3406662ebce 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp @@ -29,10 +29,6 @@ VehicleActionNode::VehicleActionNode(const std::string & name, const BT::NodeCon void VehicleActionNode::getBlackBoardValues() { ActionNode::getBlackBoardValues(); - if (!getInput( - "behavior_parameter", behavior_parameter)) { - behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter(); - } if (!getInput( "vehicle_parameters", vehicle_parameters)) { THROW_SIMULATION_ERROR("failed to get input vehicle_parameters in VehicleActionNode"); diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 29475738a35..2bd473f4f6e 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -58,7 +58,7 @@ public: \ DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) - DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr) + DEFINE_GETTER_SETTER(EuclideanDistancesMap, std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER 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 d8a03be35e6..1706a303f60 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 @@ -36,7 +36,7 @@ namespace entity_behavior using EntityStatusDict = std::unordered_map; -using DistancesMap = std::unordered_map, double>; +using EuclideanDistancesMap = std::unordered_map, double>; class BehaviorPluginBase { @@ -46,13 +46,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 @@ -76,7 +76,7 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) - DEFINE_GETTER_SETTER(DistancesMap, "distances_map", std::shared_ptr) + DEFINE_GETTER_SETTER(EuclideanDistancesMap, "euclidean_distances_map", std::shared_ptr) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 68d2a011b1a..f87c3816078 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -47,7 +47,7 @@ namespace traffic_simulator { namespace entity { -using DistancesMap = std::unordered_map, double>; +using EuclideanDistancesMap = std::unordered_map, double>; class EntityBase : public std::enable_shared_from_this { public: @@ -313,7 +313,7 @@ class EntityBase : public std::enable_shared_from_this bool verbose; - void setDistances(const std::shared_ptr & distances); + void setEuclideanDistancesMap(const std::shared_ptr & distances); protected: std::shared_ptr status_; @@ -336,7 +336,7 @@ class EntityBase : public std::enable_shared_from_this std::unique_ptr speed_planner_; - std::shared_ptr distances_map_; + std::shared_ptr euclidean_distances_map_; private: virtual auto requestSpeedChangeWithConstantAcceleration( diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 2ceb43197c3..af882ec210d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -95,7 +95,7 @@ class EntityManager auto updateNpcLogic( const std::string & name, const double current_time, const double step_time, - const std::shared_ptr distances) -> const CanonicalizedEntityStatus &; + const std::shared_ptr & distances) -> const CanonicalizedEntityStatus &; auto updateHdmapMarker() const -> void; @@ -270,6 +270,8 @@ class EntityManager } } + auto calculateEuclideanDistances() -> std::shared_ptr; + private: /* */ Configuration configuration_; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index ead546366cd..58cd7e5ac0c 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -851,9 +851,9 @@ auto EntityBase::requestSynchronize( return false; } -void EntityBase::setDistances(const std::shared_ptr & distances) +void EntityBase::setEuclideanDistancesMap(const std::shared_ptr & distances) { - distances_map_ = distances; + euclidean_distances_map_ = distances; } } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index aedf978956e..565fcd9d433 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -77,16 +77,7 @@ auto EntityManager::update(const double current_time, const double step_time) -> } all_status.clear(); - std::shared_ptr distances = std::make_shared(); - for (auto && [name_from, entity_from] : entities_) { - for (auto && [name_to, entity_to] : entities_) { - if (const auto pair = std::minmax(name_from, name_to); - distances->find(pair) == distances->end() && name_from != name_to) { - distances->emplace( - pair, math::geometry::getDistance(entity_from->getMapPose(), entity_to->getMapPose())); - } - } - } + std::shared_ptr distances = calculateEuclideanDistances(); for (const auto & [name, entity_ptr] : entities_) { all_status.try_emplace(name, updateNpcLogic(name, current_time, step_time, distances)); @@ -122,7 +113,7 @@ auto EntityManager::update(const double current_time, const double step_time) -> auto EntityManager::updateNpcLogic( const std::string & name, const double current_time, const double step_time, - const std::shared_ptr distances) -> const CanonicalizedEntityStatus & + const std::shared_ptr & distances) -> const CanonicalizedEntityStatus & { if (configuration_.verbose) { std::cout << "update " << name << " behavior" << std::endl; @@ -130,7 +121,7 @@ auto EntityManager::updateNpcLogic( auto & entity = getEntity(name); // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego if (npc_logic_started_) { - entity.setDistances(distances); + entity.setEuclideanDistancesMap(distances); entity.onUpdate(current_time, step_time); } else if (entity.is()) { getEgoEntity(name).updateFieldOperatorApplication(); @@ -410,5 +401,20 @@ auto EntityManager::getWaypoints(const std::string & name) return entities_.at(name)->getWaypoints(); } } + +auto EntityManager::calculateEuclideanDistances() -> std::shared_ptr +{ + std::shared_ptr distances = std::make_shared(); + for (auto && [name_from, entity_from] : entities_) { + for (auto && [name_to, entity_to] : entities_) { + if (const auto pair = std::minmax(name_from, name_to); + distances->find(pair) == distances->end() && name_from != name_to) { + distances->emplace( + pair, math::geometry::getDistance(entity_from->getMapPose(), entity_to->getMapPose())); + } + } + } + return distances; +} } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index b715924ddb0..0edceb591f1 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -140,7 +140,7 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) behavior_plugin_ptr_->setTargetSpeed(target_speed_); auto route_lanelets = getRouteLanelets(); behavior_plugin_ptr_->setRouteLanelets(route_lanelets); - behavior_plugin_ptr_->setDistancesMap(distances_map_); + behavior_plugin_ptr_->setEuclideanDistancesMap(euclidean_distances_map_); // recalculate spline only when input data changes if (previous_route_lanelets_ != route_lanelets) {