RJD-1930 Add see_around for WalkStraightAction

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

@@ -33,7 +33,6 @@ namespace entity_behavior
 {
 namespace pedestrian
 {
-enum class SeeAroundMode { blind, aware };
 
 class FollowLaneAction : public entity_behavior::PedestrianActionNode
 {
@@ -47,9 +46,6 @@ 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/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp

@@ -24,6 +24,7 @@
 
 namespace entity_behavior
 {
+enum class SeeAroundMode { blind, aware };
 class PedestrianActionNode : public ActionNode
 {
 public:
@@ -41,6 +42,9 @@ class PedestrianActionNode : public ActionNode
   auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const
     -> traffic_simulator::EntityStatus;
   auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus;
+
+protected:
+  SeeAroundMode should_respect_see_around;
 };
 }  // namespace entity_behavior
 

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/walk_straight_action.hpp

@@ -51,6 +51,12 @@ class WalkStraightAction : public entity_behavior::PedestrianActionNode
   {
     return entity_behavior::PedestrianActionNode::providedPorts();
   }
+  bool detectObstacleInFront(const bool see_around) const;
+
+private:
+  bool isEntityColliding(
+    const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status,
+    const double & detection_horizon) const;
 };
 }  // namespace pedestrian
 }  // namespace entity_behavior

simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp

@@ -26,18 +26,6 @@ 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(); }

simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp

@@ -34,6 +34,19 @@ void PedestrianActionNode::getBlackBoardValues()
         "pedestrian_parameters", pedestrian_parameters)) {
     THROW_SIMULATION_ERROR("failed to get input pedestrian_parameters in PedestrianActionNode");
   }
+
+  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"));
 }
 
 auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) const

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

@@ -24,6 +24,14 @@
 // limitations under the License.
 
 #include <behavior_tree_plugin/pedestrian/walk_straight_action.hpp>
+#include <geometry/bounding_box.hpp>
+#include <geometry/distance.hpp>
+#include <geometry/quaternion/get_normal_vector.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <geometry/transform.hpp>
+#include <geometry/vector3/norm.hpp>
+#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <string>
 
 namespace entity_behavior
@@ -38,6 +46,93 @@ WalkStraightAction::WalkStraightAction(
 
 void WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); }
 
+bool isPointInFront(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point)
+{
+  const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;
+  const auto angle_to_target_point =
+    std::atan2(target_point.y - pose.position.y, target_point.x - pose.position.x);
+  const auto yaw_difference = std::atan2(
+    std::sin(angle_to_target_point - self_yaw), std::cos(angle_to_target_point - self_yaw));
+  return std::fabs(yaw_difference) <= boost::math::constants::half_pi<double>();
+}
+
+bool WalkStraightAction::isEntityColliding(
+  const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status,
+  const double & detection_horizon) const
+{
+  using math::geometry::operator*;
+
+  const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();
+  const auto bounding_box_map_points = math::geometry::transformPoints(
+    pedestrian_pose,
+    math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));
+  std::vector<geometry_msgs::msg::Point> bounding_box_front_points;
+  for (const auto & point : bounding_box_map_points) {
+    if (isPointInFront(pedestrian_pose, point)) {
+      bounding_box_front_points.push_back(point);
+    }
+  }
+
+  const auto detection_horizon_vector =
+    math::geometry::normalize(
+      math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) *
+    detection_horizon;
+  std::vector<geometry_msgs::msg::Point> detection_area_points;
+
+  for (const auto & point : bounding_box_front_points) {
+    detection_area_points.push_back(point);
+    geometry_msgs::msg::Point front_detection_point;
+    front_detection_point.x = point.x + detection_horizon_vector.x;
+    front_detection_point.y = point.y + detection_horizon_vector.z;
+    front_detection_point.z = point.z + detection_horizon_vector.y;
+    detection_area_points.push_back(front_detection_point);
+  }
+
+  const auto detection_area_polygon = math::geometry::toBoostPolygon(detection_area_points);
+  const auto other_entity_polygon =
+    math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());
+
+  if (
+    boost::geometry::intersects(detection_area_polygon, other_entity_polygon) ||
+    boost::geometry::intersects(other_entity_polygon, detection_area_polygon)) {
+    return true;
+  } else if (boost::geometry::disjoint(detection_area_polygon, other_entity_polygon)) {
+    return false;
+  } else {
+    return true;
+  }
+}
+
+bool WalkStraightAction::detectObstacleInFront(const bool see_around) const
+{
+  auto isObstacleInFrontOfPedestrian = [this](const double & detection_horizon) {
+    using math::geometry::operator-;
+    const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();
+    for (const auto & [_, entity_status] : other_entity_status_) {
+      const auto & other_position = entity_status.getMapPose().position;
+      if (const auto distance = math::geometry::norm(other_position - pedestrian_pose.position);
+          distance <= detection_horizon) {
+        if (
+          isPointInFront(pedestrian_pose, other_position) &&
+          isEntityColliding(entity_status, detection_horizon)) {
+          return true;
+        }
+      }
+    }
+    return false;
+  };
+
+  if (not see_around || should_respect_see_around == SeeAroundMode::blind) {
+    return false;
+  } else {
+    const double detection_horizon =
+      calculateStopDistance(behavior_parameter_.dynamic_constraints) +
+      canonicalized_entity_status_->getBoundingBox().dimensions.x;
+    return isObstacleInFrontOfPedestrian(detection_horizon);
+  }
+}
+
 bool WalkStraightAction::checkPreconditions()
 {
   return request_ == traffic_simulator::behavior::Request::WALK_STRAIGHT;
@@ -48,6 +143,10 @@ BT::NodeStatus WalkStraightAction::doAction()
   if (!target_speed_) {
     target_speed_ = 1.111;
   }
+
+  const auto is_obstacle_in_front = detectObstacleInFront(behavior_parameter_.see_around);
+  target_speed_ = is_obstacle_in_front ? 0.0 : target_speed_;
+
   setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed_.value()));
   return BT::NodeStatus::RUNNING;
 }