RJD-1509 Filter nearest NPCs to calculate distance

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -31,6 +31,7 @@
 #include <traffic_simulator/traffic_lights/traffic_lights.hpp>
 #include <traffic_simulator/utils/lanelet_map.hpp>
 #include <traffic_simulator/utils/pose.hpp>
+#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/obstacle.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
 #include <unordered_map>
@@ -86,6 +87,8 @@ class ActionNode : public BT::ActionNodeBase
       BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("canonicalized_entity_status"),
       BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightsBase>>("traffic_lights"),
       BT::InputPort<traffic_simulator::behavior::Request>("request"),
+      BT::InputPort<std::shared_ptr<EuclideanDistancesMap>>("euclidean_distances_map"),
+      BT::InputPort<traffic_simulator_msgs::msg::BehaviorParameter>("behavior_parameter"),
       BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>("obstacle"),
       BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints"),
       BT::OutputPort<traffic_simulator::behavior::Request>("request"),
@@ -116,6 +119,8 @@ class ActionNode : public BT::ActionNodeBase
   std::optional<double> target_speed;
   EntityStatusDict other_entity_status;
   lanelet::Ids route_lanelets;
+  std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map;
+  traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter;
 
   auto getDistanceToTargetEntity(
     const math::geometry::CatmullRomSplineInterface & spline,

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<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // clang-format on
 
 #undef DEFINE_GETTER_SETTER

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<traffic_simulator_msgs::msg::BehaviorParameter>("behavior_parameter"),
       BT::InputPort<traffic_simulator_msgs::msg::PedestrianParameters>("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
 

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<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // clang-format on
 #undef DEFINE_GETTER_SETTER
 

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<std::shared_ptr<math::geometry::CatmullRomSpline>>("reference_trajectory"),
-        BT::InputPort<traffic_simulator_msgs::msg::BehaviorParameter>("behavior_parameter"),
         BT::InputPort<traffic_simulator_msgs::msg::VehicleParameters>("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<math::geometry::CatmullRomSpline> reference_trajectory;
   std::unique_ptr<math::geometry::CatmullRomSubspline> trajectory;

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -91,6 +91,14 @@ auto ActionNode::getBlackBoardValues() -> void
   if (!getInput<lanelet::Ids>("route_lanelets", route_lanelets)) {
     THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode");
   }
+  if (!getInput<std::shared_ptr<EuclideanDistancesMap>>(
+        "euclidean_distances_map", euclidean_distances_map)) {
+    THROW_SIMULATION_ERROR("failed to get input euclidean_distances_map in ActionNode");
+  }
+  if (!getInput<traffic_simulator_msgs::msg::BehaviorParameter>(
+        "behavior_parameter", behavior_parameter)) {
+    behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter();
+  }
 }
 
 auto ActionNode::getHorizon() const -> double
@@ -237,34 +245,40 @@ auto ActionNode::getDistanceToFrontEntity(
 auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const
   -> std::optional<std::string>
 {
-  std::vector<double> distances;
-  std::vector<std::string> entities;
-  for (const auto & [name, status] : other_entity_status) {
-    const auto distance = getDistanceToTargetEntity(spline, status);
-    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<double>()) {
-      if (distance && distance.value() < 40) {
-        entities.emplace_back(name);
-        distances.emplace_back(distance.value());
+  if (euclidean_distances_map != nullptr) {
+    std::map<double, std::string> 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 & [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<double>()) {
+        const auto longitudinal_distance =
+          getDistanceToTargetEntity(spline, other_entity_status.at(name));
+
+        if (longitudinal_distance && longitudinal_distance.value() < horizon) {
+          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<double>::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(

simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp

@@ -30,10 +30,6 @@ PedestrianActionNode::PedestrianActionNode(
 void PedestrianActionNode::getBlackBoardValues()
 {
   ActionNode::getBlackBoardValues();
-  if (!getInput<traffic_simulator_msgs::msg::BehaviorParameter>(
-        "behavior_parameter", behavior_parameter)) {
-    behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter();
-  }
   if (!getInput<traffic_simulator_msgs::msg::PedestrianParameters>(
         "pedestrian_parameters", pedestrian_parameters)) {
     THROW_SIMULATION_ERROR("failed to get input pedestrian_parameters in PedestrianActionNode");

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<traffic_simulator_msgs::msg::BehaviorParameter>(
-        "behavior_parameter", behavior_parameter)) {
-    behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter();
-  }
   if (!getInput<traffic_simulator_msgs::msg::VehicleParameters>(
         "vehicle_parameters", vehicle_parameters)) {
     THROW_SIMULATION_ERROR("failed to get input vehicle_parameters in VehicleActionNode");

simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp

@@ -58,6 +58,7 @@ public:                                         \
   DEFINE_GETTER_SETTER(TrafficLights,                                    std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // clang-format on
 #undef DEFINE_GETTER_SETTER
 

simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp

@@ -36,6 +36,8 @@ namespace entity_behavior
 using EntityStatusDict =
   std::unordered_map<std::string, traffic_simulator::CanonicalizedEntityStatus>;
 
+using EuclideanDistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
+
 class BehaviorPluginBase
 {
 public:
@@ -74,6 +76,7 @@ class BehaviorPluginBase
   DEFINE_GETTER_SETTER(TrafficLights,                                    "traffic_lights",                                 std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                "vehicle_parameters",                             traffic_simulator_msgs::msg::VehicleParameters)
   DEFINE_GETTER_SETTER(Waypoints,                                        "waypoints",                                      traffic_simulator_msgs::msg::WaypointsArray)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            "euclidean_distances_map",                        std::shared_ptr<EuclideanDistancesMap>)
   // clang-format on
 #undef DEFINE_GETTER_SETTER
 };

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -47,6 +47,7 @@ namespace traffic_simulator
 {
 namespace entity
 {
+using EuclideanDistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
 class EntityBase : public std::enable_shared_from_this<EntityBase>
 {
 public:
@@ -312,6 +313,8 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
 
   bool verbose;
 
+  void setEuclideanDistancesMap(const std::shared_ptr<EuclideanDistancesMap> & distances);
+
 protected:
   std::shared_ptr<CanonicalizedEntityStatus> status_;
 
@@ -333,6 +336,8 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
   std::unique_ptr<traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner>
     speed_planner_;
 
+  std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map_;
+
 private:
   virtual auto requestSpeedChangeWithConstantAcceleration(
     const double target_speed, const speed_change::Transition, const double acceleration,

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<EuclideanDistancesMap> & distances) -> const CanonicalizedEntityStatus &;
 
   auto updateHdmapMarker() const -> void;
 
@@ -269,6 +270,8 @@ class EntityManager
     }
   }
 
+  auto calculateEuclideanDistances() -> std::shared_ptr<EuclideanDistancesMap>;
+
 private:
   /* */ Configuration configuration_;
 

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -851,5 +851,10 @@ auto EntityBase::requestSynchronize(
   return false;
 }
 
+void EntityBase::setEuclideanDistancesMap(const std::shared_ptr<EuclideanDistancesMap> & distances)
+{
+  euclidean_distances_map_ = distances;
+}
+
 }  // namespace entity
 }  // namespace traffic_simulator

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 <geometry/distance.hpp>
 #include <geometry/vector3/operator.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <traffic_simulator/entity/entity_manager.hpp>
@@ -75,8 +76,11 @@ auto EntityManager::update(const double current_time, const double step_time) ->
     entity_ptr->setOtherStatus(all_status);
   }
   all_status.clear();
+
+  std::shared_ptr<EuclideanDistancesMap> distances = calculateEuclideanDistances();
+
   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,15 +112,16 @@ 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<EuclideanDistancesMap> & distances) -> const CanonicalizedEntityStatus &
 {
   if (configuration_.verbose) {
     std::cout << "update " << name << " behavior" << std::endl;
   }
   auto & entity = getEntity(name);
   // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego
   if (npc_logic_started_) {
+    entity.setEuclideanDistancesMap(distances);
     entity.onUpdate(current_time, step_time);
   } else if (entity.is<entity::EgoEntity>()) {
     getEgoEntity(name).updateFieldOperatorApplication();
@@ -396,5 +401,20 @@ auto EntityManager::getWaypoints(const std::string & name)
     return entities_.at(name)->getWaypoints();
   }
 }
+
+auto EntityManager::calculateEuclideanDistances() -> std::shared_ptr<EuclideanDistancesMap>
+{
+  std::shared_ptr<EuclideanDistancesMap> distances = std::make_shared<EuclideanDistancesMap>();
+  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

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_->setEuclideanDistancesMap(euclidean_distances_map_);
 
   // recalculate spline only when input data changes
   if (previous_route_lanelets_ != route_lanelets) {