diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md index 8d9193a1c0023ff02b6aa40f59f07aea80c98fe7..976cdc646f0368e1829ebcd016a4c8b8d06d2260 100644 --- a/nav2_dwb_controller/README.md +++ b/nav2_dwb_controller/README.md @@ -32,3 +32,57 @@ For the most part, the DWB codebase is unchanged, other than what was required f ## New local planner interface See `nav2_core` `Controller` interface, which defines an interface for the controller plugins. These plugins are loaded into the `nav2_controller_server` to compute control commands from requests to the ROS2 action server. + +## DWB Plugins + +DWB is highly configurable through the use of plugins. There are three types of +plugins used. For each of them, a few implementations are available but you can +create custom ones if you need to. + +### Trajectory Generator Plugins + +These plugins generate the set of possible trajectories that should be evaluated +by the critics. The trajectory with the best score determines the output command +velocity. + +There are two trajectory generators provided with Navigation 2. Only one can be +loaded at a time. + +* **StandardTrajectoryGenerator** - This is similar to the trajectory rollout + algorithm used in base_local_planner in ROS 1. +* **LimitedAccelGenerator** - This is similar to DWA used in ROS 1. + +### Goal Checker Plugins + +These plugins check whether we have reached the goal or not. Again, only one can +be loaded at a time. + +* **SimpleGoalChecker** - This checks whether the robot has reached the goal pose +* **StoppedGoalChecker** - This checks whether the robot has reached the goal pose + and come to a stop. + +### Critic Plugins + +These plugins score the trajectories generated by the trajectory generator. +Multiple plugins can be loaded and the sum of their scores determines the chosen +command velocity. + +* **BaseObstacle** - Scores a trajectory based on where the path passes over the + costmap. To use this properly, you must use the inflation layer in costmap to + expand obstacles by the robot's radius. +* **ObstacleFootprint** - Scores a trajectory based on verifying all points along + the robot's footprint don't touch an obstacle marked in the costmap. +* **GoalAlign** - Scores a trajectory based on how well aligned the trajectory is + with the goal pose. +* **GoalDist** - Scores a trajectory based on how close the trajectory gets the robot + to the goal pose. +* **PathAlign** - Scores a trajectory based on how well it is aligned to the path + provided by the global planner. +* **PathDist** - Scores a trajectory based on how far it ends up from the path + provided by the global planner. +* **PreferForward** - Scores trajectories that move the robot forwards more highly +* **RotateToGoal** - Only allows the robot to rotate to the goal orientation when it + is sufficiently close to the goal location +* **Oscillation** - Prevents the robot from just moving backwards and forwards. +* **Twirling** - Prevents holonomic robots from spinning as they make their way to + the goal. diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index ba381910cdf703852407c89ff6003ff3860ac8b9..933a39454db79510b0d5bff83251997304ddc264 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -81,7 +81,6 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); declare_parameter_if_not_declared(node_, "goal_checker_name", rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker"))); - declare_parameter_if_not_declared(node_, "use_dwa", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared(node_, "transform_tolerance", rclcpp::ParameterValue(0.1)); std::string traj_generator_name; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp index b37cdf59880f9c41fb6115edceb1afbf46c017eb..edbf001d4b0523df4d62d3aea1fdc655bce140b8 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp @@ -50,7 +50,6 @@ class LimitedAccelGenerator : public StandardTrajectoryGenerator { public: void initialize(const nav2_util::LifecycleNode::SharedPtr & nh) override; - void checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh) override; void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override; dwb_msgs::msg::Trajectory2D generateTrajectory( const geometry_msgs::msg::Pose2D & start_pose, diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 62ab2d8ef317ed668f45beec5063f5db2acddbd3..5c4f965f4ff2ae89335cdd5f11bf0a90c9edb122 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -71,15 +71,6 @@ protected: */ virtual void initializeIterator(const nav2_util::LifecycleNode::SharedPtr & nh); - /** - * @brief Check if the deprecated use_dwa parameter is set to the functionality that matches this class - * - * The functionality guarded by the use_dwa parameter has been split between this class and the derived - * LimitedAccelGenerator. If use_dwa was false, this class should be used. If it was true, then LimitedAccelGenerator. - * If this is NOT the case, this function will throw an exception. - */ - virtual void checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh); - /** * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits * diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp index 90b935b9b15bb5088b9277910c875607f432ce27..05adae5b4c33e1f65e2cf4e8a2dafb1a72151f38 100644 --- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp @@ -64,17 +64,6 @@ void LimitedAccelGenerator::initialize(const nav2_util::LifecycleNode::SharedPtr } } -void LimitedAccelGenerator::checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh) -{ - bool use_dwa = false; - nh->get_parameter("use_dwa", use_dwa); - if (!use_dwa) { - throw nav2_core:: - PlannerException("Deprecated parameter use_dwa set to false. " - "Please use StandardTrajectoryGenerator for that functionality."); - } -} - void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) { // Limit our search space to just those within the limited acceleration_time diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 0260d1bd63b1d72e81066d831a63649176f52760..6d98b44780eb337378cdd606372158b37519bdde 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -59,7 +59,6 @@ void StandardTrajectoryGenerator::initialize(const nav2_util::LifecycleNode::Sha "discretize_by_time", rclcpp::ParameterValue(false)); nh->get_parameter("sim_time", sim_time_); - checkUseDwaParam(nh); /* * If discretize_by_time, then sim_granularity represents the amount of time that should be between @@ -88,18 +87,6 @@ void StandardTrajectoryGenerator::initializeIterator( velocity_iterator_->initialize(nh, kinematics_); } -void StandardTrajectoryGenerator::checkUseDwaParam( - const nav2_util::LifecycleNode::SharedPtr & nh) -{ - bool use_dwa = true; - nh->get_parameter("use_dwa", use_dwa); - if (use_dwa) { - throw nav2_core:: - PlannerException("Deprecated parameter use_dwa set to true. " - "Please use LimitedAccelGenerator for that functionality."); - } -} - void StandardTrajectoryGenerator::startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity) { diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index 1bee7fdda2c052ceaa0abc397bd69103efddd620..26a5e052f0e0aa0a9ff75d1892402ed6c79d9c39 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -71,8 +71,6 @@ std::vector<rclcpp::Parameter> getDefaultKinematicParameters() parameters.push_back(rclcpp::Parameter("max_speed_xy", 0.55)); parameters.push_back(rclcpp::Parameter("min_speed_theta", 0.4)); - parameters.push_back(rclcpp::Parameter("use_dwa", false)); - return parameters; } @@ -210,26 +208,9 @@ TEST(VelocityIterator, no_limits_samples) checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0); } -TEST(VelocityIterator, dwa_gen_exception) -{ - auto nh = makeTestNode("dwa_gen_exception"); - nh->set_parameters({rclcpp::Parameter("use_dwa", true)}); - StandardTrajectoryGenerator gen; - EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException); -} - -TEST(VelocityIterator, no_dwa_gen_exception) -{ - auto nh = makeTestNode("no_dwa_gen_exception"); - nh->set_parameters({rclcpp::Parameter("use_dwa", false)}); - dwb_plugins::LimitedAccelGenerator gen; - EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException); -} - TEST(VelocityIterator, dwa_gen) { auto nh = makeTestNode("dwa_gen"); - nh->set_parameters({rclcpp::Parameter("use_dwa", true)}); nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh); @@ -242,7 +223,6 @@ TEST(VelocityIterator, dwa_gen) TEST(VelocityIterator, nonzero) { auto nh = makeTestNode("nonzero"); - nh->set_parameters({rclcpp::Parameter("use_dwa", true)}); nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh); @@ -402,7 +382,6 @@ TEST(TrajectoryGenerator, accel) TEST(TrajectoryGenerator, dwa) { auto nh = makeTestNode("dwa"); - nh->set_parameters({rclcpp::Parameter("use_dwa", true)}); nh->set_parameters({rclcpp::Parameter("sim_period", 1.0)}); nh->set_parameters({rclcpp::Parameter("sim_time", 5.0)}); nh->set_parameters({rclcpp::Parameter("discretize_by_time", true)}); diff --git a/sphinx_doc/howtos/tunning/params/tunable-params.rst b/sphinx_doc/howtos/tunning/params/tunable-params.rst index 315d4346a7b8db25d89b9999e8619031d7d19eac..e932fd37a9eb7bde57da012969177144b7f1d297 100644 --- a/sphinx_doc/howtos/tunning/params/tunable-params.rst +++ b/sphinx_doc/howtos/tunning/params/tunable-params.rst @@ -137,7 +137,6 @@ Hosts the `DWB` controller. sim_time: 1.7 trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator transform_tolerance: 0.2 - use_dwa: false use_sim_time: true vx_samples: 20 vy_samples: 5