From 7144ff6a33ab9a102739319a9cac0d34c6d78d26 Mon Sep 17 00:00:00 2001 From: "W. Jacob Wagner" Date: Fri, 29 May 2020 12:06:30 -0500 Subject: [PATCH 1/4] Unifying velocity limit parameter names for DWA and BLP. The parameter max_speed_theta now refers to the absolute value of the maximum rotational velocity that can be commanded. The parameter min_in_place_speed_theta now refers to the absolute value of the minimum rotational velocity that will be commanded when performing an in-place rotation. The max_speed_theta and min_in_place_speed_theta parameters replace following parameters which are no longer supported: max_vel_theta, min_vel_theta, max_rotational_vel, min_in_place_vel_theta Variable names within the code have also been changed to reflect these parameter name changes. The variables max_vel_th, max_vel_th_, and max_vel_theta all refer to the maximum rotational velocity, not speed. The variables min_vel_th, min_vel_th_, and min_vel_theta all refer to the minimum rotational velocity, not speed. The variables min_in_place_speed_th, min_in_place_speed_th_, and min_in_place_speed_theta all refer to the minimum rotational speed, not velocity, for in-place rotations. base_local_planner/trajectory_planner_ros: param name change: max_rotational_vel --> max_speed_theta dynamic reconfigure param name change: max_vel_theta --> max_speed_theta removed dynamic reconfigure param: min_vel_theta dynamic reconfigure param name change: min_in_place_vel_theta --> min_in_place_speed_theta dwa_planner_ros: dynamic reconfigure param name change: max_vel_theta --> max_speed_theta dynamic reconfigure param name change: min_vel_theta --> min_in_place_speed_theta move_slow_and_clear: param name change: max_vel_theta --> max_speed_theta rotate_recovery: param name change: max_vel_theta --> max_speed_theta param name change: min_vel_theta --> min_in_place_speed_theta --- base_local_planner/cfg/BaseLocalPlanner.cfg | 5 ++- .../base_local_planner/local_planner_limits.h | 3 ++ .../base_local_planner/trajectory_planner.h | 6 ++-- .../trajectory_planner_ros.h | 2 +- .../src/latched_stop_rotate_controller.cpp | 4 +-- .../src/local_planner_limits/__init__.py | 5 +-- .../src/simple_trajectory_generator.cpp | 2 +- base_local_planner/src/trajectory_planner.cpp | 14 ++++---- .../src/trajectory_planner_ros.cpp | 32 ++++++++++++------- dwa_local_planner/src/dwa_planner_ros.cpp | 12 ++++--- .../src/move_slow_and_clear.cpp | 6 ++-- .../include/rotate_recovery/rotate_recovery.h | 2 +- rotate_recovery/src/rotate_recovery.cpp | 6 ++-- 13 files changed, 57 insertions(+), 42 deletions(-) diff --git a/base_local_planner/cfg/BaseLocalPlanner.cfg b/base_local_planner/cfg/BaseLocalPlanner.cfg index fcb7b3d6c7..ca33846f37 100755 --- a/base_local_planner/cfg/BaseLocalPlanner.cfg +++ b/base_local_planner/cfg/BaseLocalPlanner.cfg @@ -18,9 +18,8 @@ gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in th gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55, 0, 20.0) gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0, 0, 20.0) -gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0, 20.0) -gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", -1.0, -20.0, 0.0) -gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0) +gen.add("max_speed_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0, 20.0) +gen.add("min_in_place_speed_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity in rad/s", 0.4, 0, 20.0) gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10) gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5) diff --git a/base_local_planner/include/base_local_planner/local_planner_limits.h b/base_local_planner/include/base_local_planner/local_planner_limits.h index d94b87d5ae..63b1c5bb83 100644 --- a/base_local_planner/include/base_local_planner/local_planner_limits.h +++ b/base_local_planner/include/base_local_planner/local_planner_limits.h @@ -52,6 +52,7 @@ class LocalPlannerLimits double min_vel_y; double max_vel_theta; double min_vel_theta; + double min_in_place_speed_theta; double acc_lim_x; double acc_lim_y; double acc_lim_theta; @@ -74,6 +75,7 @@ class LocalPlannerLimits double nmin_vel_y, double nmax_vel_theta, double nmin_vel_theta, + double nmin_in_place_speed_theta, double nacc_lim_x, double nacc_lim_y, double nacc_lim_theta, @@ -91,6 +93,7 @@ class LocalPlannerLimits min_vel_y(nmin_vel_y), max_vel_theta(nmax_vel_theta), min_vel_theta(nmin_vel_theta), + min_in_place_speed_theta(nmin_in_place_speed_theta), acc_lim_x(nacc_lim_x), acc_lim_y(nacc_lim_y), acc_lim_theta(nacc_lim_theta), diff --git a/base_local_planner/include/base_local_planner/trajectory_planner.h b/base_local_planner/include/base_local_planner/trajectory_planner.h index 19785d91c9..6337a48c75 100644 --- a/base_local_planner/include/base_local_planner/trajectory_planner.h +++ b/base_local_planner/include/base_local_planner/trajectory_planner.h @@ -92,7 +92,7 @@ namespace base_local_planner { * @param min_vel_x The minimum x velocity the controller will explore * @param max_vel_th The maximum rotational velocity the controller will explore * @param min_vel_th The minimum rotational velocity the controller will explore - * @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore + * @param min_in_place_speed_theta The absolute value of the minimum in-place rotational velocity the controller will explore * @param backup_vel The velocity to use while backing up * @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits * @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep @@ -113,7 +113,7 @@ namespace base_local_planner { double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2, bool holonomic_robot = true, double max_vel_x = 0.5, double min_vel_x = 0.1, - double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4, + double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_speed_th = 0.4, double backup_vel = -0.1, bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1, bool meter_scoring = true, @@ -299,7 +299,7 @@ namespace base_local_planner { double escape_reset_dist_, escape_reset_theta_; ///< @brief The distance the robot must travel before it can leave escape mode bool holonomic_robot_; ///< @brief Is the robot holonomic or not? - double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_; ///< @brief Velocity limits for the controller + double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_speed_th_; ///< @brief Velocity limits for the controller double backup_vel_; ///< @brief The velocity to use while backing up diff --git a/base_local_planner/include/base_local_planner/trajectory_planner_ros.h b/base_local_planner/include/base_local_planner/trajectory_planner_ros.h index 03949451f2..00e6602a46 100644 --- a/base_local_planner/include/base_local_planner/trajectory_planner_ros.h +++ b/base_local_planner/include/base_local_planner/trajectory_planner_ros.h @@ -203,7 +203,7 @@ namespace base_local_planner { nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot double rot_stopped_velocity_, trans_stopped_velocity_; - double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_; + double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_speed_th_; std::vector global_plan_; bool prune_plan_; boost::recursive_mutex odom_lock_; diff --git a/base_local_planner/src/latched_stop_rotate_controller.cpp b/base_local_planner/src/latched_stop_rotate_controller.cpp index 9977a876e2..f631753e84 100644 --- a/base_local_planner/src/latched_stop_rotate_controller.cpp +++ b/base_local_planner/src/latched_stop_rotate_controller.cpp @@ -162,7 +162,7 @@ bool LatchedStopRotateController::rotateToGoal( cmd_vel.linear.y = 0; double ang_diff = angles::shortest_angular_distance(yaw, goal_th); - double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, fabs(ang_diff))); + double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_in_place_speed_theta, fabs(ang_diff))); //take the acceleration limits of the robot into account double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period; @@ -174,7 +174,7 @@ bool LatchedStopRotateController::rotateToGoal( double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff)); v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp)); - v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, v_theta_samp)); + v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_in_place_speed_theta, v_theta_samp)); if (ang_diff < 0) { v_theta_samp = - v_theta_samp; diff --git a/base_local_planner/src/local_planner_limits/__init__.py b/base_local_planner/src/local_planner_limits/__init__.py index 03fa1deadc..8a1189b96e 100644 --- a/base_local_planner/src/local_planner_limits/__init__.py +++ b/base_local_planner/src/local_planner_limits/__init__.py @@ -23,8 +23,8 @@ def add_generic_localplanner_params(gen): gen.add("max_vel_y", double_t, 0, "The maximum y velocity for the robot in m/s", 0.1) gen.add("min_vel_y", double_t, 0, "The minimum y velocity for the robot in m/s", -0.1) - gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0) - gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.4, 0) + gen.add("max_speed_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0) + gen.add("min_in_place_speed_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.4, 0) # acceleration gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0) @@ -39,3 +39,4 @@ def add_generic_localplanner_params(gen): gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1) gen.add("theta_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1) +14 \ No newline at end of file diff --git a/base_local_planner/src/simple_trajectory_generator.cpp b/base_local_planner/src/simple_trajectory_generator.cpp index 2af290a951..a4fa2fdf0e 100644 --- a/base_local_planner/src/simple_trajectory_generator.cpp +++ b/base_local_planner/src/simple_trajectory_generator.cpp @@ -191,7 +191,7 @@ bool SimpleTrajectoryGenerator::generateTrajectory( // make sure that the robot would at least be moving with one of // the required minimum velocities for translation and rotation (if set) if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) && - (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta)) { + (limits_->min_in_place_speed_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_in_place_speed_theta)) { return false; } // make sure we do not exceed max diagonal (x+y) translational velocity (if set) diff --git a/base_local_planner/src/trajectory_planner.cpp b/base_local_planner/src/trajectory_planner.cpp index 69bad75dab..8f70696cda 100644 --- a/base_local_planner/src/trajectory_planner.cpp +++ b/base_local_planner/src/trajectory_planner.cpp @@ -71,9 +71,9 @@ namespace base_local_planner{ max_vel_x_ = config.max_vel_x; min_vel_x_ = config.min_vel_x; - max_vel_th_ = config.max_vel_theta; - min_vel_th_ = config.min_vel_theta; - min_in_place_vel_th_ = config.min_in_place_vel_theta; + max_vel_th_ = config.max_speed_theta; + min_vel_th_ = -1.0 * max_vel_th_; + min_in_place_speed_th_ = config.min_in_place_speed_theta; sim_time_ = config.sim_time; sim_granularity_ = config.sim_granularity; @@ -150,7 +150,7 @@ namespace base_local_planner{ double escape_reset_dist, double escape_reset_theta, bool holonomic_robot, double max_vel_x, double min_vel_x, - double max_vel_th, double min_vel_th, double min_in_place_vel_th, + double max_vel_th, double min_vel_th, double min_in_place_speed_th, double backup_vel, bool dwa, bool heading_scoring, double heading_scoring_timestep, bool meter_scoring, bool simple_attractor, vector y_vels, double stop_time_buffer, double sim_period, double angular_sim_granularity) @@ -166,7 +166,7 @@ namespace base_local_planner{ oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist), escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot), max_vel_x_(max_vel_x), min_vel_x_(min_vel_x), - max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th), + max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_speed_th_(min_in_place_speed_th), backup_vel_(backup_vel), dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep), simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period) @@ -655,8 +655,8 @@ namespace base_local_planner{ for(int i = 0; i < vtheta_samples_; ++i) { //enforce a minimum rotational velocity because the base can't handle small in-place rotations - double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_) - : min(vtheta_samp, -1.0 * min_in_place_vel_th_); + double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_speed_th_) + : min(vtheta_samp, -1.0 * min_in_place_speed_th_); generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj); diff --git a/base_local_planner/src/trajectory_planner_ros.cpp b/base_local_planner/src/trajectory_planner_ros.cpp index ebe1db1967..0071e22983 100644 --- a/base_local_planner/src/trajectory_planner_ros.cpp +++ b/base_local_planner/src/trajectory_planner_ros.cpp @@ -212,14 +212,22 @@ namespace base_local_planner { private_nh.param("max_vel_x", max_vel_x, 0.5); private_nh.param("min_vel_x", min_vel_x, 0.1); - double max_rotational_vel; - private_nh.param("max_rotational_vel", max_rotational_vel, 1.0); - max_vel_th_ = max_rotational_vel; - min_vel_th_ = -1.0 * max_rotational_vel; + private_nh.param("max_speed_theta", max_vel_th_, 1.0); + min_vel_th_ = -1.0 * max_vel_th_; - min_in_place_vel_th_ = nav_core::loadParameterWithDeprecation(private_nh, - "min_in_place_vel_theta", + if(private_nh.hasParam("max_vel_theta")) + ROS_ERROR("You are using max_vel_theta. Use max_speed_theta to set the maximum angular speed."); + + if(private_nh.hasParam("max_rotational_vel")) + ROS_ERROR("You are using max_rotational_vel where you should be using max_speed_theta. Please change your configuration files appropriately."); + + if(private_nh.hasParam("min_vel_theta")) + ROS_ERROR("The parameter min_vel_theta is unsupported. Use max_speed_theta to set the maximum angular speed and min_in_place_speed_theta to set the minimum angular speed."); + + min_in_place_speed_th_ = nav_core::loadParameterWithDeprecation(private_nh, + "min_in_place_speed_theta", "min_in_place_rotational_vel", 0.4); + reached_goal_ = false; backup_vel = -0.1; if(private_nh.getParam("backup_vel", backup_vel)) @@ -254,7 +262,7 @@ namespace base_local_planner { tc_ = new TrajectoryPlanner(*world_model_, *costmap_, footprint_spec_, acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, path_distance_bias, goal_distance_bias, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot, - max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel, + max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_speed_th_, backup_vel, dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity); map_viz_.initialize(name, global_frame_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6)); @@ -341,8 +349,8 @@ namespace base_local_planner { double ang_diff = angles::shortest_angular_distance(yaw, goal_th); double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_, - std::max(min_in_place_vel_th_, ang_diff)) : std::max(min_vel_th_, - std::min(-1.0 * min_in_place_vel_th_, ang_diff)); + std::max(min_in_place_speed_th_, ang_diff)) : std::max(min_vel_th_, + std::min(-1.0 * min_in_place_speed_th_, ang_diff)); //take the acceleration limits of the robot into account double max_acc_vel = fabs(vel_yaw) + acc_lim_theta_ * sim_period_; @@ -355,10 +363,10 @@ namespace base_local_planner { v_theta_samp = sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp)); - // Re-enforce min_in_place_vel_th_. It is more important than the acceleration limits. + // Re-enforce min_in_place_speed_th_. It is more important than the acceleration limits. v_theta_samp = v_theta_samp > 0.0 - ? std::min( max_vel_th_, std::max( min_in_place_vel_th_, v_theta_samp )) - : std::max( min_vel_th_, std::min( -1.0 * min_in_place_vel_th_, v_theta_samp )); + ? std::min( max_vel_th_, std::max( min_in_place_speed_th_, v_theta_samp )) + : std::max( min_vel_th_, std::min( -1.0 * min_in_place_speed_th_, v_theta_samp )); //we still want to lay down the footprint of the robot and check if the action is legal bool valid_cmd = tc_->checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw, diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index d024dcc1e9..bcfdc38012 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -72,8 +72,9 @@ namespace dwa_local_planner { limits.min_vel_x = config.min_vel_x; limits.max_vel_y = config.max_vel_y; limits.min_vel_y = config.min_vel_y; - limits.max_vel_theta = config.max_vel_theta; - limits.min_vel_theta = config.min_vel_theta; + limits.max_vel_theta = config.max_speed_theta; + limits.min_vel_theta = -1.0 * limits.max_vel_theta; + limits.min_in_place_speed_theta = limits.min_in_place_speed_theta; limits.acc_lim_x = config.acc_lim_x; limits.acc_lim_y = config.acc_lim_y; limits.acc_lim_theta = config.acc_lim_theta; @@ -125,8 +126,11 @@ namespace dwa_local_planner { // Warn about deprecated parameters -- remove this block in N-turtle nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel"); nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel"); - nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel"); - nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel"); + nav_core::warnRenamedParameter(private_nh, "max_speed_theta", "max_rot_vel"); + nav_core::warnRenamedParameter(private_nh, "max_speed_theta", "max_vel_theta"); + nav_core::warnRenamedParameter(private_nh, "min_in_place_speed_theta", "min_in_place_vel_theta"); + nav_core::warnRenamedParameter(private_nh, "min_in_place_speed_theta", "min_vel_theta"); + nav_core::warnRenamedParameter(private_nh, "min_in_place_speed_theta", "min_rot_vel"); nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans"); nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel"); diff --git a/move_slow_and_clear/src/move_slow_and_clear.cpp b/move_slow_and_clear/src/move_slow_and_clear.cpp index 2ce2ee36a7..c73c8c7444 100644 --- a/move_slow_and_clear/src/move_slow_and_clear.cpp +++ b/move_slow_and_clear/src/move_slow_and_clear.cpp @@ -136,9 +136,9 @@ namespace move_slow_and_clear ROS_ERROR("The planner %s, does not have the parameter max_vel_trans", planner_nh_.getNamespace().c_str()); } - if(!planner_nh_.getParam("max_vel_theta", old_rot_speed_)) + if(!planner_nh_.getParam("max_speed_theta", old_rot_speed_)) { - ROS_ERROR("The planner %s, does not have the parameter max_vel_theta", planner_nh_.getNamespace().c_str()); + ROS_ERROR("The planner %s, does not have the parameter max_speed_theta", planner_nh_.getNamespace().c_str()); } } @@ -208,7 +208,7 @@ namespace move_slow_and_clear { dynamic_reconfigure::Reconfigure rot_reconfigure; dynamic_reconfigure::DoubleParameter new_rot; - new_rot.name = "max_vel_theta"; + new_rot.name = "max_speed_theta"; new_rot.value = rot_speed; rot_reconfigure.request.config.doubles.push_back(new_rot); try { diff --git a/rotate_recovery/include/rotate_recovery/rotate_recovery.h b/rotate_recovery/include/rotate_recovery/rotate_recovery.h index cbfdf2daee..1f7f67bdad 100644 --- a/rotate_recovery/include/rotate_recovery/rotate_recovery.h +++ b/rotate_recovery/include/rotate_recovery/rotate_recovery.h @@ -79,7 +79,7 @@ class RotateRecovery : public nav_core::RecoveryBehavior private: costmap_2d::Costmap2DROS* local_costmap_; bool initialized_; - double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_; + double sim_granularity_, max_vel_th_, min_in_place_speed_th_, acc_lim_th_, tolerance_, frequency_; base_local_planner::CostmapModel* world_model_; }; }; // namespace rotate_recovery diff --git a/rotate_recovery/src/rotate_recovery.cpp b/rotate_recovery/src/rotate_recovery.cpp index 2d0bdfe74f..db211c2431 100644 --- a/rotate_recovery/src/rotate_recovery.cpp +++ b/rotate_recovery/src/rotate_recovery.cpp @@ -71,8 +71,8 @@ void RotateRecovery::initialize(std::string name, tf2_ros::Buffer*, private_nh.param("frequency", frequency_, 20.0); acc_lim_th_ = nav_core::loadParameterWithDeprecation(blp_nh, "acc_lim_theta", "acc_lim_th", 3.2); - max_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "max_vel_theta", "max_rotational_vel", 1.0); - min_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "min_in_place_vel_theta", "min_in_place_rotational_vel", 0.4); + max_vel_th_ = nav_core::loadParameterWithDeprecation(blp_nh, "max_speed_theta", "max_rotational_vel", 1.0); + min_in_place_speed_th_ = nav_core::loadParameterWithDeprecation(blp_nh, "min_in_place_speed_theta", "min_in_place_rotational_vel", 0.4); blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10); world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap()); @@ -168,7 +168,7 @@ void RotateRecovery::runBehavior() double vel = sqrt(2 * acc_lim_th_ * dist_left); // make sure that this velocity falls within the specified limits - vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); + vel = std::min(std::max(vel, min_in_place_speed_th_), max_vel_th_); geometry_msgs::Twist cmd_vel; cmd_vel.linear.x = 0.0; From ec63df2a5a7ac2bc98c0db8ac229f7984780ec2b Mon Sep 17 00:00:00 2001 From: Jacob Date: Mon, 18 May 2020 15:33:08 -0500 Subject: [PATCH 2/4] Fixing rotatetogoal function to properly impose acceleration and velocity limits. Made changes for both trajectory_planner_ros and latcehd_stop_rotate_controller. Updated comments to make calculations more understandable. --- .../src/latched_stop_rotate_controller.cpp | 52 +++++++++++----- .../src/trajectory_planner_ros.cpp | 60 +++++++++++++------ 2 files changed, 79 insertions(+), 33 deletions(-) diff --git a/base_local_planner/src/latched_stop_rotate_controller.cpp b/base_local_planner/src/latched_stop_rotate_controller.cpp index f631753e84..6e10303ffc 100644 --- a/base_local_planner/src/latched_stop_rotate_controller.cpp +++ b/base_local_planner/src/latched_stop_rotate_controller.cpp @@ -157,28 +157,52 @@ bool LatchedStopRotateController::rotateToGoal( Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check) { double yaw = tf2::getYaw(global_pose.pose.orientation); + // Somewhat deceivingly, this actualy is the rotational velocity measured from the odometry source. + // See odom_helper_.getRobotVel() for reference double vel_yaw = tf2::getYaw(robot_vel.pose.orientation); + // If we wanted to use the open-loop velocity estimate (commanded velocity from previous loop) instead of the measured current velocity + // then we could use the following: + // vel_yaw = prev_cmd_vel_angular_z == 0.0 ? vel_yaw : prev_cmd_vel_angular_z; + // vel_yaw = ang_diff > 0.0 ? std::max(vel_yaw, min_in_place_speed_th_) : std::min(vel_yaw, -1.0 * min_in_place_speed_th_); cmd_vel.linear.x = 0; cmd_vel.linear.y = 0; double ang_diff = angles::shortest_angular_distance(yaw, goal_th); - double v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_in_place_speed_theta, fabs(ang_diff))); - - //take the acceleration limits of the robot into account - double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period; - double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period; - - v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel); - - //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits - double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff)); - v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp)); + // Compute the maximum and minimum velocities as allowed by the maximum angular acceleration + // v1 = v0 + a * dt + double max_acc_vel = vel_yaw + acc_lim[2] * sim_period; + double min_acc_vel = vel_yaw - acc_lim[2] * sim_period; + + // We also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits + // Compute thme maximum velocity we can send such that we will be able to stop in time before reaching the goal position + // while obeying our acceleration limits. v = sqrt(2 * a * dTheta) + double max_vel_to_stop = sign(ang_diff) * sqrt(2 * acc_lim[2] * fabs(ang_diff)); + + // Impose both limits on the desired velocity. Let the acceleration limits take precedence over the stopping velocity. + // i.e. if the vehicle is somehow in a situation where commanding max_vel_to_stop would require us to decellerate faster than + // what the acceleration limit allows, then use the velocity imposed by the accelleration limit. This situation shouldn't arise + // very often, although it could happen when we change a rotational goal mid execution, or if an external disturbance is applied to the robot. + double v_theta_samp = max_vel_to_stop; + if (max_vel_to_stop < min_acc_vel) { + v_theta_samp = min_acc_vel; + } + else if (max_vel_to_stop > max_acc_vel) { + v_theta_samp = max_acc_vel; + } + // else if (max_vel_to_stop >= min_acc_vel && max_vel_to_stop <= max_acc_vel) { + // v_theta_samp = max_vel_to_stop; + // } - v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_in_place_speed_theta, v_theta_samp)); + // The range of possible velocities is [min_vel_theta, -min_in_place_speed_theta] U [max_in_place_speed_theta, max_vel_theta] - if (ang_diff < 0) { - v_theta_samp = - v_theta_samp; + // Enforce min_in_place_speed_theta + // In the case that the desired speed is smaller than the min_in_place_speed_theta + // command the minimum speed in the direction of the desired angular position delta. + if (fabs(v_theta_samp) <= limits.min_in_place_speed_theta) { + v_theta_samp = (ang_diff < 0.0 ? -1.0 * limits.min_in_place_speed_theta : limits.min_in_place_speed_theta); } + // Enforce user defined max/min speed limits. + v_theta_samp = std::min(limits.max_vel_theta, std::max(limits.min_vel_theta, v_theta_samp)); //we still want to lay down the footprint of the robot and check if the action is legal bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw), diff --git a/base_local_planner/src/trajectory_planner_ros.cpp b/base_local_planner/src/trajectory_planner_ros.cpp index 0071e22983..972eaddf46 100644 --- a/base_local_planner/src/trajectory_planner_ros.cpp +++ b/base_local_planner/src/trajectory_planner_ros.cpp @@ -343,30 +343,52 @@ namespace base_local_planner { bool TrajectoryPlannerROS::rotateToGoal(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel){ double yaw = tf2::getYaw(global_pose.pose.orientation); + // Somewhat deceivingly, this actualy is the rotational velocity measured from the odometry source. + // See odom_helper_.getRobotVel() for reference double vel_yaw = tf2::getYaw(robot_vel.pose.orientation); + // If we wanted to use the open-loop velocity estimate (commanded velocity from previous loop) instead of the measured current velocity + // then we could use the following: + // vel_yaw = prev_cmd_vel_angular_z == 0.0 ? vel_yaw : prev_cmd_vel_angular_z; + // vel_yaw = ang_diff > 0.0 ? std::max(vel_yaw, min_in_place_speed_th_) : std::min(vel_yaw, -1.0 * min_in_place_speed_th_); cmd_vel.linear.x = 0; cmd_vel.linear.y = 0; double ang_diff = angles::shortest_angular_distance(yaw, goal_th); - double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_, - std::max(min_in_place_speed_th_, ang_diff)) : std::max(min_vel_th_, - std::min(-1.0 * min_in_place_speed_th_, ang_diff)); - - //take the acceleration limits of the robot into account - double max_acc_vel = fabs(vel_yaw) + acc_lim_theta_ * sim_period_; - double min_acc_vel = fabs(vel_yaw) - acc_lim_theta_ * sim_period_; - - v_theta_samp = sign(v_theta_samp) * std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel); - - //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits - double max_speed_to_stop = sqrt(2 * acc_lim_theta_ * fabs(ang_diff)); - - v_theta_samp = sign(v_theta_samp) * std::min(max_speed_to_stop, fabs(v_theta_samp)); - - // Re-enforce min_in_place_speed_th_. It is more important than the acceleration limits. - v_theta_samp = v_theta_samp > 0.0 - ? std::min( max_vel_th_, std::max( min_in_place_speed_th_, v_theta_samp )) - : std::max( min_vel_th_, std::min( -1.0 * min_in_place_speed_th_, v_theta_samp )); + // Compute the maximum and minimum velocities as allowed by the maximum angular acceleration + // v1 = v0 + a * dt + double max_acc_vel = vel_yaw + acc_lim_theta_ * sim_period_; + double min_acc_vel = vel_yaw - acc_lim_theta_ * sim_period_; + + // We also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits + // Compute the maximum speed we can send such that we will be able to stop in time before reaching the goal position + // while obeying our acceleration limits. v = sqrt(2 * a * dTheta) + double max_vel_to_stop = sign(ang_diff)*sqrt(2 * acc_lim_theta_ * fabs(ang_diff)); + + // Impose both limits on the desired velocity. Let the acceleration limits take precedence over the stopping velocity. + // i.e. if the vehicle is somehow in a situation where commanding max_vel_to_stop would require us to decellerate faster than + // what the acceleration limit allows, then use the velocity imposed by the accelleration limit. This situation shouldn't arise + // very often, although it could happen when we change a rotational goal mid execution, or if an external disturbance is applied to the robot. + double v_theta_samp = max_vel_to_stop; + if (max_vel_to_stop < min_acc_vel) { + v_theta_samp = min_acc_vel; + } + else if (max_vel_to_stop > max_acc_vel) { + v_theta_samp = max_acc_vel; + } + // else if (max_vel_to_stop >= min_acc_vel && max_vel_to_stop <= max_acc_vel) { + // v_theta_samp = max_vel_to_stop; + // } + + // The range of possible velocities is [min_vel_th_, -min_in_place_speed_th_] U [min_in_place_speed_theta, max_vel_th_] + + // Enforce min_in_place_speed_th_ + // In the case that the desired speed is smaller than the min_in_place_speed_th_, + // command the minimum speed in the direction of the desired angular position delta. + if (fabs(v_theta_samp) <= min_in_place_speed_th_) { + v_theta_samp = (ang_diff < 0.0 ? -1.0 * min_in_place_speed_th_ : min_in_place_speed_th_); + } + // Enforce user defined max/min speed limits. + v_theta_samp = std::min(max_vel_th_, std::max(min_vel_th_, v_theta_samp));; //we still want to lay down the footprint of the robot and check if the action is legal bool valid_cmd = tc_->checkTrajectory(global_pose.pose.position.x, global_pose.pose.position.y, yaw, From 500c9955b7e4589b42f2984b839e5150269a08d4 Mon Sep 17 00:00:00 2001 From: "W. Jacob Wagner" Date: Fri, 29 May 2020 20:15:12 -0500 Subject: [PATCH 3/4] Fixing blp dynamic reconfigure so that changes are applied to both the trajectory generation and the in place rotations. --- base_local_planner/src/trajectory_planner_ros.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/base_local_planner/src/trajectory_planner_ros.cpp b/base_local_planner/src/trajectory_planner_ros.cpp index 972eaddf46..e2764bcb46 100644 --- a/base_local_planner/src/trajectory_planner_ros.cpp +++ b/base_local_planner/src/trajectory_planner_ros.cpp @@ -71,7 +71,16 @@ namespace base_local_planner { default_config_ = config; setup_ = true; } + // Update params in tc_ tc_->reconfigure(config); + // Update relevant params in this class + acc_lim_x_ = config.acc_lim_x; + acc_lim_y_ = config.acc_lim_y; + acc_lim_theta_ = config.acc_lim_theta; + max_vel_th_ = config.max_speed_theta; + min_vel_th_ = -1.0 * max_vel_th_; + min_in_place_speed_th_ = config.min_in_place_speed_theta; + reached_goal_ = false; } From 6daacbd996fd78462bd3f462b9e1c5b25e6eaea1 Mon Sep 17 00:00:00 2001 From: "Israel J. Lopez Toledo" Date: Fri, 29 May 2020 20:15:59 -0500 Subject: [PATCH 4/4] Fix for issue #147 This fixes the latched stopped rotation bug. Changes were applied to both dwa_planner_ros and base_local_planner. --- .../trajectory_planner_ros.h | 14 +++++++-- .../src/trajectory_planner_ros.cpp | 30 +++++++++++++++++-- .../dwa_local_planner/dwa_planner_ros.h | 12 ++++++-- dwa_local_planner/src/dwa_planner_ros.cpp | 27 ++++++++++++++++- 4 files changed, 74 insertions(+), 9 deletions(-) diff --git a/base_local_planner/include/base_local_planner/trajectory_planner_ros.h b/base_local_planner/include/base_local_planner/trajectory_planner_ros.h index 00e6602a46..9da702942a 100644 --- a/base_local_planner/include/base_local_planner/trajectory_planner_ros.h +++ b/base_local_planner/include/base_local_planner/trajectory_planner_ros.h @@ -127,6 +127,14 @@ namespace base_local_planner { */ bool isGoalReached(); + /** + * @brief Compare two gual poses to determine if they are equal + * @param p1 The first pose to compare + * @param p2 The second pose to compare against p1 + * @return True if the two messages represent the same poses + */ + bool isSameGoal(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); + /** * @brief Generate and score a single trajectory * @param vx_samp The x velocity used to seed the trajectory @@ -215,13 +223,15 @@ namespace base_local_planner { bool reached_goal_; bool latch_xy_goal_tolerance_, xy_tolerance_latch_; + geometry_msgs::PoseStamped previous_global_goal_; + ros::Publisher g_plan_pub_, l_plan_pub_; dynamic_reconfigure::Server *dsrv_; base_local_planner::BaseLocalPlannerConfig default_config_; - bool setup_; - + bool setup_; + bool first_goal_; bool initialized_; base_local_planner::OdometryHelperRos odom_helper_; diff --git a/base_local_planner/src/trajectory_planner_ros.cpp b/base_local_planner/src/trajectory_planner_ros.cpp index e2764bcb46..357f8d1bd3 100644 --- a/base_local_planner/src/trajectory_planner_ros.cpp +++ b/base_local_planner/src/trajectory_planner_ros.cpp @@ -276,6 +276,7 @@ namespace base_local_planner { map_viz_.initialize(name, global_frame_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6)); initialized_ = true; + first_goal_ = true; dsrv_ = new dynamic_reconfigure::Server(private_nh); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2); @@ -286,6 +287,16 @@ namespace base_local_planner { } } + bool TrajectoryPlannerROS::isSameGoal(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) + { + double yaw_diff = fabs(tf2::getYaw(p1.pose.orientation) - tf2::getYaw(p2.pose.orientation)); + double dist = hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y); + if(yaw_diff == 0.0 && dist == 0.0) { + return true; + } + return false; + } + std::vector TrajectoryPlannerROS::loadYVels(ros::NodeHandle node){ std::vector y_vels; @@ -426,9 +437,22 @@ namespace base_local_planner { global_plan_ = orig_global_plan; //when we get a new plan, we also want to clear any latch we may have on goal tolerances - xy_tolerance_latch_ = false; - //reset the at goal flag - reached_goal_ = false; + geometry_msgs::PoseStamped current_global_goal = orig_global_plan.back(); + + if (first_goal_) { + xy_tolerance_latch_ = false; + previous_global_goal_ = current_global_goal; + // Make it different from the current goal + previous_global_goal_.pose.position.x += 0.1; + first_goal_ = false; + } else { + if (!isSameGoal(current_global_goal, previous_global_goal_)) { + xy_tolerance_latch_ = false; + //reset the at goal flag + reached_goal_ = false; + previous_global_goal_ = current_global_goal; + } + } return true; } diff --git a/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h b/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h index 4da64119cd..78af1cee45 100644 --- a/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h +++ b/dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h @@ -114,7 +114,13 @@ namespace dwa_local_planner { */ bool isGoalReached(); - + /** + * @brief Compare two gual poses to determine if they are equal + * @param p1 The first pose to compare + * @param p2 The second pose to compare against p1 + * @return True if the two messages represent the same poses + */ + bool isSameGoal(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); bool isInitialized() { return initialized_; @@ -147,10 +153,10 @@ namespace dwa_local_planner { geometry_msgs::PoseStamped current_pose_; base_local_planner::LatchedStopRotateController latchedStopRotateController_; - + geometry_msgs::PoseStamped previous_global_goal_; bool initialized_; - + bool first_goal_; base_local_planner::OdometryHelperRos odom_helper_; std::string odom_topic_; diff --git a/dwa_local_planner/src/dwa_planner_ros.cpp b/dwa_local_planner/src/dwa_planner_ros.cpp index bcfdc38012..e91b0de276 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -122,6 +122,7 @@ namespace dwa_local_planner { } initialized_ = true; + first_goal_ = true; // Warn about deprecated parameters -- remove this block in N-turtle nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel"); @@ -143,13 +144,37 @@ namespace dwa_local_planner { } } + bool DWAPlannerROS::isSameGoal(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) { + double yaw_diff = fabs(tf2::getYaw(p1.pose.orientation) - tf2::getYaw(p2.pose.orientation)); + double dist = hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y); + if(yaw_diff == 0.0 && dist == 0.0) { + return true; + } + return false; + } + bool DWAPlannerROS::setPlan(const std::vector& orig_global_plan) { if (! isInitialized()) { ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); return false; } + //when we get a new plan, we also want to clear any latch we may have on goal tolerances - latchedStopRotateController_.resetLatching(); + geometry_msgs::PoseStamped current_global_goal = orig_global_plan.back(); + + if (first_goal_) { + latchedStopRotateController_.resetLatching(); + // Make it different from the current goal + previous_global_goal_.pose.position.x += 0.1; + first_goal_ = false; + } else { + if (!isSameGoal(current_global_goal, previous_global_goal_)) { + latchedStopRotateController_.resetLatching(); + previous_global_goal_ = current_global_goal; + } + } + + ROS_INFO("Got new plan"); return dp_->setPlan(orig_global_plan);