Review changes

gmajrobotec · gmajrobotec · commit 5b6c04603d63 · 2025-04-10T12:10:38.000+02:00
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
@@ -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,7 +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<DistancesMap>>("distances_map"),
+      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"),
@@ -117,7 +119,8 @@ class ActionNode : public BT::ActionNodeBase
   std::optional<double> target_speed;
   EntityStatusDict other_entity_status;
   lanelet::Ids route_lanelets;
-  std::shared_ptr<DistancesMap> distances_map;
+  std::shared_ptr<EuclideanDistancesMap> 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
@@ -70,7 +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(DistancesMap,                                     std::shared_ptr<DistancesMap>)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // 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
@@ -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
 
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
@@ -74,7 +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(DistancesMap,                                     std::shared_ptr<DistancesMap>)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // 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
@@ -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;
diff --git 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<lanelet::Ids>("route_lanelets", route_lanelets)) {
     THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode");
   }
-  if (!getInput<std::shared_ptr<DistancesMap>>("distances_map", distances_map)) {
-    THROW_SIMULATION_ERROR("failed to get input distances_map 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();
   }
 }
 
@@ -240,34 +245,35 @@ auto ActionNode::getDistanceToFrontEntity(
 auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const
   -> std::optional<std::string>
 {
-  if (distances_map == nullptr) {
-    return std::nullopt;
-  }
-  std::map<double, std::string> 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<double, std::string> euclidean_distances;
+    double stopDistance = calculateStopDistance(behavior_parameter.dynamic_constraints);
+    double horizon = spline.getLength() > stopDistance ? spline.getLength() : stopDistance;
+    for (const auto & [name_pair, euclidean_distance] : *euclidean_distances_map) {
+      if (euclidean_distance < horizon) {
+        if (name_pair.first == canonicalized_entity_status->getName()) {
+          euclidean_distances.emplace(euclidean_distance, name_pair.second);
+        } else if (name_pair.second == canonicalized_entity_status->getName()) {
+          euclidean_distances.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<double>()) {
-      const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name));
-
-      if (distance && distance.value() < spline.getLength()) {
-        return name;
+    for (const auto & [euclidean_distance, name] : euclidean_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<double>()) {
+        const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name));
+
+        if (distance && 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
@@ -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");
diff --git 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<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");
diff --git 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<traffic_simulator::TrafficLightsBase>)
   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<DistancesMap>)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            std::shared_ptr<EuclideanDistancesMap>)
   // 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
@@ -36,7 +36,7 @@ namespace entity_behavior
 using EntityStatusDict =
   std::unordered_map<std::string, traffic_simulator::CanonicalizedEntityStatus>;
 
-using DistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
+using EuclideanDistancesMap = std::unordered_map<std::pair<std::string, std::string>, 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<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(DistancesMap,                                     "distances_map",                                  std::shared_ptr<DistancesMap>)
+  DEFINE_GETTER_SETTER(EuclideanDistancesMap,                            "euclidean_distances_map",                        std::shared_ptr<EuclideanDistancesMap>)
   // 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
@@ -47,7 +47,7 @@ namespace traffic_simulator
 {
 namespace entity
 {
-using DistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
+using EuclideanDistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
 class EntityBase : public std::enable_shared_from_this<EntityBase>
 {
 public:
@@ -313,7 +313,7 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
 
   bool verbose;
 
-  void setDistances(const std::shared_ptr<DistancesMap> & distances);
+  void setDistances(const std::shared_ptr<EuclideanDistancesMap> & distances);
 
 protected:
   std::shared_ptr<CanonicalizedEntityStatus> status_;
@@ -336,7 +336,7 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
   std::unique_ptr<traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner>
     speed_planner_;
 
-  std::shared_ptr<DistancesMap> distances_map_;
+  std::shared_ptr<EuclideanDistancesMap> 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
@@ -95,7 +95,7 @@ class EntityManager
 
   auto updateNpcLogic(
     const std::string & name, const double current_time, const double step_time,
-    const std::shared_ptr<DistancesMap> distances) -> const CanonicalizedEntityStatus &;
+    const std::shared_ptr<EuclideanDistancesMap> & distances) -> const CanonicalizedEntityStatus &;
 
   auto updateHdmapMarker() const -> void;
 
@@ -270,6 +270,8 @@ class EntityManager
     }
   }
 
+  auto calculateEuclideanDistances() -> std::shared_ptr<EuclideanDistancesMap>;
+
 private:
   /* */ Configuration configuration_;
 
diff --git 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<DistancesMap> & distances)
+void EntityBase::setDistances(const std::shared_ptr<EuclideanDistancesMap> & 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
@@ -77,16 +77,7 @@ auto EntityManager::update(const double current_time, const double step_time) ->
   }
   all_status.clear();
 
-  std::shared_ptr<DistancesMap> distances = std::make_shared<DistancesMap>();
-  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<EuclideanDistancesMap> 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<DistancesMap> distances) -> const CanonicalizedEntityStatus &
+  const std::shared_ptr<EuclideanDistancesMap> & distances) -> const CanonicalizedEntityStatus &
 {
   if (configuration_.verbose) {
     std::cout << "update " << name << " behavior" << std::endl;
@@ -410,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
diff --git 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) {
