diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_lane_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_lane_action.hpp index 76b16d81095..72200fcdd8c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_lane_action.hpp +++ b/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 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 index a1804be6047..51c2b17d38f 100644 --- 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 @@ -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 diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/walk_straight_action.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/walk_straight_action.hpp index fc668f7bf29..92ea0891410 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/walk_straight_action.hpp +++ b/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 diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp index ad4a5cce069..f04aaf3ed26 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/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("pedestrian_ignore_see_around", "blind")); } void FollowLaneAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); } diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index 27890185607..fe14b54c368 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/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("pedestrian_ignore_see_around", "blind")); } auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) const diff --git a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp index cacef752fa2..fd7825d758f 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp @@ -24,6 +24,14 @@ // limitations under the License. #include +#include +#include +#include +#include +#include +#include +#include +#include #include 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(); +} + +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 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 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; }