Call reset on new plan, not new goal

The `reset()` functions were called in `setGoalPose()`, i.e. when the
user gives a new goal. However, when the local planner fails and the
global planner replans, `setGoalPose()` is not called, but `setPlan()`
is. This commit makes sure that `reset()` is also called in this case.

This is important in situations where e.g. the robot is already in the
goal pose, but cannot turn to the final orientation because of
obstacles. The new global plan might require the robot to leave the goal
pose again, and so the critics (like for example the RotateToGoal
critic) need to be reset.

Author: mintar

dwb_local_planner/README.md

@@ -72,7 +72,7 @@ virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geomet
 
  * `void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)` - called once on startup, and then calls `onInit`
  * `void onInit()` - May be overwritten to load parameters as needed.
- * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global goal via `setGoalPose`.
+ * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global plan via `setPlan`.
  * `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)` - called once per iteration of the planner, prior to the evaluation of all the trajectories
  * `double scoreTrajectory(const dwb_msgs::Trajectory2D& traj)` - called once per trajectory
  * `void debrief(const nav_2d_msgs::Twist2D& cmd_vel)` - called after all the trajectories to notify what trajectory was chosen.

dwb_local_planner/src/dwb_local_planner.cpp

@@ -161,18 +161,18 @@ void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose)
 {
   ROS_INFO_NAMED("DWBLocalPlanner", "New Goal Received.");
   goal_pose_ = goal_pose;
-  traj_generator_->reset();
-  goal_checker_->reset();
-  for (TrajectoryCritic::Ptr critic : critics_)
-  {
-    critic->reset();
-  }
 }
 
 void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path)
 {
   pub_.publishGlobalPlan(path);
   global_plan_ = path;
+  traj_generator_->reset();
+  goal_checker_->reset();
+  for (TrajectoryCritic::Ptr critic : critics_)
+  {
+    critic->reset();
+  }
 }
 
 nav_2d_msgs::Twist2DStamped DWBLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,