diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 7ca95d7f91..c844048b44 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -23,6 +23,7 @@ #include "nav2_util/robot_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" +#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { @@ -64,6 +65,10 @@ class GoalReachedCondition : public BT::ConditionNode void initialize() { node_ = config().blackboard->get("node"); + + nav2_util::declare_parameter_if_not_declared( + node_, "goal_reached_tol", + rclcpp::ParameterValue(0.25)); node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); tf_ = config().blackboard->get>("tf_buffer"); 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 3d0ae06774..91f08b1bb5 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 @@ -65,6 +65,9 @@ class OdomSubscriber nav2_util::LifecycleNode::SharedPtr nh, std::string default_topic = "odom") { + nav2_util::declare_parameter_if_not_declared( + nh, "odom_topic", rclcpp::ParameterValue(default_topic)); + std::string odom_topic; nh->get_parameter_or("odom_topic", odom_topic, default_topic); odom_sub_ =