From ec5759d0bf982cd2266715bc06a3385309ec3eb1 Mon Sep 17 00:00:00 2001 From: Howard Cochran Date: Thu, 12 Sep 2019 19:58:40 -0400 Subject: [PATCH] Add prevent_look_ahead_poses_near_goal to fix oscillation Never look ahead to the last few poses of the plan in order to prevent oscillating forward and backward over the goal when the xy_goal_tolerance is small. Specified by new parameter prevent_look_ahead_poses_near_goal whose default is zero. --- cfg/TebLocalPlannerReconfigure.cfg | 4 ++++ include/teb_local_planner/teb_config.h | 4 +++- src/optimal_planner.cpp | 2 +- src/teb_config.cpp | 2 ++ 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 983930e4..fe310357 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -66,6 +66,10 @@ grp_trajectory.add("control_look_ahead_poses", int_t, 0, "Index of the pose used to extract the velocity command", 1, 1, 100) +grp_trajectory.add("prevent_look_ahead_poses_near_goal", int_t, 0, + "Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small", + 0, 0, 20) + grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", 0, 0, 1) diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 1d9e290f..cc7c531b 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -86,6 +86,7 @@ class TebConfig bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes) double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] int control_look_ahead_poses; //! Index of the pose used to extract the velocity command + int prevent_look_ahead_poses_near_goal; //! Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small } trajectory; //!< Trajectory related parameters //! Robot related parameters @@ -262,7 +263,8 @@ class TebConfig trajectory.publish_feedback = false; trajectory.min_resolution_collision_check_angular = M_PI; trajectory.control_look_ahead_poses = 1; - + trajectory.prevent_look_ahead_poses_near_goal = 0; + // Robot robot.max_vel_x = 0.4; diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index ce303c52..f0cf64a8 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -1148,7 +1148,7 @@ bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega omega = 0; return false; } - look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); + look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1 - cfg_->trajectory.prevent_look_ahead_poses_near_goal)); double dt = 0.0; for(int counter = 0; counter < look_ahead_poses; ++counter) { diff --git a/src/teb_config.cpp b/src/teb_config.cpp index b8c5b17e..a5e4d843 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -68,6 +68,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback); nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); + nh.param("prevent_look_ahead_poses_near_goal", trajectory.prevent_look_ahead_poses_near_goal, trajectory.prevent_look_ahead_poses_near_goal); // Robot nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x); @@ -198,6 +199,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses; trajectory.publish_feedback = cfg.publish_feedback; trajectory.control_look_ahead_poses = cfg.control_look_ahead_poses; + trajectory.prevent_look_ahead_poses_near_goal = cfg.prevent_look_ahead_poses_near_goal; // Robot robot.max_vel_x = cfg.max_vel_x;