DWB Cleanup and Bug Fixes

dwb_critics/README.md

@@ -0,0 +1,31 @@
+# dwb_critics
+This package contains plugin implementations of `dwb_local_planner::TrajectoryCritic` sufficient to replicate the behavior of `dwa_local_planner`.
+
+## Obstacle Avoidance
+There are two options for critics that examine the contents of the costmap and reject trajectories that come close to obstacles.
+ * `BaseObstacleCritic` assumes a circular robot, and therefore only needs to check one cell in the costmap for each pose in the trajectory (assuming the costmap is properly inflated).
+ * `ObstacleFootprintCritic` uses the robot's footprint and checks all of the cells along the outline of the robot's footprint at each pose.
+
+## Progress Toward the Goal along the Path
+There are two critics which evaluate the robot's position at the end of the trajectory relative to the goal pose and the global plan.
+ * `GoalDistCritic` estimates the distance from the last pose to the goal pose.
+ * `PathDistCritic` estimates the distance from the last pose to the closest pose in the global plan.
+
+## Alignment
+There are also two critics for keeping the robot pointed in the right direction. They use a point on the front of the robot as a proxy to calculate which way the robot is pointed.
+ * `GoalAlignCritic` estimates the distance from the front of the robot to the goal pose. This score will be higher if the robot is pointed away from the goal.
+ * `PathAlignCritic` estimates the distance from the front of the robot to the closest point in the global plan. This score will be higher if the robot is pointed away from the global plan.
+
+## Rotating to the Goal
+There is a special case in the navigation when the robot reaches the correct XY position, but still needs to rotate to the proper yaw. The standard critics will not be useful in this case. Instead we have `RotateToGoalCritic` which operates in a couple different modes.
+ * If the robot is not yet in the correct XY position, it has no effect on the trajectory score.
+ * If the robot is at the correct XY position but still moving, this critic will score the trajecotries such that the robot slows down.
+ * If the robot is at the correct XY position and stopped its forward motion, the critic will
+    A) Disallow trajectories with forward motion
+    B) Score trajectories (rotations) based on how close to the goal yaw they get.
+
+## Other Critics
+ * `OscillationCritic` detects oscillations by looking at the sign of the commanded motions. For example, if in a short window, the robot moves forward and then backward, it will penalize further trajectories that move forward again, as that is considered an oscillation.
+ * `PreferForwardCritic` was implemented but not used by `DWA` and penalize trajectories that move backwards and/or turn too much.
+ * `TwirlingCritic` penalizes trajectories with rotational velocities
+

dwb_critics/include/dwb_critics/base_obstacle.h

@@ -55,7 +55,7 @@ class BaseObstacleCritic : public dwb_local_planner::TrajectoryCritic
 public:
   void onInit() override;
   double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
-  void addGridScores(sensor_msgs::PointCloud& pc) override;
+  void addCriticVisualization(sensor_msgs::PointCloud& pc) override;
 
   /**
    * @brief Return the obstacle score for a particular pose

dwb_critics/include/dwb_critics/map_grid.h

@@ -62,7 +62,7 @@ class MapGridCritic: public dwb_local_planner::TrajectoryCritic
   // Standard TrajectoryCritic Interface
   void onInit() override;
   double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
-  void addGridScores(sensor_msgs::PointCloud& pc) override;
+  void addCriticVisualization(sensor_msgs::PointCloud& pc) override;
   double getScale() const override { return costmap_->getResolution() * 0.5 * scale_; }
 
   // Helper Functions

dwb_critics/include/dwb_critics/oscillation.h

@@ -86,7 +86,7 @@ class OscillationCritic: public dwb_local_planner::TrajectoryCritic
   void reset() override;
   void debrief(const nav_2d_msgs::Twist2D& cmd_vel) override;
 
-private:
+protected:
   /**
    * @class CommandTrend
    * @brief Helper class for performing the same logic on the x,y and theta dimensions
@@ -117,7 +117,7 @@ class OscillationCritic: public dwb_local_planner::TrajectoryCritic
      */
     bool hasSignFlipped();
 
-  private:
+  protected:
     // Simple Enum for Tracking
     enum class Sign { ZERO, POSITIVE, NEGATIVE };
 

dwb_critics/include/dwb_critics/prefer_forward.h

@@ -57,7 +57,7 @@ class PreferForwardCritic: public dwb_local_planner::TrajectoryCritic
   void onInit() override;
   double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
 
-private:
+protected:
   double penalty_, strafe_x_, strafe_theta_, theta_scale_;
 };
 

dwb_critics/include/dwb_critics/rotate_to_goal.h

@@ -45,23 +45,55 @@ namespace dwb_critics
  * @class RotateToGoalCritic
  * @brief Forces the commanded trajectories to only be rotations if within a certain distance window
  *
- * This used to be built in to the DWA Local Planner, but has been moved to a critic for consistency.
+ * This used to be built in to the DWA Local Planner as the LatchedStopRotate controller,
+ * but has been moved to a critic for consistency.
  *
- * If the current pose is within xy_goal_tolerance LINEAR distance from the goal pose, score trajectories
- * that have linear movement as invalid and prefer trajectories that will move toward the goal yaw.
+ * The critic has three distinct phases.
+ *  1) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose, this critic
+ *     will just return score 0.0.
+ *  2) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion, this critic
+ *     will only allow trajectories that are slower than the current speed in order to stop the robot (within
+ *     the robot's acceleration limits). The returned score will be the robot's linear speed squared, multiplied
+ *     by the slowing_factor parameter (default 5.0) added to the result of scoreRotation.
+ *  3) If within the xy_goal_tolerance and the robot has sufficiently small linear motion, this critic will
+ *     score trajectories that have linear movement as invalid and score the rest based on the result of the
+ *     scoreRotation method
+ *
+ * The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance
+ * between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.
+ *  * If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory,
+ *    which is the same as DWA's behavior. This is the default.
+ *  * Otherwise, a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead
+ *    time shorter than sim_time, the critic will be less concerned about overshooting the goal yaw and thus will
+ *    continue to turn faster for longer.
  */
 class RotateToGoalCritic : public dwb_local_planner::TrajectoryCritic
 {
 public:
   void onInit() override;
+  void reset() override;
   bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
                const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
   double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
-private:
-  bool in_window_;
+
+  /**
+   * @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
+   *
+   * This (easily overridden) method assumes that the critic is in the third phase (as described above)
+   * and returns a numeric score for the trajectory relative to the goal yaw.
+   * @param traj Trajectory to score
+   * @return numeric score
+   */
+  virtual double scoreRotation(const dwb_msgs::Trajectory2D& traj);
+
+protected:
+  bool in_window_, rotating_;
   double goal_yaw_;
   double xy_goal_tolerance_;
   double xy_goal_tolerance_sq_;  ///< Cached squared tolerance
+  double current_xy_speed_sq_, stopped_xy_velocity_sq_;
+  double slowing_factor_;
+  double lookahead_time_;
 };
 
 }  // namespace dwb_critics

dwb_critics/src/base_obstacle.cpp

@@ -78,7 +78,7 @@ bool BaseObstacleCritic::isValidCost(const unsigned char cost)
          cost != costmap_->NO_INFORMATION;
 }
 
-void BaseObstacleCritic::addGridScores(sensor_msgs::PointCloud& pc)
+void BaseObstacleCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
 {
   sensor_msgs::ChannelFloat32 grid_scores;
   grid_scores.name = name_;

dwb_critics/src/map_grid.cpp

@@ -175,7 +175,7 @@ double MapGridCritic::scorePose(const geometry_msgs::Pose2D& pose)
   return getScore(cell_x, cell_y);
 }
 
-void MapGridCritic::addGridScores(sensor_msgs::PointCloud& pc)
+void MapGridCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
 {
   sensor_msgs::ChannelFloat32 grid_scores;
   grid_scores.name = name_;

dwb_critics/src/rotate_to_goal.cpp

@@ -32,6 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <dwb_critics/rotate_to_goal.h>
+#include <dwb_local_planner/trajectory_utils.h>
 #include <nav_2d_utils/parameters.h>
 #include <nav_core2/exceptions.h>
 #include <pluginlib/class_list_macros.h>
@@ -46,23 +47,36 @@ PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_local_planner::Traje
 namespace dwb_critics
 {
 
+inline double hypot_sq(double dx, double dy)
+{
+  return dx * dx + dy * dy;
+}
+
 void RotateToGoalCritic::onInit()
 {
   xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+  double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
+  stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
+  critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
+  critic_nh_.param("lookahead_time", lookahead_time_, -1.0);
+  reset();
+}
+
+void RotateToGoalCritic::reset()
+{
+  in_window_ = false;
+  rotating_ = false;
 }
 
 bool RotateToGoalCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
                                  const geometry_msgs::Pose2D& goal,
                                  const nav_2d_msgs::Path2D& global_plan)
 {
-  double dx = pose.x - goal.x,
-         dy = pose.y - goal.y;
-  double dxy_sq = dx * dx + dy * dy;
-  if (dxy_sq > xy_goal_tolerance_sq_)
-  {
-    in_window_ = false;
-  }
+  double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
+  in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
+  current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
+  rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
   goal_yaw_ = goal.theta;
   return true;
 }
@@ -74,20 +88,43 @@ double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
   {
     return 0.0;
   }
+  else if (!rotating_)
+  {
+    double speed_sq = hypot_sq(traj.velocity.x, traj.velocity.y);
+    if (speed_sq >= current_xy_speed_sq_)
+    {
+      throw nav_core2::IllegalTrajectoryException(name_, "Not slowing down near goal.");
+    }
+    return speed_sq * slowing_factor_ + scoreRotation(traj);
+  }
 
   // If we're sufficiently close to the goal, any transforming velocity is invalid
   if (fabs(traj.velocity.x) > EPSILON || fabs(traj.velocity.y) > EPSILON)
   {
     throw nav_core2::IllegalTrajectoryException(name_, "Nonrotation command near goal.");
   }
 
+  return scoreRotation(traj);
+}
+
+double RotateToGoalCritic::scoreRotation(const dwb_msgs::Trajectory2D& traj)
+{
   if (traj.poses.empty())
   {
     throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
   }
 
-  double end_yaw = traj.poses.back().theta;
-  return angles::shortest_angular_distance(end_yaw, goal_yaw_);
+  double end_yaw;
+  if (lookahead_time_ >= 0.0)
+  {
+    geometry_msgs::Pose2D eval_pose = dwb_local_planner::projectPose(traj, lookahead_time_);
+    end_yaw = eval_pose.theta;
+  }
+  else
+  {
+    end_yaw = traj.poses.back().theta;
+  }
+  return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
 }
 
 } /* namespace dwb_critics */

dwb_local_planner/CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES dwb_local_planner debug_dwb_local_planner
+    LIBRARIES dwb_local_planner debug_dwb_local_planner trajectory_utils
     CATKIN_DEPENDS
         dwb_msgs geometry_msgs nav_2d_msgs nav_2d_utils nav_core2
         nav_msgs pluginlib roscpp sensor_msgs tf visualization_msgs
@@ -42,20 +42,27 @@ add_library(debug_dwb_local_planner src/debug_dwb_local_planner.cpp)
 target_link_libraries(debug_dwb_local_planner ${catkin_LIBRARIES} dwb_local_planner)
 add_dependencies(debug_dwb_local_planner ${catkin_EXPORTED_TARGETS})
 
+add_library(trajectory_utils src/trajectory_utils.cpp)
+target_link_libraries(trajectory_utils ${catkin_LIBRARIES})
+
 add_executable(planner_node src/planner_node.cpp)
 target_link_libraries(planner_node ${catkin_LIBRARIES} debug_dwb_local_planner)
 add_dependencies(planner_node ${catkin_EXPORTED_TARGETS})
 
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS roslint)
+  find_package(rostest REQUIRED)
+  find_package(roslint)
   roslint_cpp()
   roslint_add_test()
+
+  catkin_add_gtest(utils_test test/utils_test.cpp)
+  target_link_libraries(utils_test trajectory_utils)
 endif()
 
 install(TARGETS planner_node
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-install(TARGETS dwb_local_planner debug_dwb_local_planner
+install(TARGETS dwb_local_planner debug_dwb_local_planner trajectory_utils
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}

dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h

@@ -120,8 +120,8 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
    * @param best_score If positive, the threshold for early termination
    * @return The full scoring of the input trajectory
    */
-  dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D& traj,
-                                            double best_score = -1);
+  virtual dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D& traj, double best_score = -1);
+
   /**
    * @brief Compute the best command given the current pose and velocity, with possible debug information
    *
@@ -131,12 +131,12 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
    *
    * @param pose      Current robot pose
    * @param velocity  Current robot velocity
-   * @param results   Output param, if not NULL, will be filled in with full evaluation results
+   * @param results   Output param, if not null, will be filled in with full evaluation results
    * @return          Best command
    */
-  nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
-                                                      const nav_2d_msgs::Twist2D& velocity,
-                                                      std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
+  virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
+                                                              const nav_2d_msgs::Twist2D& velocity,
+                                                              std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
 
 
 protected:
@@ -150,9 +150,9 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
   /**
    * @brief Iterate through all the twists and find the best one
    */
-  dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D& pose,
-                                                 const nav_2d_msgs::Twist2D velocity,
-                                                 std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
+  virtual dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D& pose,
+                                                         const nav_2d_msgs::Twist2D velocity,
+                                                         std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
 
   /**
    * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
@@ -164,7 +164,7 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
    *     and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_
    *     of the robot and erases all poses before that.
    */
-  nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped& pose);
+  virtual nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped& pose);
 
   /**
    * @brief Helper method to transform a given pose to the local costmap frame.
@@ -176,6 +176,7 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
   bool prune_plan_;
   double prune_distance_;
   bool debug_trajectory_details_;
+  bool short_circuit_trajectory_evaluation_;
 
   // Plugin handling
   pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
@@ -197,7 +198,7 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
    * @brief Load the critic parameters from the namespace
    * @param name The namespace of this planner.
    */
-  void loadCritics(const std::string name);
+  virtual void loadCritics(const std::string name);
 
   std::vector<std::string> default_critic_namespaces_;
 

dwb_local_planner/include/dwb_local_planner/goal_checker.h

@@ -54,7 +54,7 @@ namespace dwb_local_planner
 class GoalChecker
 {
 public:
-  typedef std::shared_ptr<dwb_local_planner::GoalChecker> Ptr;
+  using Ptr = std::shared_ptr<dwb_local_planner::GoalChecker>;
 
   virtual ~GoalChecker() {}
 
@@ -64,6 +64,11 @@ class GoalChecker
    */
   virtual void initialize(const ros::NodeHandle& nh) = 0;
 
+  /**
+   * @brief Reset the state of the goal checker (if any)
+   */
+  virtual void reset() {}
+
   /**
    * @brief Check whether the goal should be considered reached
    * @param query_pose The pose to check