Merge pull request #1566 from tier4/feature/pedestrian_awareness

Feature/pedestrian awareness

Author: hakuturu583

docs/developer_guide/NPCBehavior.md

@@ -159,6 +159,29 @@ graph TD
 | FollowLane   | Entity following the lane which it is exist.      |         |         |
 | WalkStraight | Entity walk forward and without considering lane. |         |         |
 
+### Property `isBlind`
+
+**Summary** - Specifies whether the behavior takes surrounding entities into consideration.
+
+**Purpose** - Prevents specific scenarios from failing, such as a pedestrian colliding with a stopped vehicle, by behaving considerately toward surrounding entities. 
+
+**Specification** - The vehicle stops only when it enters a lane currently occupied by another entity and the entity is located ahead.
+
+**Note** - For pedestrians, this feature is disabled by default to maintain backward compatibility. It can be enabled by setting the `pedestrian_ignore_see_around` to `aware` in the launch file execution options.
+
+**Default behavior** - If the property is not specified, the default value is `"false"`.
+
+**Example** -
+```
+        ObjectController:
+          Controller:
+            name: '...'
+            Properties:
+              Property:
+                - name: "isBlind"
+                  value: "false"
+```
+
 ## Pedestrian NPC (with Do-Nothing)
 
 When this behavior is used, entity can only be moved by specifying its pose, velocity, acceleration, jerk, etc. via the `API::setEntityStatus` function, etc.  

openscenario/openscenario_interpreter/src/syntax/add_entity_action.cpp

@@ -139,6 +139,7 @@ try {
       }
 
       TeleportAction::teleport(entity_ref, position);
+      AssignControllerAction(entity.as<ScenarioObject>().object_controller)(entity_ref);
     },
     [&](const MiscObject & misc_object) {
       if (position.is<WorldPosition>()) {

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

@@ -19,7 +19,10 @@
 #include <behaviortree_cpp_v3/bt_factory.h>
 
 #include <behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp>
+#include <geometry/vector3/operator.hpp>
+#include <get_parameter/get_parameter.hpp>
 #include <memory>
+#include <scenario_simulator_exception/exception.hpp>
 #include <string>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
@@ -29,6 +32,8 @@ namespace entity_behavior
 {
 namespace pedestrian
 {
+enum class SeeAroundMode { blind, aware };
+
 class FollowLaneAction : public entity_behavior::PedestrianActionNode
 {
 public:
@@ -39,6 +44,10 @@ class FollowLaneAction : public entity_behavior::PedestrianActionNode
   {
     return entity_behavior::PedestrianActionNode::providedPorts();
   }
+  bool detectObstacleInLane(const lanelet::Ids pedestrian_lanes, const bool see_around) const;
+
+private:
+  SeeAroundMode should_respect_see_around;
 };
 }  // namespace pedestrian
 }  // namespace entity_behavior

simulation/behavior_tree_plugin/package.xml

@@ -11,6 +11,7 @@
 
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry</depend>
+  <depend>get_parameter</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>traffic_simulator</depend>

simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

@@ -26,10 +26,73 @@ namespace pedestrian
 FollowLaneAction::FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config)
 : entity_behavior::PedestrianActionNode(name, config)
 {
+  auto parameterToSeeAroundMode = [](std::string_view parameter) {
+    if (parameter == "blind") {
+      return SeeAroundMode::blind;
+    } else if (parameter == "aware") {
+      return SeeAroundMode::aware;
+    } else {
+      THROW_SIMULATION_ERROR("Unknown see_around mode. It must be \"blind\" or \"aware\".");
+    }
+  };
+
+  should_respect_see_around = parameterToSeeAroundMode(
+    common::getParameter<std::string>("pedestrian_ignore_see_around", "blind"));
 }
 
 void FollowLaneAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); }
 
+bool FollowLaneAction::detectObstacleInLane(
+  const lanelet::Ids pedestrian_lanes, const bool see_around) const
+{
+  if (should_respect_see_around == SeeAroundMode::blind) {
+    return false;
+  }
+
+  if (!see_around) {
+    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()) {
+        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;
+      }
+    }
+    return false;
+  };
+
+  auto hasObstacleInFrontOfPedestrian = [this]() {
+    using math::geometry::operator-;
+
+    const auto & pedestrian_position = canonicalized_entity_status->getMapPose().position;
+
+    for (const auto & [_, entity_status] : other_entity_status) {
+      const auto & other_position = entity_status.getMapPose().position;
+      const auto relative_position = other_position - pedestrian_position;
+      const double relative_angle_rad = std::atan2(relative_position.y, relative_position.x);
+      if (relative_angle_rad > 0) {
+        return true;
+      }
+    }
+    return false;
+  };
+
+  if (hasObstacleInPedestrianLanes(pedestrian_lanes) && hasObstacleInFrontOfPedestrian()) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
 BT::NodeStatus FollowLaneAction::tick()
 {
   getBlackBoardValues();
@@ -47,6 +110,11 @@ BT::NodeStatus FollowLaneAction::tick()
   if (!target_speed) {
     target_speed = hdmap_utils->getSpeedLimit(following_lanelets);
   }
+
+  const auto obstacle_detector_result =
+    detectObstacleInLane(following_lanelets, behavior_parameter.see_around);
+  target_speed = obstacle_detector_result ? 0.0 : target_speed;
+
   setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value()));
   return BT::NodeStatus::RUNNING;
 }

test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py

@@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
     output_directory                    = LaunchConfiguration("output_directory",                       default=Path("/tmp"))
     override_parameters                 = LaunchConfiguration("override_parameters",                    default="")
     parameter_file_path                 = LaunchConfiguration("parameter_file_path",                    default=Path(get_package_share_directory("scenario_test_runner")) / "config/parameters.yaml")
+    pedestrian_ignore_see_around        = LaunchConfiguration("pedestrian_ignore_see_around",           default="blind")
     port                                = LaunchConfiguration("port",                                   default=5555)
     publish_empty_context               = LaunchConfiguration("publish_empty_context",                  default=False)
     record                              = LaunchConfiguration("record",                                 default=True)
@@ -114,6 +115,7 @@ def launch_setup(context, *args, **kwargs):
     print(f"output_directory                    := {output_directory.perform(context)}")
     print(f"override_parameters                 := {override_parameters.perform(context)}")
     print(f"parameter_file_path                 := {parameter_file_path.perform(context)}")
+    print(f"pedestrian_ignore_see_around        := {pedestrian_ignore_see_around.perform(context)}")
     print(f"port                                := {port.perform(context)}")
     print(f"publish_empty_context               := {publish_empty_context.perform(context)}")
     print(f"record                              := {record.perform(context)}")
@@ -142,6 +144,7 @@ def make_parameters():
             {"consider_pose_by_road_slope": consider_pose_by_road_slope},
             {"initialize_duration": initialize_duration},
             {"launch_autoware": launch_autoware},
+            {"pedestrian_ignore_see_around": pedestrian_ignore_see_around},
             {"port": port},
             {"publish_empty_context" : publish_empty_context},
             {"record": record},
@@ -200,6 +203,7 @@ def collect_prefixed_parameters():
         DeclareLaunchArgument("launch_rviz",                         default_value=launch_rviz                        ),
         DeclareLaunchArgument("output_directory",                    default_value=output_directory                   ),
         DeclareLaunchArgument("parameter_file_path",                 default_value=parameter_file_path                ),
+        DeclareLaunchArgument("pedestrian_ignore_see_around",        default_value=pedestrian_ignore_see_around       ),
         DeclareLaunchArgument("publish_empty_context",               default_value=publish_empty_context              ),
         DeclareLaunchArgument("rviz_config",                         default_value=rviz_config                        ),
         DeclareLaunchArgument("scenario",                            default_value=scenario                           ),