planner - a goal position is reached with a higher accuracy before …

…rotating in place [#112]
rayvburn · Jan 19, 2024 · a929625 · a929625
1 parent 9ec1d20
commit a929625
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git 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
@@ -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<bool()>([&]{
 		return crossing_.isCrossingDetected();