From a0a079a0d4a78ec5d174d245c90d3d29a9b7d8ea Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 1 May 2023 14:38:45 +0900 Subject: [PATCH 1/4] feat(planner_manager): use distance & yaw constraints for closest lanelet search (#3534) feat(planner_manager): use getClosestLaneletWithConstraints Signed-off-by: satoshi-ota --- .../behavior_path_planner/planner_manager.hpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 94a9a0e5bb54..0d4714fd0f17 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -212,11 +212,17 @@ class PlannerManager root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); lanelet::ConstLanelet closest_lane{}; - if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return {}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); } - return utils::getReferencePath(closest_lane, data); + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); + } + + return {}; // something wrong. } /** From 514fea4bf80bf460fa880d96377eb1230823fb7b Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 1 May 2023 19:12:47 +0900 Subject: [PATCH 2/4] feat(motion_utils): add isTargetPointFront function (#3599) --- .../motion_utils/trajectory/trajectory.hpp | 24 +++++ .../test/src/trajectory/test_trajectory.cpp | 98 +++++++++++++++++++ 2 files changed, 122 insertions(+) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index a9db9f51faa4..c65a7eae189e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1784,6 +1784,30 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } + +/** + * @brief Check if the given target point is in front of the based pose from the trajectory. + * if the points is empty, the function returns false + * @param points Points of the trajectory, path, ... + * @param base_point Base point + * @param target_point Target point + * @param threshold threshold for judging front point + * @return true if the target pose is in front of the base pose + */ +template +bool isTargetPointFront( + const T & points, const geometry_msgs::msg::Point & base_point, + const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) +{ + if (points.empty()) { + return false; + } + + const double s_base = calcSignedArcLength(points, 0, base_point); + const double s_target = calcSignedArcLength(points, 0, target_point); + + return s_target - s_base > threshold; +} } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 4b2b6118c74a..6237813a8084 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -5452,3 +5452,101 @@ TEST(trajectory, calcYawDeviation) EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); } } + +TEST(trajectory, isTargetPointFront) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::isTargetPointFront; + using tier4_autoware_utils::createPoint; + + // Generate test trajectory + const auto trajectory = generateTestTrajectory(10, 1.0); + + // Front point is base + { + const auto base_point = createPoint(5.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // Front point is target + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // boundary condition + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // before the start point + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(-3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = createPoint(-5.0, 0.0, 0.0); + const auto target_point = createPoint(1.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // after the end point + { + const auto base_point = createPoint(10.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = createPoint(2.0, 0.0, 0.0); + const auto target_point = createPoint(14.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // empty point + { + const Trajectory traj; + const auto base_point = createPoint(2.0, 0.0, 0.0); + const auto target_point = createPoint(5.0, 0.0, 0.0); + EXPECT_FALSE(isTargetPointFront(traj.points, base_point, target_point)); + } + + // non-default threshold + { + const double threshold = 3.0; + + { + const auto base_point = createPoint(5.0, 0.0, 0.0); + const auto target_point = createPoint(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(4.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = createPoint(1.0, 0.0, 0.0); + const auto target_point = createPoint(4.1, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + } +} From d99718d262a4cdb951c07ae21f9848664bde334d Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 1 May 2023 19:59:19 +0900 Subject: [PATCH 3/4] feat(behavior_path_planner): add isTargetObjectFront function (#3600) --- .../utils/safety_check.hpp | 4 ++++ .../src/utils/safety_check.cpp | 20 +++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index dc6bcfa0ed52..b1ae41beeb22 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -55,6 +55,10 @@ struct ProjectedDistancePoint double distance{0.0}; }; +bool isTargetObjectFront( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); + /** * @brief Project nearest point on a line segment. * @param [in] reference_point point to project diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index dd5db1ad44d9..1e5f9b8c52fe 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -14,10 +14,30 @@ #include "behavior_path_planner/utils/safety_check.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "perception_utils/predicted_path_utils.hpp" namespace behavior_path_planner::utils::safety_check { +bool isTargetObjectFront( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { + return true; + } + } + + return false; +} + template ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & polygon_segment_start, From f944153e492a6e894fd7b02d818c48290d1f5f95 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 1 May 2023 20:52:55 +0900 Subject: [PATCH 4/4] feat(behavior_path_planner): output shorten path if ego vehicle is out of route (#3075) * fix(behavior_path_planner): output shorten path if ego vehicle is out of route Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix function name Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix for shoulder lanes and modified goal Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix for new planner manager Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): move createPathAroundGoal to utils Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): move isInLanelets to utils Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../behavior_tree_manager.hpp | 5 ++ .../behavior_path_planner/data_manager.hpp | 1 + .../behavior_path_planner/planner_manager.hpp | 3 ++ .../behavior_path_planner/utils/utils.hpp | 6 +++ .../src/behavior_path_planner_node.cpp | 1 + .../src/behavior_tree_manager.cpp | 54 +++++++++++++++++++ .../src/planner_manager.cpp | 54 ++++++++++++++++++- .../behavior_path_planner/src/utils/utils.cpp | 37 +++++++++++++ 8 files changed, 160 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index ef26ff81d61d..f4ae0870f14b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -19,6 +19,9 @@ #include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include +#include + #include #include @@ -81,6 +84,8 @@ class BehaviorTreeManager uint16_t max_msg_per_second = 25); void resetGrootMonitor(); + + bool isEgoOutOfRoute(const std::shared_ptr & data) const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 601612b11a57..41981f17b4f9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -154,6 +154,7 @@ struct PlannerData OperationModeState::ConstSharedPtr operation_mode{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; + std::optional prev_modified_goal{}; lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 0d4714fd0f17..e10646204589 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include #include #include @@ -353,6 +354,8 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); + bool isEgoOutOfRoute(const std::shared_ptr & data) const; + boost::optional root_lanelet_{boost::none}; std::vector manager_ptrs_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index cd005a7cd335..fe9dd875b629 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -292,6 +292,12 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +PathWithLaneId createGoalAroundPath( + const std::shared_ptr & route_handler, + const std::optional & modified_goal); + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 8a98a6f4b84a..4170f6b63e46 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1152,6 +1152,7 @@ void BehaviorPathPlannerNode::run() if (output.modified_goal) { PoseWithUuidStamped modified_goal = *(output.modified_goal); modified_goal.header.stamp = path->header.stamp; + planner_data_->prev_modified_goal = modified_goal; modified_goal_publisher_->publish(modified_goal); } diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 560e035c1697..692b87d5a69b 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -89,6 +89,15 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr *s = SceneModuleStatus{s->module_name}; }); + if (isEgoOutOfRoute(data)) { + BehaviorModuleOutput output{}; + const auto output_path = + utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal); + output.path = std::make_shared(output_path); + output.reference_path = std::make_shared(output_path); + return output; + } + // reset blackboard blackboard_->set("output", BehaviorModuleOutput{}); @@ -150,6 +159,51 @@ void BehaviorTreeManager::resetGrootMonitor() } } +bool BehaviorTreeManager::isEgoOutOfRoute(const std::shared_ptr & data) const +{ + const auto self_pose = data->self_odometry->pose.pose; + const Pose goal_pose = + data->prev_modified_goal ? data->prev_modified_goal->pose : data->route_handler->getGoalPose(); + const auto shoulder_lanes = data->route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet goal_lane; + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (utils::isInLanelets(goal_pose, shoulder_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + } + return !data->route_handler->getGoalLanelet(&goal_lane); + }); + if (is_failed_getting_lanelet) { + RCLCPP_WARN_STREAM(logger_, "cannot find goal lanelet"); + return true; + } + + // If ego vehicle is over goal on goal lane, return true + if (lanelet::utils::isInLanelet(self_pose, goal_lane)) { + constexpr double buffer = 1.0; + const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); + const auto goal_arc_coord = + lanelet::utils::getArcCoordinates({goal_lane}, data->route_handler->getGoalPose()); + if (ego_arc_coord.length > goal_arc_coord.length + buffer) { + return true; + } else { + return false; + } + } + + // If ego vehicle is out of the closest lanelet, return true + lanelet::ConstLanelet closest_lane; + if (!data->route_handler->getClosestLaneletWithinRoute(self_pose, &closest_lane)) { + RCLCPP_WARN_STREAM(logger_, "cannot find closest lanelet"); + return true; + } + if (!lanelet::utils::isInLanelet(self_pose, closest_lane)) { + return true; + } + + return false; +} + std::shared_ptr BehaviorTreeManager::getAllSceneModuleDebugMsgData() { scene_module_visitor_ptr_ = std::make_shared(); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index e396aa784613..8ad650a526bd 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -26,7 +26,6 @@ namespace behavior_path_planner { - PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) : logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), @@ -48,6 +47,14 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); auto result_output = [&]() { + if (isEgoOutOfRoute(data)) { + BehaviorModuleOutput output{}; + const auto output_path = + utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal); + output.path = std::make_shared(output_path); + output.reference_path = std::make_shared(output_path); + return output; + } while (rclcpp::ok()) { /** * STEP1: get approved modules' output @@ -606,6 +613,51 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) RCLCPP_INFO_EXPRESSION(logger_, verbose_, "change ego's following lane. reset."); } +bool PlannerManager::isEgoOutOfRoute(const std::shared_ptr & data) const +{ + const auto self_pose = data->self_odometry->pose.pose; + const Pose goal_pose = + data->prev_modified_goal ? data->prev_modified_goal->pose : data->route_handler->getGoalPose(); + const auto shoulder_lanes = data->route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet goal_lane; + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (utils::isInLanelets(goal_pose, shoulder_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + } + return !data->route_handler->getGoalLanelet(&goal_lane); + }); + if (is_failed_getting_lanelet) { + RCLCPP_WARN_STREAM(logger_, "cannot find goal lanelet"); + return true; + } + + // If ego vehicle is over goal on goal lane, return true + if (lanelet::utils::isInLanelet(self_pose, goal_lane)) { + constexpr double buffer = 1.0; + const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); + const auto goal_arc_coord = + lanelet::utils::getArcCoordinates({goal_lane}, data->route_handler->getGoalPose()); + if (ego_arc_coord.length > goal_arc_coord.length + buffer) { + return true; + } else { + return false; + } + } + + // If ego vehicle is out of the closest lanelet, return true + lanelet::ConstLanelet closest_lane; + if (!data->route_handler->getClosestLaneletWithinRoute(self_pose, &closest_lane)) { + RCLCPP_WARN_STREAM(logger_, "cannot find closest lanelet"); + return true; + } + if (!lanelet::utils::isInLanelet(self_pose, closest_lane)) { + return true; + } + + return false; +} + void PlannerManager::print() const { if (!verbose_) { diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 647715aa1541..01a3238bb525 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -921,6 +921,43 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal return false; } +PathWithLaneId createGoalAroundPath( + const std::shared_ptr & route_handler, + const std::optional & modified_goal) +{ + const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet goal_lane; + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (isInLanelets(goal_pose, shoulder_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + } + return !route_handler->getGoalLanelet(&goal_lane); + }); + if (is_failed_getting_lanelet) { + PathWithLaneId path{}; + return path; + } + + constexpr double backward_length = 1.0; + const auto arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, goal_pose); + const double s_start = std::max(arc_coord.length - backward_length, 0.0); + const double s_end = arc_coord.length; + + return route_handler->getCenterLinePath({goal_lane}, s_start, s_end); +} + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (lanelet::utils::isInLanelet(pose, lane)) { + return true; + } + } + return false; +} + lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) { lanelet::ConstLanelets lanes;