From c40b7ca2dd523b1b215d8c8bdf6db5d8eddab109 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Wed, 30 Jan 2019 13:21:31 +0100 Subject: [PATCH 1/2] Call reset on new plan, not new goal The documentation on `reset()` says: > called at the beginning of every new navigation, i.e. when we get a > new global plan 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. --- dwb_local_planner/src/dwb_local_planner.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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, From 9365c17f5644808d8fbdbe888f8492405438401c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 3 Jul 2020 12:55:25 +0200 Subject: [PATCH 2/2] Revert "Update reset documentation (#31)" This reverts commit 8248d6c67b6279e446cfa0c5c63bfeb728b386f6. --- dwb_local_planner/README.md | 2 +- dwb_local_planner/include/dwb_local_planner/trajectory_critic.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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() {}