From a929625499bbd7a9c10b4d08c9fed4f61d86e13e Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Fri, 5 Jan 2024 17:37:52 +0100 Subject: [PATCH] `planner` - a goal position is reached with a higher accuracy before rotating in place [#112] --- include/humap_local_planner/humap_planner.h | 6 ++++++ src/humap_planner.cpp | 8 +++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/include/humap_local_planner/humap_planner.h b/include/humap_local_planner/humap_planner.h index bc59bcb8..f57744a7 100644 --- a/include/humap_local_planner/humap_planner.h +++ b/include/humap_local_planner/humap_planner.h @@ -72,6 +72,12 @@ class HumapPlanner { */ static constexpr double PERSON_POLYGON_CONTAINMENT_RATE = 0.667; + /** + * Multiplier for the xy_goal_tolerance + * Used for reaching the goal position more accurately before adjusting to the goal orientation. Should be <1.0 + */ + static constexpr double XY_GOAL_TOLERANCE_MULTIPLIER = 0.65; + // both environmental and internal drivers struct MotionDriverData { Vector force_combined_; diff --git a/src/humap_planner.cpp b/src/humap_planner.cpp index a85c5325..e3e57918 100644 --- a/src/humap_planner.cpp +++ b/src/humap_planner.cpp @@ -109,7 +109,13 @@ HumapPlanner::HumapPlanner( pose.pose = pose_.getAsMsgPose(); geometry_msgs::PoseStamped goal_pose; goal_pose.pose = goal_.getAsMsgPose(); - return stop_rotate_controller_.isPositionReached(goal_pose, pose, cfg_->getLimits()->xy_goal_tolerance); + double dist = base_local_planner::getGoalPositionDistance( + pose, + goal_.getX(), + goal_.getY() + ); + double dist_threshold = XY_GOAL_TOLERANCE_MULTIPLIER * cfg_->getLimits()->xy_goal_tolerance; + return dist <= dist_threshold; }); auto crossing_detected_fun = std::function([&]{ return crossing_.isCrossingDetected();