From d90ad509e2836ea8f416fe8d378ba40612391a91 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Thu, 28 Apr 2022 15:53:47 +0900 Subject: [PATCH] [app] switch vacant check / feasible path check --- .../include/auto_parking_planner.hpp | 1 + .../src/auto_parking_planner.cpp | 14 ++++++++------ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/planning/auto_parking_planner/include/auto_parking_planner.hpp b/planning/auto_parking_planner/include/auto_parking_planner.hpp index f5c22d2738713..ee3a16c359b78 100644 --- a/planning/auto_parking_planner/include/auto_parking_planner.hpp +++ b/planning/auto_parking_planner/include/auto_parking_planner.hpp @@ -62,6 +62,7 @@ bool containPolygon( struct AutoParkingConfig { + bool check_goal_only; double lookahead_length; double reedsshepp_threashold_length; double euclid_threashold_length; diff --git a/planning/auto_parking_planner/src/auto_parking_planner.cpp b/planning/auto_parking_planner/src/auto_parking_planner.cpp index 4a2d9529e2eae..16df09cb7441e 100644 --- a/planning/auto_parking_planner/src/auto_parking_planner.cpp +++ b/planning/auto_parking_planner/src/auto_parking_planner.cpp @@ -35,6 +35,7 @@ AutoParkingPlanner::AutoParkingPlanner(const rclcpp::NodeOptions & node_options) base_link_frame_ = declare_parameter("base_link_frame", "base_link"); { // set node config + config_.check_goal_only = declare_parameter("check_only_goal", true); config_.lookahead_length = declare_parameter("lookahead_length", 4.0); config_.reedsshepp_threashold_length = declare_parameter("reedsshepp_threashold_length", 6.0); config_.euclid_threashold_length = declare_parameter("euclid_threashold_length", 2.0); @@ -200,15 +201,16 @@ std::vector AutoParkingPlanner::askFeasibleGoalIndex( auto freespace_plan_req = std::make_shared(); for (const auto & goal : goal_poses) { - PoseStamped start_pose; - PoseStamped goal_pose; + if (!config_.check_goal_only) { + PoseStamped start_pose; + start_pose.header.frame_id = map_frame_; + start_pose.pose = start; + freespace_plan_req->start_poses.push_back(start_pose); + } - start_pose.header.frame_id = map_frame_; - start_pose.pose = start; + PoseStamped goal_pose; goal_pose.header.frame_id = map_frame_; goal_pose.pose = goal; - - freespace_plan_req->start_poses.push_back(start_pose); freespace_plan_req->goal_poses.push_back(goal_pose); }