added an initial controller draft

lbr_ros2_control/CMakeLists.txt

@@ -20,19 +20,21 @@ find_package(hardware_interface REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(realtime_tools REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(lbr_fri_ros2 REQUIRED)
 find_package(fri_vendor REQUIRED)
 find_package(FRIClient REQUIRED)
 
-# Define targets
+# LBR ROS 2 control library
 add_library(
   ${PROJECT_NAME}
   SHARED
+  src/lbr_state_broadcaster.cpp
   src/lbr_system_interface.cpp
 )
 
-# Add include directories for target
+# Add include directories
 target_include_directories(
   ${PROJECT_NAME}
   PUBLIC
@@ -49,6 +51,7 @@ ament_target_dependencies(
   controller_interface
   pluginlib
   rclcpp
+  realtime_tools
   lbr_fri_msgs
   lbr_fri_ros2
   fri_vendor
@@ -58,7 +61,7 @@ target_link_libraries(${PROJECT_NAME}
   FRIClient::FRIClient
 )
 
-pluginlib_export_plugin_description_file(hardware_interface lbr_system_interface.xml)
+pluginlib_export_plugin_description_file(hardware_interface lbr_ros2_control.xml)
 
 # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html
 ament_export_targets(
@@ -70,6 +73,7 @@ ament_export_dependencies(
   controller_interface
   pluginlib
   rclcpp
+  realtime_tools
   lbr_fri_msgs
   lbr_fri_ros2
   fri_vendor

lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp

@@ -0,0 +1,43 @@
+#ifndef LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_
+#define LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_
+
+#include <limits>
+#include <memory>
+#include <stdexcept>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_publisher.h"
+
+#include "lbr_fri_msgs/msg/lbr_state.hpp"
+
+namespace lbr_ros2_control {
+class LBRStateBroadcaster : public controller_interface::ControllerInterface {
+public:
+  LBRStateBroadcaster() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::return_type update(const rclcpp::Time &time,
+                                           const rclcpp::Duration &period) override;
+
+  controller_interface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+protected:
+  rclcpp::Publisher<lbr_fri_msgs::msg::LBRState>::SharedPtr state_publisher_ptr_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<lbr_fri_msgs::msg::LBRState>>
+      rt_state_publisher_ptr_;
+};
+} // end of namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_

lbr_ros2_control/include/lbr_ros2_control/lbr_state_interface_reference.hpp

@@ -0,0 +1,47 @@
+#ifndef LBR_ROS2_CONTROL__LBR_STATE_INTERFACE_REFERENCE_HPP_
+#define LBR_ROS2_CONTROL__LBR_STATE_INTERFACE_REFERENCE_HPP_
+
+#include <functional>
+#include <vector>
+
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include "lbr_ros2_control/lbr_system_interface_type_values.hpp"
+
+namespace lbr_ros2_control {
+struct LBRStateInterface {
+  using loaned_state_interface_t = std::reference_wrapper<hardware_interface::LoanedStateInterface>;
+
+  void reference(std::vector<hardware_interface::LoanedStateInterface> &state_interfaces) {
+    for (auto &state_interface : state_interfaces) {
+      if (state_interface.get_interface_name() == HW_IF_CLIENT_COMMAND_MODE) {
+        client_command_mode = std::ref(state_interface);
+      }
+
+      if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
+        commanded_joint_position.emplace_back(std::ref(state_interface));
+      }
+    }
+  };
+
+  loaned_state_interface_t client_command_mode;
+  std::vector<loaned_state_interface_t> commanded_joint_position;
+  std::vector<loaned_state_interface_t> commanded_torque;
+  loaned_state_interface_t connection_quality;
+  loaned_state_interface_t control_mode;
+  loaned_state_interface_t drive_state;
+  loaned_state_interface_t external_torque;
+  loaned_state_interface_t ipo_joint_position;
+  loaned_state_interface_t measured_joint_position;
+  loaned_state_interface_t measured_torque;
+  loaned_state_interface_t overlay_type;
+  loaned_state_interface_t safety_state;
+  loaned_state_interface_t sample_time;
+  loaned_state_interface_t session_state;
+  loaned_state_interface_t time_stamp_nano_sec;
+  loaned_state_interface_t time_stamp_sec;
+  loaned_state_interface_t tracking_performance;
+};
+} // end of namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__LBR_STATE_INTERFACE_REFERENCE_HPP_

lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface.hpp

@@ -23,6 +23,7 @@
 #include "lbr_fri_ros2/app.hpp"
 #include "lbr_fri_ros2/client.hpp"
 #include "lbr_fri_ros2/enum_maps.hpp"
+#include "lbr_ros2_control/lbr_state_interface_reference.hpp"
 #include "lbr_ros2_control/lbr_system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {

lbr_ros2_control/lbr_ros2_control.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<library path="lbr_ros2_control">
+    <description>
+        TODO
+    </description>
+
+    <!-- LBR system interface plugin -->
+    <class type="lbr_ros2_control::LBRSystemInterface"
+        base_class_type="hardware_interface::SystemInterface" />
+
+    <!-- LBR state broadcaster plugin -->
+    <class type="lbr_ros2_control::LBRStateBroadcaster"
+        base_class_type="controller_interface::ControllerInterface" />
+</library>

lbr_ros2_control/package.xml

@@ -15,6 +15,7 @@
   <depend>lbr_fri_ros2</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
+  <depend>realtime_tools</depend>
   <depend>ros2_control</depend>
 
   <exec_depend>lbr_description</exec_depend>

lbr_ros2_control/src/lbr_state_broadcaster.cpp

@@ -0,0 +1,85 @@
+#include "lbr_ros2_control/lbr_state_broadcaster.hpp"
+
+namespace lbr_ros2_control {
+controller_interface::InterfaceConfiguration
+LBRStateBroadcaster::command_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::InterfaceConfiguration
+LBRStateBroadcaster::state_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::ALL};
+}
+
+controller_interface::CallbackReturn LBRStateBroadcaster::on_init() {
+  try {
+    state_publisher_ptr_ = this->get_node()->create_publisher<lbr_fri_msgs::msg::LBRState>(
+        "~/state", rclcpp::SensorDataQoS());
+
+    rt_state_publisher_ptr_ =
+        std::make_shared<realtime_tools::RealtimePublisher<lbr_fri_msgs::msg::LBRState>>(
+            state_publisher_ptr_);
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR(this->get_node()->get_logger(),
+                 "Failed to initialize LBR state broadcaster with: %s.", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time & /*time*/,
+                                                              const rclcpp::Duration & /*period*/) {
+
+  // read state from command interfaces
+
+  // publish state from command interfaces
+
+  // rt_state_publisher_ptr_->msg_
+
+  // state
+
+  return controller_interface::return_type::OK;
+}
+
+controller_interface::CallbackReturn
+LBRStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) {
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+LBRStateBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+  rt_state_publisher_ptr_->msg_.client_command_mode = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.commanded_joint_position.fill(
+      std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.commanded_torque.fill(std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.connection_quality = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.control_mode = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.drive_state = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.external_torque.fill(std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.ipo_joint_position.fill(std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.measured_joint_position.fill(
+      std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.measured_torque.fill(std::numeric_limits<double>::quiet_NaN());
+  rt_state_publisher_ptr_->msg_.overlay_type = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.safety_state = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.sample_time = std::numeric_limits<double>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.session_state = std::numeric_limits<int8_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = std::numeric_limits<uint32_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits<uint32_t>::quiet_NaN();
+  rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits<double>::quiet_NaN();
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+LBRStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+} // end of namespace lbr_ros2_control
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRStateBroadcaster,
+                       controller_interface::ControllerInterface)