diff --git a/dwb_local_planner/README.md b/dwb_local_planner/README.md index 870ac46..4e9ff24 100644 --- a/dwb_local_planner/README.md +++ b/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. * `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. diff --git a/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h b/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h index 55fb8ce..f74fb19 100644 --- a/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h +++ b/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h @@ -104,7 +104,7 @@ class TrajectoryCritic /** * @brief Reset the state of the critic * - * Reset is called when the planner receives a new global goal. + * Reset is called when the planner receives a new global plan. * This can be used to discard information specific to one plan. */ virtual void reset() {} diff --git a/dwb_local_planner/src/dwb_local_planner.cpp b/dwb_local_planner/src/dwb_local_planner.cpp index a87e8b9..d338ceb 100644 --- a/dwb_local_planner/src/dwb_local_planner.cpp +++ b/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,