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,7 @@ class WalkStraightAction : public entity_behavior::PedestrianActionNode
   {
     return entity_behavior::PedestrianActionNode::providedPorts();
   }
+  bool detectObstacleInFront(const bool see_around) const;
 };
 }  // namespace pedestrian
 }  // namespace entity_behavior

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
@@ -34,10 +42,102 @@ WalkStraightAction::WalkStraightAction(
   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 WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); }
 
+bool checkPointIsInfront(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point)
+{
+  const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;
+  const auto dx = point.x - pose.position.x;
+  const auto dy = point.y - pose.position.y;
+  const auto vec_yaw = std::atan2(dy, dx);
+  const auto yaw_diff = std::atan2(std::sin(vec_yaw - self_yaw), std::cos(vec_yaw - self_yaw));
+
+  return std::fabs(yaw_diff) <= boost::math::constants::half_pi<double>();
+}
+
+bool WalkStraightAction::detectObstacleInFront(const bool see_around) const
+{
+  if (should_respect_see_around == SeeAroundMode::blind) {
+    return false;
+  }
+
+  if (!see_around) {
+    return false;
+  }
+
+  auto hasObstacleInFrontOfPedestrian = [this](double distance) {
+    using math::geometry::operator-;
+    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;
+      const auto norm = math::geometry::norm(other_position - pedestrian_pose.position);
+      if (norm > distance) continue;
+
+      if (checkPointIsInfront(pedestrian_pose, other_position)) {
+        // is in front of pedestrian check if collide
+        const auto bounding_box_map_points = math::geometry::transformPoints(
+          pedestrian_pose,
+          math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));
+
+        std::vector<geometry_msgs::msg::Point> front_points;
+        std::vector<geometry_msgs::msg::Point> check_area_points;
+        for (const auto & point : bounding_box_map_points) {
+          if (checkPointIsInfront(pedestrian_pose, point)) {
+            front_points.push_back(point);
+          }
+        }
+        auto orientation_vector =
+          math::geometry::normalize(
+            math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) *
+          distance;
+        for (auto & point : front_points) {
+          check_area_points.push_back(point);
+          point.x += orientation_vector.x;
+          point.y += orientation_vector.z;
+          point.z += orientation_vector.y;
+          check_area_points.push_back(point);
+        }
+        const auto check_polygon = math::geometry::toBoostPolygon(check_area_points);
+        const auto poly =
+          math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());
+        if (
+          boost::geometry::intersects(check_polygon, poly) ||
+          boost::geometry::intersects(poly, check_polygon)) {
+          return true;
+        } else if (boost::geometry::disjoint(check_polygon, poly)) {
+          return false;
+        }
+        return true;
+      }
+    }
+    return false;
+  };
+
+  const double min_stop = calculateStopDistance(behavior_parameter_.dynamic_constraints);
+  const double stable_distance =
+    min_stop + canonicalized_entity_status_->getBoundingBox().dimensions.x;
+
+  return hasObstacleInFrontOfPedestrian(stable_distance);
+}
+
 bool WalkStraightAction::checkPreconditions()
 {
   return request_ == traffic_simulator::behavior::Request::WALK_STRAIGHT;
@@ -48,6 +148,10 @@ BT::NodeStatus WalkStraightAction::doAction()
   if (!target_speed_) {
     target_speed_ = 1.111;
   }
+
+  const auto obstacle_detector_result = detectObstacleInFront(behavior_parameter_.see_around);
+  target_speed_ = obstacle_detector_result ? 0.0 : target_speed_;
+
   setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed_.value()));
   return BT::NodeStatus::RUNNING;
 }