[Joint State Broadcaster] Add option to support only specific interfa…

…ces on specific joints (ros-controls#216)

* Joint state broadcaster support for specific joint_names and interface_names.
* Extend documentation.
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>

joint_state_broadcaster/doc/userdoc.rst

@@ -13,13 +13,32 @@ Broadcasters are not real controllers, and therefore take no commands.
 Hardware interface type
 -----------------------
 
-All available *joint state interfaces* are used by this broadcaster.
+By default, all available *joint state interfaces* are used, unless configured otherwise.
+In the latter case, resulting interfaces is defined by a "matrix" of interfaces defined by the cross-product of the ``joints`` and ``interfaces`` parameters.
+If some requested interfaces are missing, the controller will print a warning about that, but work for other interfaces.
+If none of the requested interface are not defined, the controller returns error on activation.
 
 Parameters
 ----------
 
 ``use_local_topics``
+
   Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``.
 
+
+``joints``
+
+  Optional parameter (string array) to support broadcasting of only specific joints and interfaces.
+  It has to be used in combination with the ``interfaces`` parameter.
+  Joint state broadcaster asks for access to all defined interfaces on all defined joints.
+
+
+``interfaces``
+
+  Optional parameter (string array) to support broadcasting of only specific joints and interfaces.
+  It has to be used in combination with the ``joints`` parameter.
+
+
 ``extra_joints``
+
   Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0.

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -31,6 +31,25 @@ namespace joint_state_broadcaster
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
+/**
+ * \brief Joint State Broadcaster for all or some state in a ros2_control system.
+ *
+ * JointStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
+ * There is a possibility to publish all available states (typical use), or only specific ones.
+ * The latter is, for example, used when hardware provides multiple measurement sources for some
+ * of its states, e.g., position.
+ * If "joints" or "interfaces" parameter is empty, all available states are published.
+ *
+ * \param use_local_topics Flag to publish topics in local namespace.
+ * \param joints Names of the joints to publish.
+ * \param interfaces Names of interfaces to publish.
+ *
+ * Publishes to:
+ * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement
+ * (position, velocity, effort).
+ * - \b dynamic_joint_states (control_msgs::msg::DynamicJointState): Joint states regardless of
+ * its interface type.
+ */
 class JointStateBroadcaster : public controller_interface::ControllerInterface
 {
 public:
@@ -63,9 +82,13 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   bool init_joint_data();
   void init_joint_state_msg();
   void init_dynamic_joint_state_msg();
+  bool use_all_available_interfaces() const;
 
 protected:
+  // Optional parameters
   bool use_local_topics_;
+  std::vector<std::string> joints_;
+  std::vector<std::string> interfaces_;
 
   //  For the JointState message,
   //  we store the name of joints with compatible interfaces

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -51,6 +51,8 @@ CallbackReturn JointStateBroadcaster::on_init()
   try
   {
     auto_declare<bool>("use_local_topics", false);
+    auto_declare<std::vector<std::string>>("joints", std::vector<std::string>({}));
+    auto_declare<std::vector<std::string>>("interfaces", std::vector<std::string>({}));
   }
   catch (const std::exception & e)
   {
@@ -71,14 +73,47 @@ JointStateBroadcaster::command_interface_configuration() const
 controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interface_configuration()
   const
 {
-  return controller_interface::InterfaceConfiguration{
-    controller_interface::interface_configuration_type::ALL};
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+
+  if (use_all_available_interfaces())
+  {
+    state_interfaces_config.type = controller_interface::interface_configuration_type::ALL;
+  }
+  else
+  {
+    state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+    for (const auto & joint : joints_)
+    {
+      for (const auto & interface : interfaces_)
+      {
+        state_interfaces_config.names.push_back(joint + "/" + interface);
+      }
+    }
+  }
+
+  return state_interfaces_config;
 }
 
 CallbackReturn JointStateBroadcaster::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
   use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool();
+  joints_ = get_node()->get_parameter("joints").as_string_array();
+  interfaces_ = get_node()->get_parameter("interfaces").as_string_array();
+
+  if (use_all_available_interfaces())
+  {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "'joints' or 'interfaces' parameter is empty. "
+      "All available state interfaces will be published");
+  }
+  else
+  {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "Publishing state interfaces defined in 'joints' and 'interfaces' parameters.");
+  }
 
   try
   {
@@ -105,12 +140,24 @@ CallbackReturn JointStateBroadcaster::on_activate(
 {
   if (!init_joint_data())
   {
+    RCLCPP_ERROR(
+      node_->get_logger(), "None of requested interfaces exist. Controller will not run.");
     return CallbackReturn::ERROR;
   }
 
   init_joint_state_msg();
   init_dynamic_joint_state_msg();
 
+  if (
+    !use_all_available_interfaces() &&
+    state_interfaces_.size() != (joints_.size() * interfaces_.size()))
+  {
+    RCLCPP_WARN(
+      node_->get_logger(),
+      "Not all requested interfaces exists. "
+      "Check ControllerManager output for more detailed information.");
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -139,6 +186,11 @@ bool has_any_key(
 
 bool JointStateBroadcaster::init_joint_data()
 {
+  if (state_interfaces_.empty())
+  {
+    return false;
+  }
+
   // loop in reverse order, this maintains the order of values at retrieval time
   for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
   {
@@ -213,6 +265,11 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg()
   }
 }
 
+bool JointStateBroadcaster::use_all_available_interfaces() const
+{
+  return joints_.empty() || interfaces_.empty();
+}
+
 double get_value(
   const std::unordered_map<std::string, std::unordered_map<std::string, double>> & map,
   const std::string & name, const std::string & interface_name)