Add auto declaration of parameters. (#224)

diff_drive_controller/src/diff_drive_controller.cpp

@@ -59,55 +59,48 @@ controller_interface::return_type DiffDriveController::init(const std::string &
 
   try
   {
-    auto node = get_node();
     // with the lifecycle node being initialized, we can declare parameters
-    node->declare_parameter<std::vector<std::string>>(
-      "left_wheel_names", std::vector<std::string>());
-    node->declare_parameter<std::vector<std::string>>(
-      "right_wheel_names", std::vector<std::string>());
-
-    node->declare_parameter<double>("wheel_separation", wheel_params_.separation);
-    node->declare_parameter<int>("wheels_per_side", wheel_params_.wheels_per_side);
-    node->declare_parameter<double>("wheel_radius", wheel_params_.radius);
-    node->declare_parameter<double>(
-      "wheel_separation_multiplier", wheel_params_.separation_multiplier);
-    node->declare_parameter<double>(
-      "left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier);
-    node->declare_parameter<double>(
-      "right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier);
-
-    node->declare_parameter<std::string>("odom_frame_id", odom_params_.odom_frame_id);
-    node->declare_parameter<std::string>("base_frame_id", odom_params_.base_frame_id);
-    node->declare_parameter<std::vector<double>>("pose_covariance_diagonal", std::vector<double>());
-    node->declare_parameter<std::vector<double>>(
-      "twist_covariance_diagonal", std::vector<double>());
-    node->declare_parameter<bool>("open_loop", odom_params_.open_loop);
-    node->declare_parameter<bool>("enable_odom_tf", odom_params_.enable_odom_tf);
-
-    node->declare_parameter<double>("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0);
-    node->declare_parameter<bool>("publish_limited_velocity", publish_limited_velocity_);
-    node->declare_parameter<int>("velocity_rolling_window_size", 10);
-    node->declare_parameter<bool>("use_stamped_vel", use_stamped_vel_);
-
-    node->declare_parameter<bool>("linear.x.has_velocity_limits", false);
-    node->declare_parameter<bool>("linear.x.has_acceleration_limits", false);
-    node->declare_parameter<bool>("linear.x.has_jerk_limits", false);
-    node->declare_parameter<double>("linear.x.max_velocity", NAN);
-    node->declare_parameter<double>("linear.x.min_velocity", NAN);
-    node->declare_parameter<double>("linear.x.max_acceleration", NAN);
-    node->declare_parameter<double>("linear.x.min_acceleration", NAN);
-    node->declare_parameter<double>("linear.x.max_jerk", NAN);
-    node->declare_parameter<double>("linear.x.min_jerk", NAN);
-
-    node->declare_parameter<bool>("angular.z.has_velocity_limits", false);
-    node->declare_parameter<bool>("angular.z.has_acceleration_limits", false);
-    node->declare_parameter<bool>("angular.z.has_jerk_limits", false);
-    node->declare_parameter<double>("angular.z.max_velocity", NAN);
-    node->declare_parameter<double>("angular.z.min_velocity", NAN);
-    node->declare_parameter<double>("angular.z.max_acceleration", NAN);
-    node->declare_parameter<double>("angular.z.min_acceleration", NAN);
-    node->declare_parameter<double>("angular.z.max_jerk", NAN);
-    node->declare_parameter<double>("angular.z.min_jerk", NAN);
+    auto_declare<std::vector<std::string>>("left_wheel_names", std::vector<std::string>());
+    auto_declare<std::vector<std::string>>("right_wheel_names", std::vector<std::string>());
+
+    auto_declare<double>("wheel_separation", wheel_params_.separation);
+    auto_declare<int>("wheels_per_side", wheel_params_.wheels_per_side);
+    auto_declare<double>("wheel_radius", wheel_params_.radius);
+    auto_declare<double>("wheel_separation_multiplier", wheel_params_.separation_multiplier);
+    auto_declare<double>("left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier);
+    auto_declare<double>("right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier);
+
+    auto_declare<std::string>("odom_frame_id", odom_params_.odom_frame_id);
+    auto_declare<std::string>("base_frame_id", odom_params_.base_frame_id);
+    auto_declare<std::vector<double>>("pose_covariance_diagonal", std::vector<double>());
+    auto_declare<std::vector<double>>("twist_covariance_diagonal", std::vector<double>());
+    auto_declare<bool>("open_loop", odom_params_.open_loop);
+    auto_declare<bool>("enable_odom_tf", odom_params_.enable_odom_tf);
+
+    auto_declare<double>("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0);
+    auto_declare<bool>("publish_limited_velocity", publish_limited_velocity_);
+    auto_declare<int>("velocity_rolling_window_size", 10);
+    auto_declare<bool>("use_stamped_vel", use_stamped_vel_);
+
+    auto_declare<bool>("linear.x.has_velocity_limits", false);
+    auto_declare<bool>("linear.x.has_acceleration_limits", false);
+    auto_declare<bool>("linear.x.has_jerk_limits", false);
+    auto_declare<double>("linear.x.max_velocity", NAN);
+    auto_declare<double>("linear.x.min_velocity", NAN);
+    auto_declare<double>("linear.x.max_acceleration", NAN);
+    auto_declare<double>("linear.x.min_acceleration", NAN);
+    auto_declare<double>("linear.x.max_jerk", NAN);
+    auto_declare<double>("linear.x.min_jerk", NAN);
+
+    auto_declare<bool>("angular.z.has_velocity_limits", false);
+    auto_declare<bool>("angular.z.has_acceleration_limits", false);
+    auto_declare<bool>("angular.z.has_jerk_limits", false);
+    auto_declare<double>("angular.z.max_velocity", NAN);
+    auto_declare<double>("angular.z.min_velocity", NAN);
+    auto_declare<double>("angular.z.max_acceleration", NAN);
+    auto_declare<double>("angular.z.min_acceleration", NAN);
+    auto_declare<double>("angular.z.max_jerk", NAN);
+    auto_declare<double>("angular.z.min_jerk", NAN);
   }
   catch (const std::exception & e)
   {

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -39,14 +39,14 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::init(
 
   try
   {
-    node_->declare_parameter<std::string>("sensor_name", "");
-    node_->declare_parameter<std::string>("interface_names.force.x", "");
-    node_->declare_parameter<std::string>("interface_names.force.y", "");
-    node_->declare_parameter<std::string>("interface_names.force.z", "");
-    node_->declare_parameter<std::string>("interface_names.torque.x", "");
-    node_->declare_parameter<std::string>("interface_names.torque.y", "");
-    node_->declare_parameter<std::string>("interface_names.torque.z", "");
-    node_->declare_parameter<std::string>("frame_id", "");
+    auto_declare<std::string>("sensor_name", "");
+    auto_declare<std::string>("interface_names.force.x", "");
+    auto_declare<std::string>("interface_names.force.y", "");
+    auto_declare<std::string>("interface_names.force.z", "");
+    auto_declare<std::string>("interface_names.torque.x", "");
+    auto_declare<std::string>("interface_names.torque.y", "");
+    auto_declare<std::string>("interface_names.torque.z", "");
+    auto_declare<std::string>("frame_id", "");
   }
   catch (const std::exception & e)
   {

forward_command_controller/src/forward_command_controller.cpp

@@ -46,10 +46,9 @@ controller_interface::return_type ForwardCommandController::init(
 
   try
   {
-    auto node = get_node();
-    node->declare_parameter<std::vector<std::string>>("joints", std::vector<std::string>());
+    auto_declare<std::vector<std::string>>("joints", std::vector<std::string>());
 
-    node->declare_parameter<std::string>("interface_name", "");
+    auto_declare<std::string>("interface_name", "");
   }
   catch (const std::exception & e)
   {

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -46,12 +46,12 @@ controller_interface::return_type GripperActionController<HardwareInterface>::in
   }
 
   // with the lifecycle node being initialized, we can declare parameters
-  node_->declare_parameter<double>("action_monitor_rate", 20.0);
-  node_->declare_parameter<std::string>("joint", joint_name_);
-  node_->declare_parameter<double>("goal_tolerance", 0.01);
-  node_->declare_parameter<double>("max_effort", 0.0);
-  node_->declare_parameter<double>("stall_velocity_threshold", 0.001);
-  node_->declare_parameter<double>("stall_timeout", 1.0);
+  auto_declare<double>("action_monitor_rate", 20.0);
+  auto_declare<std::string>("joint", joint_name_);
+  auto_declare<double>("goal_tolerance", 0.01);
+  auto_declare<double>("max_effort", 0.0);
+  auto_declare<double>("stall_velocity_threshold", 0.001);
+  auto_declare<double>("stall_timeout", 1.0);
 
   return controller_interface::return_type::OK;
 }

gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

@@ -114,6 +114,21 @@ template <>
 class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
 {
 public:
+  template <typename ParameterT>
+  auto auto_declare(
+    const rclcpp::Node::SharedPtr & node, const std::string & name,
+    const ParameterT & default_value)
+  {
+    if (!node->has_parameter(name))
+    {
+      return node->declare_parameter<ParameterT>(name, default_value);
+    }
+    else
+    {
+      return node->get_parameter(name).get_value<ParameterT>();
+    }
+  }
+
   bool init(
     std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_handle,
@@ -122,10 +137,10 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     joint_handle_ = joint_handle;
     // Init PID gains from ROS parameter server
     const std::string prefix = "gains." + joint_handle_->get().get_name();
-    const auto k_p = node->declare_parameter<double>(prefix + ".p", 0.0);
-    const auto k_i = node->declare_parameter<double>(prefix + ".i", 0.0);
-    const auto k_d = node->declare_parameter<double>(prefix + ".d", 0.0);
-    const auto i_clamp = node->declare_parameter<double>(prefix + ".i_clamp", 0.0);
+    const auto k_p = auto_declare<double>(node, prefix + ".p", 0.0);
+    const auto k_i = auto_declare<double>(node, prefix + ".i", 0.0);
+    const auto k_d = auto_declare<double>(node, prefix + ".d", 0.0);
+    const auto i_clamp = auto_declare<double>(node, prefix + ".i_clamp", 0.0);
     // Initialize PID
     pid_ = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
     return true;

imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

@@ -33,8 +33,8 @@ controller_interface::return_type IMUSensorBroadcaster::init(const std::string &
 
   try
   {
-    node_->declare_parameter<std::string>("sensor_name", "");
-    node_->declare_parameter<std::string>("frame_id", "");
+    auto_declare<std::string>("sensor_name", "");
+    auto_declare<std::string>("frame_id", "");
   }
   catch (const std::exception & e)
   {

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -56,7 +56,7 @@ controller_interface::return_type JointStateBroadcaster::init(const std::string
 
   try
   {
-    get_node()->declare_parameter<bool>("use_local_topics", false);
+    auto_declare<bool>("use_local_topics", false);
   }
   catch (const std::exception & e)
   {

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -60,16 +60,15 @@ controller_interface::return_type JointTrajectoryController::init(
   }
 
   // with the lifecycle node being initialized, we can declare parameters
-  node_->declare_parameter<std::vector<std::string>>("joints", joint_names_);
-  node_->declare_parameter<std::vector<std::string>>(
-    "command_interfaces", command_interface_types_);
-  node_->declare_parameter<std::vector<std::string>>("state_interfaces", state_interface_types_);
-  node_->declare_parameter<double>("state_publish_rate", 50.0);
-  node_->declare_parameter<double>("action_monitor_rate", 20.0);
-  node_->declare_parameter<bool>("allow_partial_joints_goal", allow_partial_joints_goal_);
-  node_->declare_parameter<bool>("open_loop_control", open_loop_control_);
-  node_->declare_parameter<double>("constraints.stopped_velocity_tolerance", 0.01);
-  node_->declare_parameter<double>("constraints.goal_time", 0.0);
+  auto_declare<std::vector<std::string>>("joints", joint_names_);
+  auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+  auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+  auto_declare<double>("state_publish_rate", 50.0);
+  auto_declare<double>("action_monitor_rate", 20.0);
+  auto_declare<bool>("allow_partial_joints_goal", allow_partial_joints_goal_);
+  auto_declare<bool>("open_loop_control", open_loop_control_);
+  auto_declare<double>("constraints.stopped_velocity_tolerance", 0.01);
+  auto_declare<double>("constraints.goal_time", 0.0);
 
   return controller_interface::return_type::OK;
 }