Add optional control_msgs/JointCommand publisher support

joint_state_topic_hardware_interface/CHANGELOG.rst

@@ -4,6 +4,7 @@ Changelog for package joint_state_topic_hardware_interface
 
 1.1.0 (2026-05-13)
 ------------------
+* Add optional ``command_type`` parameter to publish ``control_msgs/JointCommand`` instead of ``sensor_msgs/JointState`` for joint commands (`#131 <https://github.com/ros-controls/topic_based_hardware_interfaces/pull/131>`_)
 * Publish JointState commands for velocity-only and effort-only interfaces (`#118 <https://github.com/ros-controls/topic_based_hardware_interfaces/issues/118>`_)
 * Bump C++ version to C++20 (`#119 <https://github.com/ros-controls/topic_based_hardware_interfaces/issues/119>`_)
 * Bump version of pre-commit hooks (`#98 <https://github.com/ros-controls/topic_based_hardware_interfaces/issues/98>`_)

joint_state_topic_hardware_interface/CMakeLists.txt

@@ -12,6 +12,7 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   angles
+  control_msgs
   rclcpp
   hardware_interface
   sensor_msgs
@@ -35,6 +36,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
                       angles::angles
                       rclcpp::rclcpp
                       hardware_interface::hardware_interface
+                      ${control_msgs_TARGETS}
                       ${sensor_msgs_TARGETS})
 target_include_directories(${PROJECT_NAME}
   PUBLIC  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -66,14 +68,14 @@ if(BUILD_TESTING)
                         angles::angles
                         rclcpp::rclcpp
                         hardware_interface::hardware_interface
+                        ${control_msgs_TARGETS}
                         ${sensor_msgs_TARGETS}
                         ros2_control_test_assets::ros2_control_test_assets)
 
   # Integration tests
   find_package(ament_cmake_ros REQUIRED)
   find_package(launch_testing_ament_cmake REQUIRED)
   find_package(rclcpp_action REQUIRED)
-  find_package(control_msgs REQUIRED)
   add_executable(jtc_client_position test/rrr/jtc_client_position.cpp)
   target_link_libraries(jtc_client_position PUBLIC
     ${control_msgs_TARGETS}

joint_state_topic_hardware_interface/README.md

@@ -12,6 +12,7 @@ The `joint_state_topic_hardware_interface` has a few `ros2_control` urdf tags to
 * joint_states_topic: (default: "/robot_joint_states"). Example: `<param name="joint_states_topic">/my_topic_joint_states</param>`.
 * trigger_joint_command_threshold: (default: 1e-5). Used to avoid spamming the joint command topic when the difference between the current joint state and the joint command is smaller than this value, set to -1 to always send the joint command. Example: `<param name="trigger_joint_command_threshold">0.001</param>`.
 * sum_wrapped_joint_states: (default: "false"). Used to track the total rotation for joint states the values reported on the `joint_commands_topic` wrap from 2*pi to -2*pi when rotating in the positive direction. (Isaac Sim only reports joint states from 2*pi to -2*pi) Example: `<param name="sum_wrapped_joint_states">true</param>`.
+* command_type: (default: "joint_state"). Selects the command message type published on `joint_commands_topic`. Set to `"joint_state"` to publish `sensor_msgs/JointState` (default), or `"joint_command"` to publish `control_msgs/JointCommand` messages grouped by interface type. Example: `<param name="command_type">joint_command</param>`.
 
 ### Per-joint Parameters
 
@@ -75,3 +76,27 @@ If your robot description support mock_components you simply add an if-else stat
             </joint>
         </ros2_control>
 ```
+
+## Using `control_msgs/JointCommand` messages
+
+To publish commands as `control_msgs/JointCommand` messages instead of `sensor_msgs/JointState`, set the `command_type` parameter to `"joint_command"`. This publishes one `JointCommand` message per interface type (position, velocity, effort) on each `write()` call.
+
+```xml
+        <ros2_control name="name" type="system">
+            <hardware>
+              <plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
+              <param name="joint_commands_topic">/topic_based_joint_commands</param>
+              <param name="joint_states_topic">/topic_based_joint_states</param>
+              <param name="command_type">joint_command</param>
+            </hardware>
+            <joint name="joint_1">
+                <command_interface name="position"/>
+                <command_interface name="velocity"/>
+                <state_interface name="position">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            ...
+        </ros2_control>
+```

joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp

@@ -27,6 +27,7 @@
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/subscription.hpp>
 
+#include <control_msgs/msg/joint_command.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 
 namespace joint_state_topic_hardware_interface
@@ -44,9 +45,11 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
 
 private:
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_states_subscriber_;
-  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_commands_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_state_publisher_;
+  rclcpp::Publisher<control_msgs::msg::JointCommand>::SharedPtr topic_based_joint_command_publisher_;
   sensor_msgs::msg::JointState latest_joint_state_;
   bool sum_wrapped_joint_states_{ false };
+  bool use_joint_command_msg_{ false };
 
   // If the difference between the current joint state and joint command is less than this value,
   // the joint command will not be published.

joint_state_topic_hardware_interface/package.xml

@@ -27,10 +27,10 @@
   <depend>rclcpp</depend>
   <depend>hardware_interface</depend>
   <depend>sensor_msgs</depend>
+  <depend>control_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_ros</test_depend>
-  <test_depend>control_msgs</test_depend>
   <test_depend>controller_manager</test_depend>
   <test_depend>forward_command_controller</test_depend>
   <test_depend>joint_state_broadcaster</test_depend>

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

@@ -17,6 +17,7 @@
 #include <cmath>
 #include <iterator>
 #include <limits>
+#include <map>
 #include <set>
 #include <string>
 #include <vector>
@@ -70,12 +71,23 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
     trigger_joint_command_threshold_ = std::stod(it->second);
   }
 
-  topic_based_joint_commands_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
-      get_hardware_parameter("joint_commands_topic", "/robot_joint_commands"), rclcpp::QoS(1));
   topic_based_joint_states_subscriber_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
       get_hardware_parameter("joint_states_topic", "/robot_joint_states"), rclcpp::SensorDataQoS(),
       [this](const sensor_msgs::msg::JointState::SharedPtr joint_state) { latest_joint_state_ = *joint_state; });
 
+  const auto commands_topic = get_hardware_parameter("joint_commands_topic", "/robot_joint_commands");
+  if (get_hardware_parameter("command_type", "joint_state") == "joint_command")
+  {
+    use_joint_command_msg_ = true;
+    topic_based_joint_command_publisher_ =
+        get_node()->create_publisher<control_msgs::msg::JointCommand>(commands_topic, rclcpp::QoS(1));
+  }
+  else
+  {
+    topic_based_joint_state_publisher_ =
+        get_node()->create_publisher<sensor_msgs::msg::JointState>(commands_topic, rclcpp::QoS(1));
+  }
+
   // if the values on the `joint_states_topic` are wrapped between -2*pi and 2*pi (like they are in Isaac Sim)
   // sum the total joint rotation returned on the `joint_state_values_` interface
   if (get_hardware_parameter("sum_wrapped_joint_states", "false") == "true")
@@ -199,37 +211,70 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time&
     return hardware_interface::return_type::OK;
   }
 
-  sensor_msgs::msg::JointState joint_state;
-  for (std::size_t i = 0; i < joints.size(); ++i)
+  if (use_joint_command_msg_)
   {
-    joint_state.name.push_back(joints[i].name);
-    joint_state.header.stamp = get_node()->now();
-    // only send commands to the interfaces that are defined for this joint
-    for (const auto& interface : joints[i].command_interfaces)
+    std::map<std::string, control_msgs::msg::JointCommand> commands_by_interface;
+    for (std::size_t i = 0; i < joints.size(); ++i)
     {
-      if (interface.name == hardware_interface::HW_IF_POSITION)
-      {
-        joint_state.position.push_back(get_command(joints[i].name + "/" + interface.name));
-      }
-      else if (interface.name == hardware_interface::HW_IF_VELOCITY)
+      for (const auto& interface : joints[i].command_interfaces)
       {
-        joint_state.velocity.push_back(get_command(joints[i].name + "/" + interface.name));
-      }
-      else if (interface.name == hardware_interface::HW_IF_EFFORT)
-      {
-        joint_state.effort.push_back(get_command(joints[i].name + "/" + interface.name));
+        const bool supported_command_interface = interface.name == hardware_interface::HW_IF_POSITION ||
+                                                 interface.name == hardware_interface::HW_IF_VELOCITY ||
+                                                 interface.name == hardware_interface::HW_IF_EFFORT;
+        if (!supported_command_interface)
+        {
+          continue;
+        }
+        if (commands_by_interface.find(interface.name) == commands_by_interface.end())
+        {
+          commands_by_interface[interface.name].header.stamp = get_node()->now();
+          commands_by_interface[interface.name].interface_name = interface.name;
+        }
+        commands_by_interface[interface.name].joint_names.push_back(joints[i].name);
+        commands_by_interface[interface.name].values.push_back(get_command(joints[i].name + "/" + interface.name));
       }
-      else
+    }
+    if (rclcpp::ok())
+    {
+      for (auto& [interface_name, msg] : commands_by_interface)
       {
-        RCLCPP_WARN_ONCE(get_node()->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.",
-                         joints[i].name.c_str(), interface.name.c_str());
+        topic_based_joint_command_publisher_->publish(msg);
       }
     }
   }
-
-  if (rclcpp::ok())
+  else
   {
-    topic_based_joint_commands_publisher_->publish(joint_state);
+    sensor_msgs::msg::JointState joint_state;
+    for (std::size_t i = 0; i < joints.size(); ++i)
+    {
+      joint_state.name.push_back(joints[i].name);
+      joint_state.header.stamp = get_node()->now();
+      // only send commands to the interfaces that are defined for this joint
+      for (const auto& interface : joints[i].command_interfaces)
+      {
+        if (interface.name == hardware_interface::HW_IF_POSITION)
+        {
+          joint_state.position.push_back(get_command(joints[i].name + "/" + interface.name));
+        }
+        else if (interface.name == hardware_interface::HW_IF_VELOCITY)
+        {
+          joint_state.velocity.push_back(get_command(joints[i].name + "/" + interface.name));
+        }
+        else if (interface.name == hardware_interface::HW_IF_EFFORT)
+        {
+          joint_state.effort.push_back(get_command(joints[i].name + "/" + interface.name));
+        }
+        else
+        {
+          RCLCPP_WARN_ONCE(get_node()->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.",
+                           joints[i].name.c_str(), interface.name.c_str());
+        }
+      }
+    }
+    if (rclcpp::ok())
+    {
+      topic_based_joint_state_publisher_->publish(joint_state);
+    }
   }
 
   return hardware_interface::return_type::OK;