[JSB] Remove dynamic_joint_states and use INDIVIDUAL_BEST_EFFORT for interfaces

joint_state_broadcaster/CMakeLists.txt

@@ -7,7 +7,6 @@ export_windows_symbols()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   builtin_interfaces
-  control_msgs
   controller_interface
   generate_parameter_library
   pluginlib
@@ -44,7 +43,6 @@ target_link_libraries(joint_state_broadcaster PUBLIC
                       realtime_tools::realtime_tools
                       urdf::urdf
                       ${sensor_msgs_TARGETS}
-                      ${control_msgs_TARGETS}
                       ${builtin_interfaces_TARGETS})
 pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml)
 

joint_state_broadcaster/doc/userdoc.rst

@@ -5,7 +5,7 @@
 joint_state_broadcaster
 =======================
 
-The broadcaster reads all state interfaces and reports them on ``/joint_states`` and ``/dynamic_joint_states``.
+The broadcaster reads all state interfaces and reports them on ``/joint_states``.
 
 Commands
 --------
@@ -29,31 +29,11 @@ Published topics
   and ``effort`` — for joints that provide them. If a joint does not expose a given
   movement interface, that field is omitted/left empty for that joint in the message.
 
-* ``/dynamic_joint_states`` (``control_msgs/msg/DynamicJointState``):
-
-  Publishes **all available state interfaces** for each joint. This includes the
-  movement interfaces (position/velocity/effort) *and* any additional or custom
-  interfaces provided by the hardware (e.g., temperature, voltage, torque sensor
-  readings, calibration flags).
-
-  The message maps ``joint_names`` to per-joint interface name lists and values.
-  Example payload::
-
-    joint_names: [joint1, joint2]
-    interface_values:
-      - interface_names: [position, velocity, effort]
-        values: [1.5708, 0.0, 0.20]
-      - interface_names: [position, temperature]
-        values: [0.7854, 42.1]
-
-  Use this topic if you need *every* reported interface, not just movement.
-
 .. note::
 
-   If ``use_local_topics`` is set to ``true``, both topics are published in the
-   controller’s namespace (e.g., ``/my_state_broadcaster/joint_states`` and
-   ``/my_state_broadcaster/dynamic_joint_states``). If ``false`` (default),
-   they are published at the root (e.g., ``/joint_states``).
+   If ``use_local_topics`` is set to ``true``, the topic is published in the
+   controller’s namespace (e.g., ``/my_state_broadcaster/joint_states``). If ``false`` (default),
+   it is published at the root (e.g., ``/joint_states``).
 
 
 Parameters

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -20,7 +20,6 @@
 #include <unordered_map>
 #include <vector>
 
-#include "control_msgs/msg/dynamic_joint_state.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -58,8 +57,6 @@ namespace joint_state_broadcaster
  * 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
 {
@@ -88,9 +85,13 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   bool init_joint_data();
   void init_auxiliary_data();
   void init_joint_state_msg();
-  void init_dynamic_joint_state_msg();
+
+  [[deprecated(
+    "use_all_available_interfaces is deprecated. Use use_urdf_joint_interfaces method instead")]]
   bool use_all_available_interfaces() const;
 
+  bool use_urdf_joint_interfaces() const;
+
 protected:
   // Optional parameters
   std::shared_ptr<ParamListener> param_listener_;
@@ -107,14 +108,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
     realtime_joint_state_publisher_;
   sensor_msgs::msg::JointState joint_state_msg_;
 
-  //  For the DynamicJointState format, we use a map to buffer values in for easier lookup
-  //  This allows to preserve whatever order or names/interfaces were initialized.
   std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
-  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
-    dynamic_joint_state_publisher_;
-  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
-    realtime_dynamic_joint_state_publisher_;
-  control_msgs::msg::DynamicJointState dynamic_joint_state_msg_;
 
   urdf::Model model_;
   bool is_model_loaded_ = false;
@@ -134,8 +128,6 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   };
 
   std::vector<JointStateData> joint_states_data_;
-
-  std::vector<std::vector<const double *>> dynamic_joint_states_data_;
 };
 
 }  // namespace joint_state_broadcaster

joint_state_broadcaster/package.xml

@@ -24,7 +24,6 @@
 
   <depend>backward_ros</depend>
   <depend>builtin_interfaces</depend>
-  <depend>control_msgs</depend>
   <depend>controller_interface</depend>
   <depend>generate_parameter_library</depend>
   <depend>pluginlib</depend>

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -68,9 +68,22 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf
 {
   controller_interface::InterfaceConfiguration state_interfaces_config;
 
-  if (use_all_available_interfaces())
+  if (use_urdf_joint_interfaces())
   {
-    state_interfaces_config.type = controller_interface::interface_configuration_type::ALL;
+    state_interfaces_config.type =
+      controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT;
+    for (const auto & joint : model_.joints_)
+    {
+      if (
+        joint.second->type == urdf::Joint::CONTINUOUS ||
+        joint.second->type == urdf::Joint::REVOLUTE || joint.second->type == urdf::Joint::PRISMATIC)
+      {
+        for (const auto & interface : map_interface_to_joint_state_)
+        {
+          state_interfaces_config.names.push_back(joint.first + "/" + interface.first);
+        }
+      }
+    }
   }
   else
   {
@@ -92,12 +105,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
 {
   params_ = param_listener_->get_params();
 
-  if (use_all_available_interfaces())
+  if (use_urdf_joint_interfaces())
   {
     RCLCPP_INFO(
       get_node()->get_logger(),
-      "'joints' or 'interfaces' parameter is empty. "
-      "All available state interfaces will be published");
+      "'joints' or 'interfaces' parameter is empty. Will try to publish all available state "
+      "interfaces of the URDF joints.");
     params_.joints.clear();
     params_.interfaces.clear();
   }
@@ -143,16 +156,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
     realtime_joint_state_publisher_ =
       std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(
         joint_state_publisher_);
-
-    if (params_.publish_dynamic_joint_states)
-    {
-      dynamic_joint_state_publisher_ =
-        get_node()->create_publisher<control_msgs::msg::DynamicJointState>(
-          topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
-      realtime_dynamic_joint_state_publisher_ =
-        std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(
-          dynamic_joint_state_publisher_);
-    }
   }
   catch (const std::exception & e)
   {
@@ -166,10 +169,33 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
   is_model_loaded_ = !urdf.empty() && model_.initString(urdf);
   if (!is_model_loaded_)
   {
-    RCLCPP_ERROR(
-      get_node()->get_logger(),
-      "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'",
-      HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT);
+    if (use_urdf_joint_interfaces())
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "Robot description could not be loaded. Cannot determine URDF joints to publish. "
+        "Either provide a valid robot description, or explicitly set both 'joints' and "
+        "'interfaces' parameters.");
+      return CallbackReturn::ERROR;
+    }
+    else
+    {
+      if (params_.use_urdf_to_filter)
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(),
+          "'use_urdf_to_filter' parameter is set to true, but robot description could not be "
+          "parsed. Will publish the joints defined in 'joints' parameter without filtering with "
+          "URDF. Fix the robot description to filter joints with URDF.");
+      }
+      else
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(),
+          "Failed to parse robot description. Will publish the joints defined in 'joints' "
+          "parameter along with the defined interfaces in 'interface' parameter.");
+      }
+    }
   }
 
   // joint_names reserve space for all joints
@@ -205,11 +231,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate(
   init_auxiliary_data();
   init_joint_state_msg();
 
-  if (params_.publish_dynamic_joint_states)
-  {
-    init_dynamic_joint_state_msg();
-  }
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -278,18 +299,16 @@ bool JointStateBroadcaster::init_joint_data()
   std::reverse(joint_names_.begin(), joint_names_.end());
   if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty())
   {
-    std::vector<std::string> joint_names_filtered;
-    for (const auto & [joint_name, urdf_joint] : model_.joints_)
-    {
-      if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED)
-      {
-        if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end())
+    // Preserve the order from the first pass; only remove fixed joints
+    joint_names_.erase(
+      std::remove_if(
+        joint_names_.begin(), joint_names_.end(),
+        [this](const std::string & name)
         {
-          joint_names_filtered.push_back(joint_name);
-        }
-      }
-    }
-    joint_names_ = joint_names_filtered;
+          const auto urdf_joint = model_.getJoint(name);
+          return !urdf_joint || urdf_joint->type == urdf::Joint::FIXED;
+        }),
+      joint_names_.end());
   }
 
   // Add extra joints from parameters, each joint will be added to joint_names_ and
@@ -366,44 +385,12 @@ void JointStateBroadcaster::init_joint_state_msg()
   }
 }
 
-void JointStateBroadcaster::init_dynamic_joint_state_msg()
+bool JointStateBroadcaster::use_all_available_interfaces() const
 {
-  dynamic_joint_state_msg_.header.frame_id = frame_id_;
-  dynamic_joint_state_msg_.joint_names.clear();
-  dynamic_joint_state_msg_.interface_values.clear();
-  for (const auto & name_ifv : name_if_value_mapping_)
-  {
-    const auto & name = name_ifv.first;
-    const auto & interfaces_and_values = name_ifv.second;
-    dynamic_joint_state_msg_.joint_names.push_back(name);
-    control_msgs::msg::InterfaceValue if_value;
-    for (const auto & interface_and_value : interfaces_and_values)
-    {
-      if_value.interface_names.emplace_back(interface_and_value.first);
-      if_value.values.emplace_back(kUninitializedValue);
-    }
-    dynamic_joint_state_msg_.interface_values.emplace_back(if_value);
-  }
-
-  // save dynamic joint state data
-  dynamic_joint_states_data_.clear();
-  for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji)
-  {
-    dynamic_joint_states_data_.push_back(std::vector<const double *>());
-
-    const auto & name = dynamic_joint_state_msg_.joint_names[ji];
-
-    for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size();
-         ++ii)
-    {
-      const auto & interface_name =
-        dynamic_joint_state_msg_.interface_values[ji].interface_names[ii];
-      dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]);
-    }
-  }
+  return this->use_urdf_joint_interfaces();
 }
 
-bool JointStateBroadcaster::use_all_available_interfaces() const
+bool JointStateBroadcaster::use_urdf_joint_interfaces() const
 {
   return params_.joints.empty() || params_.interfaces.empty();
 }
@@ -429,7 +416,6 @@ controller_interface::return_type JointStateBroadcaster::update(
   {
     joint_state_msg_.header.stamp = time;
 
-    // update joint state message and dynamic joint state message
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
       joint_state_msg_.position[i] = joint_states_data_[i].position_;
@@ -439,21 +425,6 @@ controller_interface::return_type JointStateBroadcaster::update(
     realtime_joint_state_publisher_->try_publish(joint_state_msg_);
   }
 
-  if (realtime_dynamic_joint_state_publisher_)
-  {
-    dynamic_joint_state_msg_.header.stamp = time;
-    for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji)
-    {
-      for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size();
-           ++ii)
-      {
-        dynamic_joint_state_msg_.interface_values[ji].values[ii] =
-          *dynamic_joint_states_data_[ji][ii];
-      }
-    }
-    realtime_dynamic_joint_state_publisher_->try_publish(dynamic_joint_state_msg_);
-  }
-
   return controller_interface::return_type::OK;
 }
 

joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml

@@ -3,7 +3,7 @@ joint_state_broadcaster:
     type: bool,
     default_value: false,
     read_only: true,
-    description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``."
+    description: "Defining if ``joint_states`` message should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``."
   }
   joints: {
     type: string_array,
@@ -19,7 +19,7 @@ joint_state_broadcaster:
     type: string_array,
     default_value: [],
     read_only: true,
-    description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0."
+    description: "Names of extra joints to be added to ``joint_states`` with state set to 0."
   }
   interfaces: {
     type: string_array,
@@ -60,9 +60,3 @@ joint_state_broadcaster:
     read_only: true,
     description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints."
   }
-  publish_dynamic_joint_states: {
-    type: bool,
-    default_value: true,
-    read_only: true,
-    description: "Whether to publish dynamic joint states."
-  }