adding copy all params primitive for BT navigator (to ingest into rcl…

…cpp) (#3804)

* adding copy all params primitive

* fix linting

* lint

* I swear to god, this better be the last linting issue

* allowing params to be declared from yaml

* Update bt_navigator.cpp

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -126,16 +126,16 @@ bool BtActionServer<ActionT>::on_configure()
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
-  // Declare parameters for client node to share with BT nodes
-  // Declare if not declared in case being used an external application
+  // Declare parameters for common client node applications to share with BT nodes
+  // Declare if not declared in case being used an external application, then copying
+  // all of the main node's parameters to the client for BT nodes to obtain
   nav2_util::declare_parameter_if_not_declared(
     node, "global_frame", rclcpp::ParameterValue(std::string("map")));
   nav2_util::declare_parameter_if_not_declared(
     node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
-  client_node_->declare_parameter(
-    "robot_base_frame", node->get_parameter("robot_base_frame").as_string());
-  client_node_->declare_parameter(
-    "global_frame", node->get_parameter("global_frame").as_string());
+  nav2_util::declare_parameter_if_not_declared(
+    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
+  nav2_util::copy_all_parameters(node, client_node_);
 
   // set the timeout in seconds for the action server to discard goal handles if not finished
   double action_server_result_timeout;

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -44,7 +44,7 @@ class BtNavigator : public nav2_util::LifecycleNode
    * @brief A constructor for nav2_bt_navigator::BtNavigator class
    * @param options Additional options to control creation of the node.
    */
-  explicit BtNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions());
   /**
    * @brief A destructor for nav2_bt_navigator::BtNavigator class
    */

nav2_bt_navigator/src/bt_navigator.cpp

@@ -29,8 +29,9 @@ using nav2_util::declare_parameter_if_not_declared;
 namespace nav2_bt_navigator
 {
 
-BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
-: nav2_util::LifecycleNode("bt_navigator", "", options),
+BtNavigator::BtNavigator(rclcpp::NodeOptions options)
+: nav2_util::LifecycleNode("bt_navigator", "",
+    options.automatically_declare_parameters_from_overrides(true)),
   class_loader_("nav2_core", "nav2_core::NavigatorBase")
 {
   RCLCPP_INFO(get_logger(), "Creating");
@@ -89,11 +90,16 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
     "nav2_is_battery_charging_condition_bt_node"
   };
 
-  declare_parameter("plugin_lib_names", plugin_libs);
-  declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
-  declare_parameter("global_frame", std::string("map"));
-  declare_parameter("robot_base_frame", std::string("base_link"));
-  declare_parameter("odom_topic", std::string("odom"));
+  declare_parameter_if_not_declared(
+    this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs));
+  declare_parameter_if_not_declared(
+    this, "transform_tolerance", rclcpp::ParameterValue(0.1));
+  declare_parameter_if_not_declared(
+    this, "global_frame", rclcpp::ParameterValue(std::string("map")));
+  declare_parameter_if_not_declared(
+    this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
+  declare_parameter_if_not_declared(
+    this, "odom_topic", rclcpp::ParameterValue(std::string("odom")));
 }
 
 BtNavigator::~BtNavigator()

nav2_util/CMakeLists.txt

@@ -32,6 +32,7 @@ set(dependencies
     bondcpp
     bond
     action_msgs
+    rcl_interfaces
 )
 
 nav2_package()

nav2_util/include/nav2_util/node_utils.hpp

@@ -15,8 +15,10 @@
 #ifndef NAV2_UTIL__NODE_UTILS_HPP_
 #define NAV2_UTIL__NODE_UTILS_HPP_
 
+#include <vector>
 #include <string>
 #include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
 
 namespace nav2_util
 {
@@ -150,6 +152,25 @@ std::string get_plugin_type_param(
   return plugin_type;
 }
 
+/**
+ * @brief A method to copy all parameters from one node (parent) to another (child).
+ * May throw parameter exceptions in error conditions
+ * @param parent Node to copy parameters from
+ * @param child Node to copy parameters to
+ */
+template<typename NodeT1, typename NodeT2>
+void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child)
+{
+  using Parameters = std::vector<rclcpp::Parameter>;
+  std::vector<std::string> param_names = parent->list_parameters({}, 0).names;
+  Parameters params = parent->get_parameters(param_names);
+  for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) {
+    if (!child->has_parameter(iter->get_name())) {
+      child->declare_parameter(iter->get_name(), iter->get_parameter_value());
+    }
+  }
+}
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__NODE_UTILS_HPP_

nav2_util/package.xml

@@ -28,6 +28,7 @@
   <depend>launch</depend>
   <depend>launch_testing_ament_cmake</depend>
   <depend>action_msgs</depend>
+  <depend>rcl_interfaces</depend>
 
   <exec_depend>libboost-program-options</exec_depend>
 

nav2_util/test/test_node_utils.cpp

@@ -94,3 +94,35 @@ TEST(GetPluginTypeParam, GetPluginTypeParam)
   ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar");
   ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*");
 }
+
+TEST(TestParamCopying, TestParamCopying)
+{
+  auto node1 = std::make_shared<rclcpp::Node>("test_node1");
+  auto node2 = std::make_shared<rclcpp::Node>("test_node2");
+
+  // Tests for (1) multiple types, (2) recursion, (3) overriding values
+  node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1"))));
+  node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123));
+  node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar"))));
+  node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve"))));
+  node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2"))));
+
+  // Show Node2 is empty of Node1's parameters, but contains its own
+  EXPECT_FALSE(node2->has_parameter("Foo1"));
+  EXPECT_FALSE(node2->has_parameter("Foo2"));
+  EXPECT_FALSE(node2->has_parameter("Foo.bar"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+
+  nav2_util::copy_all_parameters(node1, node2);
+
+  // Test new parameters exist, of expected value, and original param is not overridden
+  EXPECT_TRUE(node2->has_parameter("Foo1"));
+  EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1"));
+  EXPECT_TRUE(node2->has_parameter("Foo2"));
+  EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123);
+  EXPECT_TRUE(node2->has_parameter("Foo.bar"));
+  EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+}