diff --git a/nav2_dwb_controller/dwb_controller/CMakeLists.txt b/nav2_dwb_controller/dwb_controller/CMakeLists.txt
index bcc4f3336c338650cddfbdb00a5d14b8fd821c79..9d5db2bbf06e6334380c2afb8f93e6e869f93cd6 100644
--- a/nav2_dwb_controller/dwb_controller/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_controller/CMakeLists.txt
@@ -4,9 +4,9 @@ project(dwb_controller)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav2_util REQUIRED)
-find_package(nav2_tasks REQUIRED)
find_package(nav2_robot REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(dwb_core REQUIRED)
@@ -35,8 +35,8 @@ add_library(${library_name}
set(dependencies
rclcpp
+ rclcpp_action
std_msgs
- nav2_tasks
nav2_robot
nav2_msgs
dwb_core
diff --git a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
index 48d511a93381329eac5974e8a37fe8a6376bea57..99714f9bb1dccb7a0623df68fb54f39aef5ed664 100644
--- a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
+++ b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
@@ -24,7 +24,8 @@
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_2d_utils/odom_subscriber.hpp"
#include "nav2_lifecycle/lifecycle_node.hpp"
-#include "nav2_tasks/follow_path_task.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_util/simple_action_server.hpp"
namespace dwb_controller
{
@@ -36,6 +37,7 @@ public:
~DwbController();
protected:
+ // The lifecycle interface
nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
@@ -43,23 +45,29 @@ protected:
nav2_lifecycle::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
nav2_lifecycle::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
- // This module is a task server that implements the FollowPath task
- std::unique_ptr<nav2_tasks::FollowPathTaskServer> task_server_;
- nav2_tasks::TaskStatus followPath(const nav2_tasks::FollowPathCommand::SharedPtr path);
+ using GoalHandle = rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>;
+ using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>;
+
+ // Our action server implements the FollowPath action
+ std::unique_ptr<ActionServer> action_server_;
+
+ // The action server callback
+ void followPath(const std::shared_ptr<GoalHandle> goal_handle);
bool isGoalReached(const nav_2d_msgs::msg::Pose2DStamped & pose2d);
void publishVelocity(const nav_2d_msgs::msg::Twist2DStamped & velocity);
void publishZeroVelocity();
bool getRobotPose(nav_2d_msgs::msg::Pose2DStamped & pose2d);
- // It uses a costmap node
- dwb_core::CostmapROSPtr costmap_ros_;
+ // The DWBController contains a costmap node
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<std::thread> costmap_thread_;
// Publishers and subscribers
std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+ // The local planner
std::unique_ptr<dwb_core::DWBLocalPlanner> planner_;
};
diff --git a/nav2_dwb_controller/dwb_controller/package.xml b/nav2_dwb_controller/dwb_controller/package.xml
index b7a8f2041a35753e7c3799d686ba24eb7f9607ad..83e615248aa56fea8a57a2ed4cd4cc85aa34d2af 100644
--- a/nav2_dwb_controller/dwb_controller/package.xml
+++ b/nav2_dwb_controller/dwb_controller/package.xml
@@ -11,8 +11,8 @@
<build_depend>nav2_common</build_depend>
<depend>rclcpp</depend>
+ <depend>rclcpp_action</depend>
<depend>std_msgs</depend>
- <depend>nav2_tasks</depend>
<depend>nav2_util</depend>
<depend>nav2_robot</depend>
<depend>nav2_msgs</depend>
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 133343f9e1642a98a60f1000d9331317ab0752e3..8989385572ef4107289657d2a6ef93648c95d959 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -24,13 +24,12 @@
#include "dwb_controller/progress_checker.hpp"
using namespace std::chrono_literals;
-using nav2_tasks::TaskStatus;
namespace nav2_dwb_controller
{
DwbController::DwbController()
-: LifecycleNode("dwb_controller")
+: LifecycleNode("dwb_controller", "", true)
{
RCLCPP_INFO(get_logger(), "Creating");
@@ -62,10 +61,9 @@ DwbController::on_configure(const rclcpp_lifecycle::State & state)
odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this);
vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
- task_server_ = std::make_unique<nav2_tasks::FollowPathTaskServer>(node);
- task_server_->on_configure(state);
- task_server_->setExecuteCallback(
- std::bind(&DwbController::followPath, this, std::placeholders::_1));
+ // Create the action server that we implement with our followPath method
+ action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "FollowPath",
+ std::bind(&DwbController::followPath, this, std::placeholders::_1));
return nav2_lifecycle::CallbackReturn::SUCCESS;
}
@@ -78,7 +76,6 @@ DwbController::on_activate(const rclcpp_lifecycle::State & state)
planner_->on_activate(state);
costmap_ros_->on_activate(state);
vel_pub_->on_activate();
- task_server_->on_activate(state);
return nav2_lifecycle::CallbackReturn::SUCCESS;
}
@@ -91,7 +88,6 @@ DwbController::on_deactivate(const rclcpp_lifecycle::State & state)
planner_->on_deactivate(state);
costmap_ros_->on_deactivate(state);
vel_pub_->on_deactivate();
- task_server_->on_deactivate(state);
return nav2_lifecycle::CallbackReturn::SUCCESS;
}
@@ -104,13 +100,12 @@ DwbController::on_cleanup(const rclcpp_lifecycle::State & state)
// Cleanup the helper classes
planner_->on_cleanup(state);
costmap_ros_->on_cleanup(state);
- task_server_->on_cleanup(state);
// Release any allocated resources
planner_.reset();
odom_sub_.reset();
vel_pub_.reset();
- task_server_.reset();
+ action_server_.reset();
return nav2_lifecycle::CallbackReturn::SUCCESS;
}
@@ -129,66 +124,74 @@ DwbController::on_shutdown(const rclcpp_lifecycle::State &)
return nav2_lifecycle::CallbackReturn::SUCCESS;
}
-TaskStatus
-DwbController::followPath(const nav2_tasks::FollowPathCommand::SharedPtr command)
+void
+DwbController::followPath(const std::shared_ptr<GoalHandle> goal_handle)
{
- RCLCPP_INFO(get_logger(), "Starting controller");
+ RCLCPP_INFO(get_logger(), "Received a goal, begin following path");
+ auto result = std::make_shared<nav2_msgs::action::FollowPath::Result>();
+
+ std::shared_ptr<GoalHandle> current_goal_handle = goal_handle;
+
+ rclcpp::Rate loop_rate(100ms); // period vs. hz
+
+ auto goal = current_goal_handle->get_goal();
+
try {
- auto path = nav_2d_utils::pathToPath2D(*command);
+ auto path = nav_2d_utils::pathToPath2D(goal->path);
+ RCLCPP_DEBUG(get_logger(), "Providing path to the local planner");
planner_->setPlan(path);
- RCLCPP_INFO(get_logger(), "Initialized");
ProgressChecker progress_checker(std::shared_ptr<rclcpp::Node>(this, [](auto) {}));
rclcpp::Rate loop_rate(10);
while (rclcpp::ok()) {
nav_2d_msgs::msg::Pose2DStamped pose2d;
if (!getRobotPose(pose2d)) {
- RCLCPP_INFO(get_logger(), "No pose. Stopping robot");
+ RCLCPP_INFO(get_logger(), "No pose. Stopping the robot");
publishZeroVelocity();
} else {
if (isGoalReached(pose2d)) {
+ RCLCPP_INFO(get_logger(), "Reached the goal");
break;
}
progress_checker.check(pose2d);
auto velocity = odom_sub_->getTwist();
auto cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
- publishVelocity(cmd_vel_2d);
+
RCLCPP_INFO(get_logger(), "Publishing velocity at time %.2f", now().seconds());
+ publishVelocity(cmd_vel_2d);
- // Check if this task has been canceled
- if (task_server_->cancelRequested()) {
- RCLCPP_INFO(this->get_logger(), "execute: task has been canceled");
- task_server_->setCanceled();
+ if (current_goal_handle->is_canceling()) {
+ RCLCPP_INFO(this->get_logger(), "Canceling execution of the local planner");
+ current_goal_handle->canceled(result);
publishZeroVelocity();
- return TaskStatus::CANCELED;
+ return;
}
// Check if there is an update to the path to follow
- if (task_server_->updateRequested()) {
- // Get the new, updated path
- auto path_cmd = std::make_shared<nav2_tasks::FollowPathCommand>();
- task_server_->getCommandUpdate(path_cmd);
- task_server_->setUpdated();
-
- // and pass it to the local planner
- auto path = nav_2d_utils::pathToPath2D(*path_cmd);
+ if (action_server_->preempt_requested()) {
+ RCLCPP_INFO(get_logger(), "Received a new goal, pre-empting the old one");
+ current_goal_handle = action_server_->get_updated_goal_handle();
+ goal = current_goal_handle->get_goal();
+
+ // If so, pass the new path to the local planner
+ auto path = nav_2d_utils::pathToPath2D(goal->path);
planner_->setPlan(path);
}
}
+
loop_rate.sleep();
}
} catch (nav_core2::PlannerException & e) {
RCLCPP_ERROR(this->get_logger(), e.what());
publishZeroVelocity();
- return TaskStatus::FAILED;
+ current_goal_handle->abort(result);
+ return;
}
- nav2_tasks::FollowPathResult result;
- task_server_->setResult(result);
+ RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result");
+ current_goal_handle->succeed(result);
publishZeroVelocity();
-
- return TaskStatus::SUCCEEDED;
}
void DwbController::publishVelocity(const nav_2d_msgs::msg::Twist2DStamped & velocity)
@@ -203,6 +206,7 @@ void DwbController::publishZeroVelocity()
velocity.velocity.x = 0;
velocity.velocity.y = 0;
velocity.velocity.theta = 0;
+
publishVelocity(velocity);
}
diff --git a/nav2_dwb_controller/dwb_controller/src/main.cpp b/nav2_dwb_controller/dwb_controller/src/main.cpp
index c24c2858d705081121f77933d0594e7f0872205d..d50759ef01c87a16f9d3c491ba15f7106a7aaf3e 100644
--- a/nav2_dwb_controller/dwb_controller/src/main.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/main.cpp
@@ -13,8 +13,9 @@
// limitations under the License.
#include <memory>
-#include "rclcpp/rclcpp.hpp"
+
#include "dwb_controller/dwb_controller.hpp"
+#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
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 208a700043e205605529a8a63337d1a872c36f03..f825d013a7b42017cccfb06af3ce4588e4dc43da 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -62,32 +62,38 @@ DWBLocalPlanner::DWBLocalPlanner(
goal_checker_loader_("dwb_core", "dwb_core::GoalChecker"),
critic_loader_("dwb_core", "dwb_core::TrajectoryCritic")
{
+ node_->declare_parameter("critics");
+ node_->declare_parameter("prune_plan", rclcpp::ParameterValue(true));
+ node_->declare_parameter("prune_distance", rclcpp::ParameterValue(1.0));
+ node_->declare_parameter("debug_trajectory_details", rclcpp::ParameterValue(false));
+ node_->declare_parameter("trajectory_generator_name",
+ rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator")));
+ node_->declare_parameter("goal_checker_name",
+ rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker")));
+ node_->declare_parameter("use_dwa", rclcpp::ParameterValue(false));
}
nav2_lifecycle::CallbackReturn
DWBLocalPlanner::on_configure(const rclcpp_lifecycle::State & state)
{
std::string traj_generator_name;
- nh_->get_parameter_or("trajectory_generator_name", traj_generator_name,
- std::string("dwb_plugins::StandardTrajectoryGenerator"));
- traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name);
- traj_generator_->initialize(nh_);
-
std::string goal_checker_name;
- node_->get_parameter_or("prune_plan", prune_plan_, true);
- node_->get_parameter_or("prune_distance", prune_distance_, 1.0);
- node_->get_parameter_or("debug_trajectory_details", debug_trajectory_details_, false);
- node_->get_parameter_or("trajectory_generator_name", traj_generator_name,
- std::string("dwb_plugins::StandardTrajectoryGenerator"));
- node_->get_parameter_or("goal_checker_name", goal_checker_name,
- std::string("dwb_plugins::SimpleGoalChecker"));
- goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name);
- goal_checker_->initialize(nh_);
+ node_->get_parameter("prune_plan", prune_plan_);
+ node_->get_parameter("prune_distance", prune_distance_);
+ node_->get_parameter("debug_trajectory_details", debug_trajectory_details_);
+ node_->get_parameter("trajectory_generator_name", traj_generator_name);
+ node_->get_parameter("goal_checker_name", goal_checker_name);
pub_ = std::make_unique<DWBPublisher>(node_);
pub_->on_configure(state);
+ traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name);
+ goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name);
+
+ traj_generator_->initialize(nh_);
+ goal_checker_->initialize(nh_);
+
loadCritics();
return nav2_lifecycle::CallbackReturn::SUCCESS;
@@ -155,11 +161,14 @@ DWBLocalPlanner::loadCritics()
for (unsigned int i = 0; i < critic_names.size(); i++) {
std::string plugin_name = critic_names[i];
std::string plugin_class;
- node_->get_parameter_or(plugin_name + "/class", plugin_class, plugin_name);
+
+ node_->declare_parameter(plugin_name + "/class", rclcpp::ParameterValue(plugin_name));
+ node_->get_parameter(plugin_name + "/class", plugin_class);
+
plugin_class = resolveCriticClassName(plugin_class);
TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(plugin_class);
- RCLCPP_INFO(nh_->get_logger(),
+ RCLCPP_INFO(node_->get_logger(),
"Using critic \"%s\" (%s)", plugin_name.c_str(), plugin_class.c_str());
critics_.push_back(plugin);
plugin->initialize(node_, plugin_name, costmap_ros_);
@@ -184,6 +193,20 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters()
critic_names.push_back("GoalDist"); // prefers trajectories that go towards
// (local) goal, based on wave propagation
node_->set_parameters({rclcpp::Parameter("critics", critic_names)});
+
+ node_->declare_parameter("path_distance_bias");
+ node_->declare_parameter("goal_distance_bias");
+ node_->declare_parameter("occdist_scale");
+ node_->declare_parameter("max_scaling_factor");
+ node_->declare_parameter("scaling_speed");
+ node_->declare_parameter("PathAlign.scale");
+ node_->declare_parameter("GoalAlign.scale");
+ node_->declare_parameter("PathDist.scale");
+ node_->declare_parameter("GoalDist.scale");
+ node_->declare_parameter("ObstacleFootprint.scale");
+ node_->declare_parameter("ObstacleFootprint.max_scaling_factor");
+ node_->declare_parameter("ObstacleFootprint.scaling_speed");
+
/* *INDENT-OFF* */
nav_2d_utils::moveParameter(node_, "path_distance_bias", "PathAlign.scale", 32.0, false);
nav_2d_utils::moveParameter(node_, "goal_distance_bias", "GoalAlign.scale", 24.0, false);
diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
index f5032ce3f762d5a96fa9992f1d28f22c6abea7af..1dd7c2023f3fca5cd2713696908174f1b9276662 100644
--- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
@@ -45,7 +45,9 @@ namespace dwb_critics
void BaseObstacleCritic::onInit()
{
costmap_ = costmap_ros_->getCostmap();
- nh_->get_parameter_or(name_ + ".sum_scores", sum_scores_, false);
+
+ nh_->declare_parameter(name_ + ".sum_scores", rclcpp::ParameterValue(false));
+ nh_->get_parameter(name_ + ".sum_scores", sum_scores_);
}
double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
index 0cbf72b38422d5aee84a2c04c9ce004792eb57be..8c66565b209daa1f867bb3f9b31b60a1906bbf23 100644
--- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
@@ -69,8 +69,10 @@ void MapGridCritic::onInit()
// Always set to true, but can be overriden by subclasses
stop_on_failure_ = true;
+ nh_->declare_parameter(name_ + ".aggregation_type", rclcpp::ParameterValue(std::string("last")));
+
std::string aggro_str;
- nh_->get_parameter_or(name_ + ".aggregation_type", aggro_str, std::string("last"));
+ nh_->get_parameter(name_ + ".aggregation_type", aggro_str);
std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower);
if (aggro_str == "last") {
aggregationType_ = ScoreAggregationType::Last;
diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
index a0b737d910f21c650765bb1ddb83cbf525ffc219..708059b06e02879d08632585f9cb1af10a0902ac 100644
--- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp
@@ -97,6 +97,8 @@ void OscillationCritic::onInit()
oscillation_reset_time_ = nav2_util::duration_from_seconds(
nav_2d_utils::searchAndGetParam(nh_, "oscillation_reset_time", -1.0));
+ nh_->declare_parameter(name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05));
+
/**
* Historical Parameter Loading
* If x_only_threshold is set, use that.
@@ -104,7 +106,7 @@ void OscillationCritic::onInit()
* If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that.
* Otherwise, set x_only_threshold_ to 0.05
*/
- nh_->get_parameter_or(name_ + ".x_only_threshold", x_only_threshold_, 0.05);
+ nh_->get_parameter(name_ + ".x_only_threshold", x_only_threshold_);
// TODO(crdelsey): How to handle searchParam?
// std::string resolved_name;
// if (nh_->hasParam("x_only_threshold"))
diff --git a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
index ed2fdd4fb62d90c04ec05c2ac6c53ae3b2177ba9..82b08ce2e9242643abb37f993f9c379e98e08f65 100644
--- a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp
@@ -43,10 +43,15 @@ namespace dwb_critics
void PreferForwardCritic::onInit()
{
- nh_->get_parameter_or(name_ + ".penalty", penalty_, 1.0);
- nh_->get_parameter_or(name_ + ".strafe_x", strafe_x_, 0.1);
- nh_->get_parameter_or(name_ + ".strafe_theta", strafe_theta_, 0.2);
- nh_->get_parameter_or(name_ + ".theta_scale", theta_scale_, 10.0);
+ nh_->declare_parameter(name_ + ".penalty", rclcpp::ParameterValue(1.0));
+ nh_->declare_parameter(name_ + ".strafe_x", rclcpp::ParameterValue(0.1));
+ nh_->declare_parameter(name_ + ".strafe_theta", rclcpp::ParameterValue(0.2));
+ nh_->declare_parameter(name_ + ".theta_scale", rclcpp::ParameterValue(10.0));
+
+ nh_->get_parameter(name_ + ".penalty", penalty_);
+ nh_->get_parameter(name_ + ".strafe_x", strafe_x_);
+ nh_->get_parameter(name_ + ".strafe_theta", strafe_theta_);
+ nh_->get_parameter(name_ + ".theta_scale", theta_scale_);
}
double PreferForwardCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
diff --git a/nav2_dwb_controller/dwb_critics/src/twirling.cpp b/nav2_dwb_controller/dwb_critics/src/twirling.cpp
index 979a8aa18515d3e4a0995df5d8eefc9ba4b04465..c203469356f43fb8c01caaca1c4213e8f43ab4c2 100644
--- a/nav2_dwb_controller/dwb_critics/src/twirling.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/twirling.cpp
@@ -39,8 +39,10 @@ namespace dwb_critics
{
void TwirlingCritic::onInit()
{
+ nh_->declare_parameter(name_ + ".scale", rclcpp::ParameterValue(0.0));
+
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
- nh_->get_parameter_or(name_ + ".scale", scale_, 0.0);
+ nh_->get_parameter(name_ + ".scale", scale_);
}
double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
index 9d76f9729efcb73d0329e5fc29ce699936ce3fb8..7dc382767eee29ed18b5f7c1c1020ef8d2a3327b 100644
--- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
@@ -58,20 +58,35 @@ void KinematicParameters::initialize(const nav2_lifecycle::LifecycleNode::Shared
moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");
- nh->get_parameter_or_set("min_vel_x", min_vel_x_, 0.0);
- nh->get_parameter_or_set("min_vel_y", min_vel_y_, 0.0);
- nh->get_parameter_or_set("max_vel_x", max_vel_x_, 0.0);
- nh->get_parameter_or_set("max_vel_y", max_vel_y_, 0.0);
- nh->get_parameter_or_set("max_vel_theta", max_vel_theta_, 0.0);
- nh->get_parameter_or_set("min_speed_xy", min_speed_xy_, 0.0);
- nh->get_parameter_or_set("max_speed_xy", max_speed_xy_, 0.0);
- nh->get_parameter_or_set("min_speed_theta", min_speed_theta_, 0.0);
- nh->get_parameter_or_set("acc_lim_x", acc_lim_x_, 0.0);
- nh->get_parameter_or_set("acc_lim_y", acc_lim_y_, 0.0);
- nh->get_parameter_or_set("acc_lim_theta", acc_lim_theta_, 0.0);
- nh->get_parameter_or_set("decel_lim_x", decel_lim_x_, 0.0);
- nh->get_parameter_or_set("decel_lim_y", decel_lim_y_, 0.0);
- nh->get_parameter_or_set("decel_lim_theta", decel_lim_theta_, 0.0);
+ nh->declare_parameter("min_vel_x", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("min_vel_y", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("max_vel_x", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("max_vel_y", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("max_vel_theta", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("min_speed_xy", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("max_speed_xy", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("min_speed_theta", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("acc_lim_x", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("acc_lim_y", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("acc_lim_theta", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("decel_lim_x", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("decel_lim_y", rclcpp::ParameterValue(0.0));
+ nh->declare_parameter("decel_lim_theta", rclcpp::ParameterValue(0.0));
+
+ nh->get_parameter("min_vel_x", min_vel_x_);
+ nh->get_parameter("min_vel_y", min_vel_y_);
+ nh->get_parameter("max_vel_x", max_vel_x_);
+ nh->get_parameter("max_vel_y", max_vel_y_);
+ nh->get_parameter("max_vel_theta", max_vel_theta_);
+ nh->get_parameter("min_speed_xy", min_speed_xy_);
+ nh->get_parameter("max_speed_xy", max_speed_xy_);
+ nh->get_parameter("min_speed_theta", min_speed_theta_);
+ nh->get_parameter("acc_lim_x", acc_lim_x_);
+ nh->get_parameter("acc_lim_y", acc_lim_y_);
+ nh->get_parameter("acc_lim_theta", acc_lim_theta_);
+ nh->get_parameter("decel_lim_x", decel_lim_x_);
+ nh->get_parameter("decel_lim_y", decel_lim_y_);
+ nh->get_parameter("decel_lim_theta", decel_lim_theta_);
min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
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 684d5a2a38ad3c70dd28fd320f22b1768635d221..25e8d3621e484ecaec76f44b51490ceb28936316 100644
--- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
@@ -46,6 +46,9 @@ namespace dwb_plugins
void LimitedAccelGenerator::initialize(const nav2_lifecycle::LifecycleNode::SharedPtr & nh)
{
StandardTrajectoryGenerator::initialize(nh);
+
+ nh->declare_parameter("sim_period");
+
if (nh->get_parameter("sim_period", acceleration_time_)) {
} else {
double controller_frequency = nav_2d_utils::searchAndGetParam(
@@ -64,7 +67,7 @@ void LimitedAccelGenerator::initialize(const nav2_lifecycle::LifecycleNode::Shar
void LimitedAccelGenerator::checkUseDwaParam(const nav2_lifecycle::LifecycleNode::SharedPtr & nh)
{
bool use_dwa;
- nh->get_parameter_or("use_dwa", use_dwa, true);
+ nh->get_parameter("use_dwa", use_dwa);
if (!use_dwa) {
throw nav_core2::PlannerException("Deprecated parameter use_dwa set to false. "
"Please use StandardTrajectoryGenerator for that functionality.");
diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
index a4cc85c84daa17fbec03671201f93b62ca9a8b17..da95a820c04ad70900a57d2bf3fa4117fb17c713 100644
--- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp
@@ -47,8 +47,12 @@ SimpleGoalChecker::SimpleGoalChecker()
void SimpleGoalChecker::initialize(const nav2_lifecycle::LifecycleNode::SharedPtr & nh)
{
- nh->get_parameter_or("xy_goal_tolerance", xy_goal_tolerance_, 0.25);
- nh->get_parameter_or("yaw_goal_tolerance", yaw_goal_tolerance_, 0.25);
+ nh->declare_parameter("xy_goal_tolerance", rclcpp::ParameterValue(0.25));
+ nh->declare_parameter("yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
+
+ nh->get_parameter("xy_goal_tolerance", xy_goal_tolerance_);
+ nh->get_parameter("yaw_goal_tolerance", yaw_goal_tolerance_);
+
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
}
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 b168e2db6f2f402c91fe2614a5ee994d6b85baf0..efeb6f8f20cdaa87e12e8073847894865a6e40fd 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -54,7 +54,10 @@ void StandardTrajectoryGenerator::initialize(const nav2_lifecycle::LifecycleNode
kinematics_->initialize(nh);
initializeIterator(nh);
- nh->get_parameter_or("sim_time", sim_time_, 1.7);
+ nh->declare_parameter("sim_time", rclcpp::ParameterValue(1.7));
+ nh->declare_parameter("discretize_by_time", rclcpp::ParameterValue(false));
+
+ nh->get_parameter("sim_time", sim_time_);
checkUseDwaParam(nh);
/*
@@ -65,7 +68,7 @@ void StandardTrajectoryGenerator::initialize(const nav2_lifecycle::LifecycleNode
* two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
* angular distance between two successive points.
*/
- nh->get_parameter_or("discretize_by_time", discretize_by_time_, false);
+ nh->get_parameter("discretize_by_time", discretize_by_time_);
if (discretize_by_time_) {
time_granularity_ = loadParameterWithDeprecation(
nh, "time_granularity", "sim_granularity", 0.5);
@@ -88,7 +91,7 @@ void StandardTrajectoryGenerator::checkUseDwaParam(
const nav2_lifecycle::LifecycleNode::SharedPtr & nh)
{
bool use_dwa;
- nh->get_parameter_or("use_dwa", use_dwa, false);
+ nh->get_parameter("use_dwa", use_dwa);
if (use_dwa) {
throw nav_core2::PlannerException("Deprecated parameter use_dwa set to true. "
"Please use LimitedAccelGenerator for that functionality.");
diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
index 9a1af7acfd0c2e170ff104e4d179972474d19a65..d32c8dfad8797a79a59816da742420bc20a29202 100644
--- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp
@@ -50,8 +50,12 @@ StoppedGoalChecker::StoppedGoalChecker()
void StoppedGoalChecker::initialize(const nav2_lifecycle::LifecycleNode::SharedPtr & nh)
{
SimpleGoalChecker::initialize(nh);
- nh->get_parameter_or("rot_stopped_velocity", rot_stopped_velocity_, 0.25);
- nh->get_parameter_or("trans_stopped_velocity", trans_stopped_velocity_, 0.25);
+
+ nh->declare_parameter("rot_stopped_velocity", rclcpp::ParameterValue(0.25));
+ nh->declare_parameter("trans_stopped_velocity", rclcpp::ParameterValue(0.25));
+
+ nh->get_parameter("rot_stopped_velocity", rot_stopped_velocity_);
+ nh->get_parameter("trans_stopped_velocity", trans_stopped_velocity_);
}
bool StoppedGoalChecker::isGoalReached(
diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
index 1e20e770bfca97366171c8a666e66ace3017d692..ee039a7ef3af933528a6a7654d0114741eb7bbfb 100644
--- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
@@ -43,8 +43,13 @@ void XYThetaIterator::initialize(
KinematicParameters::Ptr kinematics)
{
kinematics_ = kinematics;
- nh->get_parameter_or("vx_samples", vx_samples_, 20);
- nh->get_parameter_or("vy_samples", vy_samples_, 5);
+
+ nh->declare_parameter("vx_samples", rclcpp::ParameterValue(20));
+ nh->declare_parameter("vy_samples", rclcpp::ParameterValue(5));
+
+ nh->get_parameter("vx_samples", vx_samples_);
+ nh->get_parameter("vy_samples", vy_samples_);
+
vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples",
20);
}
diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
index da1f01fd4b17c6175c4fb01fae11180b433c8893..84785cdb43ec5e6c38c5416622bdf8d342b5033f 100644
--- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
+++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp
@@ -67,6 +67,7 @@ public:
nh.get_parameter_or("odom_topic", odom_topic, default_topic);
odom_sub_ =
nh.create_subscription<nav_msgs::msg::Odometry>(odom_topic,
+ rclcpp::SystemDefaultsQoS(),
std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1));
nh.get_parameter_or("min_x_velocity_threshold", min_x_velocity_threshold_, 0.0001);