Rotational fixes

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)

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),

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
 

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
@@ -203,7 +211,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<geometry_msgs::PoseStamped> global_plan_;
       bool prune_plan_;
       boost::recursive_mutex odom_lock_;
@@ -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<BaseLocalPlannerConfig> *dsrv_;
       base_local_planner::BaseLocalPlannerConfig default_config_;
-      bool setup_;
-
 
+      bool setup_;
+      bool first_goal_;
       bool initialized_;
       base_local_planner::OdometryHelperRos odom_helper_;
 

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_vel_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_vel_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),

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

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)

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<double> 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);