Update pedestrian consideration to factor in distance from ego vehicle

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_lane_action.hpp

@@ -19,6 +19,7 @@
 #include <behaviortree_cpp_v3/bt_factory.h>
 
 #include <behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp>
+#include <geometry/vector3/norm.hpp>
 #include <geometry/vector3/operator.hpp>
 #include <get_parameter/get_parameter.hpp>
 #include <memory>

simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

@@ -53,22 +53,33 @@ bool FollowLaneAction::detectObstacleInLane(
     return false;
   }
 
-  auto hasObstacleInPedestrianLanes = [this](const lanelet::Ids pedestrian_lanes_local) {
-    lanelet::Ids other_entity_lane_ids;
-    for (const auto & [_, status] : other_entity_status) {
-      if (status.isInLanelet()) {
+  auto hasObstacleInPedestrianLanes =
+    [this](const lanelet::Ids pedestrian_lanes_local, const double max_detect_length) {
+      using math::geometry::operator-;
+      const auto & pedestrian_position = canonicalized_entity_status->getMapPose().position;
+      lanelet::Ids other_entity_lane_ids;
+      for (const auto & [_, status] : other_entity_status) {
+        if (status.getType().type != traffic_simulator_msgs::msg::EntityType::EGO) {
+          continue;
+        }
+        if (!status.isInLanelet()) {
+          continue;
+        }
+        const auto norm = math::geometry::norm(status.getMapPose().position - pedestrian_position);
+        if (!(norm < max_detect_length)) {
+          continue;
+        }
         other_entity_lane_ids.push_back(status.getLaneletId());
       }
-    }
-    std::unordered_set<lanelet::Id> other_lane_id_set(
-      other_entity_lane_ids.begin(), other_entity_lane_ids.end());
-    for (const auto & pedestrian_lane : pedestrian_lanes_local) {
-      if (other_lane_id_set.count(pedestrian_lane)) {
-        return true;
+      std::unordered_set<lanelet::Id> other_lane_id_set(
+        other_entity_lane_ids.begin(), other_entity_lane_ids.end());
+      for (const auto & pedestrian_lane : pedestrian_lanes_local) {
+        if (other_lane_id_set.count(pedestrian_lane)) {
+          return true;
+        }
       }
-    }
-    return false;
-  };
+      return false;
+    };
 
   auto hasObstacleInFrontOfPedestrian = [this]() {
     using math::geometry::operator-;
@@ -86,7 +97,7 @@ bool FollowLaneAction::detectObstacleInLane(
     return false;
   };
 
-  if (hasObstacleInPedestrianLanes(pedestrian_lanes) && hasObstacleInFrontOfPedestrian()) {
+  if (hasObstacleInPedestrianLanes(pedestrian_lanes, 10) && hasObstacleInFrontOfPedestrian()) {
     return true;
   } else {
     return false;