From e7e2c56ecbc21c900dd1f0c34af47948d515b33a Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 25 Sep 2025 13:42:59 +0200 Subject: [PATCH 1/2] RJD-1930 Add see_around for WalkStraightAction --- .../pedestrian/follow_lane_action.hpp | 4 - .../pedestrian/pedestrian_action_node.hpp | 4 + .../pedestrian/walk_straight_action.hpp | 1 + .../src/pedestrian/walk_straight_action.cpp | 104 ++++++++++++++++++ 4 files changed, 109 insertions(+), 4 deletions(-) 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..66ff11f7aa4 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,7 @@ class WalkStraightAction : public entity_behavior::PedestrianActionNode { return entity_behavior::PedestrianActionNode::providedPorts(); } + bool detectObstacleInFront(const bool see_around) const; }; } // namespace pedestrian } // namespace entity_behavior 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..1001124f045 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 @@ -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("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(); +} + +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 front_points; + std::vector 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; } From b37c32ed1e572c219627f0005ebb43121cd0f0ec Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Tue, 30 Sep 2025 12:56:40 +0200 Subject: [PATCH 2/2] Changed names and structure --- .../pedestrian/walk_straight_action.hpp | 5 + .../src/pedestrian/follow_lane_action.cpp | 12 -- .../src/pedestrian/pedestrian_action_node.cpp | 13 ++ .../src/pedestrian/walk_straight_action.cpp | 135 +++++++++--------- 4 files changed, 83 insertions(+), 82 deletions(-) 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 66ff11f7aa4..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 @@ -52,6 +52,11 @@ 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 1001124f045..fd7825d758f 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp @@ -42,100 +42,95 @@ 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("pedestrian_ignore_see_around", "blind")); } void WalkStraightAction::getBlackBoardValues() { PedestrianActionNode::getBlackBoardValues(); } -bool checkPointIsInfront( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point) +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 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(); + 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::detectObstacleInFront(const bool see_around) const +bool WalkStraightAction::isEntityColliding( + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status, + const double & detection_horizon) const { - if (should_respect_see_around == SeeAroundMode::blind) { - return false; + 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); + } } - if (!see_around) { + 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; } +} - auto hasObstacleInFrontOfPedestrian = [this](double distance) { +bool WalkStraightAction::detectObstacleInFront(const bool see_around) const +{ + auto isObstacleInFrontOfPedestrian = [this](const double & detection_horizon) { 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 front_points; - std::vector 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 (const auto distance = math::geometry::norm(other_position - pedestrian_pose.position); + distance <= detection_horizon) { if ( - boost::geometry::intersects(check_polygon, poly) || - boost::geometry::intersects(poly, check_polygon)) { + isPointInFront(pedestrian_pose, other_position) && + isEntityColliding(entity_status, detection_horizon)) { 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); + 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() @@ -149,8 +144,8 @@ BT::NodeStatus WalkStraightAction::doAction() target_speed_ = 1.111; } - const auto obstacle_detector_result = detectObstacleInFront(behavior_parameter_.see_around); - target_speed_ = obstacle_detector_result ? 0.0 : target_speed_; + 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;