Multi-robot support

controllers/fri_configuration_controller/CMakeLists.txt

@@ -6,22 +6,32 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(kuka_driver_interfaces REQUIRED)
 find_package(kuka_drivers_core REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
+generate_parameter_library(
+  fri_configuration_controller_parameters
+  src/fri_configuration_controller_parameters.yaml
+)
 
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
   src/fri_configuration_controller.cpp)
 
+add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
+target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)
+
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
 )
 
-ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces kuka_drivers_core
-)
+ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces kuka_drivers_core)
+target_link_libraries(${PROJECT_NAME} fri_configuration_controller_parameters)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -31,33 +41,31 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNC
 
 pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
 
-install(TARGETS ${PROJECT_NAME}
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(
+  TARGETS ${PROJECT_NAME} fri_configuration_controller_parameters
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-install(DIRECTORY include/
-  DESTINATION include
+ament_export_include_directories(
+  "include/${PROJECT_NAME}"
 )
-
-install(FILES controller_plugins.xml
-  DESTINATION share/${PROJECT_NAME}
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
 )
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
 
 if(BUILD_TESTING)
 
 endif()
 
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-
-ament_export_include_directories(
-  include
-)
-
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
-
 ament_package()

controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
 
+#include "fri_configuration_controller/fri_configuration_controller_parameters.hpp"
 #include "fri_configuration_controller/visibility_control.h"
 
 namespace kuka_controllers
@@ -53,9 +54,18 @@ class FRIConfigurationController : public controller_interface::ControllerInterf
   FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override;
 
 private:
+  static std::string ComposeInterfaceName(
+    const std::string & robot_prefix, const std::string & interface_name);
+
+  using Params = fri_configuration_controller::Params;
+  using ParamListener = fri_configuration_controller::ParamListener;
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
   rclcpp::Subscription<kuka_driver_interfaces::msg::FriConfiguration>::SharedPtr fri_config_sub_;
   double receive_multiplier_ = 1;
   double send_period_ms_ = 10;
+  std::vector<std::string> robot_prefixes_;
 };
 }  // namespace kuka_controllers
 #endif  // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_

controllers/fri_configuration_controller/package.xml

@@ -12,12 +12,13 @@
 
   <license>Apache-2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>controller_interface</depend>
   <depend>pluginlib</depend>
   <depend>kuka_driver_interfaces</depend>
   <depend>kuka_drivers_core</depend>
+  <depend>generate_parameter_library</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

controllers/fri_configuration_controller/src/fri_configuration_controller.cpp

@@ -12,14 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kuka_drivers_core/hardware_interface_types.hpp"
+#include "pluginlib/class_list_macros.hpp"
 
+#include "kuka_drivers_core/hardware_interface_types.hpp"
 #include "fri_configuration_controller/fri_configuration_controller.hpp"
 
 namespace kuka_controllers
 {
 controller_interface::CallbackReturn FRIConfigurationController::on_init()
 {
+  param_listener_ = std::make_shared<ParamListener>(get_node());
+  params_ = param_listener_->get_params();
+
   auto callback = [this](const kuka_driver_interfaces::msg::FriConfiguration::SharedPtr msg)
   {
     receive_multiplier_ = msg->receive_multiplier;
@@ -30,15 +34,34 @@ controller_interface::CallbackReturn FRIConfigurationController::on_init()
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
+std::string FRIConfigurationController::ComposeInterfaceName(
+  const std::string & robot_prefix, const std::string & interface_name)
+{
+  if (robot_prefix.empty())
+  {
+    return std::string(hardware_interface::CONFIG_PREFIX) + "/" + interface_name;
+  }
+  else
+  {
+    return robot_prefix + "_" + std::string(hardware_interface::CONFIG_PREFIX) + "/" +
+           interface_name;
+  }
+}
+
 controller_interface::InterfaceConfiguration
 FRIConfigurationController::command_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration config;
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-  config.names.emplace_back(
-    std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::RECEIVE_MULTIPLIER);
-  config.names.emplace_back(
-    std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::SEND_PERIOD);
+
+  for (const auto & robot_prefix : params_.robot_prefixes)
+  {
+    config.names.emplace_back(
+      ComposeInterfaceName(robot_prefix, hardware_interface::RECEIVE_MULTIPLIER));
+    config.names.emplace_back(
+      ComposeInterfaceName(robot_prefix, hardware_interface::SEND_PERIOD));
+  }
+
   return config;
 }
 
@@ -52,6 +75,11 @@ FRIConfigurationController::state_interface_configuration() const
 controller_interface::CallbackReturn FRIConfigurationController::on_configure(
   const rclcpp_lifecycle::State &)
 {
+  robot_prefixes_ = params_.robot_prefixes;
+  RCLCPP_INFO(
+    get_node()->get_logger(),
+    "FRI configuration controller configured with %zu robot instance(s)",
+    robot_prefixes_.size());
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -71,16 +99,29 @@ controller_interface::return_type FRIConfigurationController::update(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
   // TODO(Svastits): disable changes if HWIF is active
-  if (!command_interfaces_[0].set_value(receive_multiplier_))
-  {
-    return controller_interface::return_type::ERROR;
-  }
-  if (!command_interfaces_[1].set_value(send_period_ms_))
+  // Set the same FRI configuration values for all robots
+  bool all_set = true;
+  for (size_t idx = 0; idx < robot_prefixes_.size(); ++idx)
   {
-    return controller_interface::return_type::ERROR;
+    size_t cmd_idx = idx * 2;  // Each robot has 2 command interfaces
+    if (!command_interfaces_[cmd_idx].set_value(receive_multiplier_))
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "Failed to set receive multiplier for robot '%s'",
+        robot_prefixes_[idx].c_str());
+      all_set = false;
+    }
+    if (!command_interfaces_[cmd_idx + 1].set_value(send_period_ms_))
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(), "Failed to set send period for robot '%s'",
+        robot_prefixes_[idx].c_str());
+      all_set = false;
+    }
   }
 
-  return controller_interface::return_type::OK;
+  return all_set ? controller_interface::return_type::OK : controller_interface::return_type::ERROR;
 }
 
 }  // namespace kuka_controllers

controllers/fri_configuration_controller/src/fri_configuration_controller_parameters.yaml

@@ -0,0 +1,6 @@
+fri_configuration_controller:
+  robot_prefixes:
+    type: string_array
+    # Keep one entry by default; empty string means unprefixed single-robot behavior.
+    default_value: [""]
+    description: List of robot prefixes used for binding FRI configuration command interfaces.

controllers/fri_state_broadcaster/CMakeLists.txt

@@ -6,22 +6,32 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(kuka_driver_interfaces REQUIRED)
 find_package(kuka_drivers_core REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
+generate_parameter_library(
+  fri_state_broadcaster_parameters
+  src/fri_state_broadcaster_parameters.yaml
+)
 
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
   src/fri_state_broadcaster.cpp)
 
+add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
+target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)
+
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
 )
 
-ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces kuka_drivers_core
-)
+ament_target_dependencies(${PROJECT_NAME} controller_interface kuka_driver_interfaces kuka_drivers_core)
+target_link_libraries(${PROJECT_NAME} fri_state_broadcaster_parameters)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -31,33 +41,31 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNC
 
 pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
 
-install(TARGETS ${PROJECT_NAME}
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(
+  TARGETS ${PROJECT_NAME} fri_state_broadcaster_parameters
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-install(DIRECTORY include/
-  DESTINATION include
+ament_export_include_directories(
+  "include/${PROJECT_NAME}"
 )
-
-install(FILES controller_plugins.xml
-  DESTINATION share/${PROJECT_NAME}
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
 )
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
 
 if(BUILD_TESTING)
 
 endif()
 
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-
-ament_export_include_directories(
-  include
-)
-
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
-
 ament_package()

controllers/fri_state_broadcaster/include/fri_state_broadcaster/fri_state_broadcaster.hpp

@@ -20,11 +20,13 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "kuka_driver_interfaces/msg/fri_state.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "kuka_driver_interfaces/msg/fri_state_array.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
 
+#include "fri_state_broadcaster/fri_state_broadcaster_parameters.hpp"
 #include "fri_state_broadcaster/visibility_control.h"
 
 namespace kuka_controllers
@@ -53,9 +55,26 @@ class FRIStateBroadcaster : public controller_interface::ControllerInterface
   FRI_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override;
 
 private:
+  static std::string ComposeInterfaceName(
+    const std::string & robot_prefix, const std::string & interface_name);
+
+  static void AssignStateFromInterfaces(
+    kuka_driver_interfaces::msg::FRIState & state,
+    const std::vector<hardware_interface::LoanedStateInterface> & state_interfaces,
+    size_t start_idx);
+
+  using Params = fri_state_broadcaster::Params;
+  using ParamListener = fri_state_broadcaster::ParamListener;
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
   int counter_ = 0;
-  rclcpp::Publisher<kuka_driver_interfaces::msg::FRIState>::SharedPtr robot_state_publisher_;
-  kuka_driver_interfaces::msg::FRIState state_msg_;
+  rclcpp::Publisher<kuka_driver_interfaces::msg::FRIStateArray>::SharedPtr state_publisher_;
+  std::vector<std::string> robot_prefixes_;
+  std::vector<kuka_driver_interfaces::msg::FRIState> current_states_;
+  kuka_driver_interfaces::msg::FRIStateArray state_msg_;
+
+  static constexpr size_t STATE_INTERFACE_COUNT = 9;
 };
 }  // namespace kuka_controllers
 #endif  // FRI_STATE_BROADCASTER__FRI_STATE_BROADCASTER_HPP_

controllers/fri_state_broadcaster/package.xml

@@ -12,12 +12,13 @@
 
   <license>Apache-2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <depend>controller_interface</depend>
   <depend>pluginlib</depend>
   <depend>kuka_driver_interfaces</depend>
   <depend>kuka_drivers_core</depend>
+  <depend>generate_parameter_library</depend>
 
   <export>
     <build_type>ament_cmake</build_type>