fix: prioritize FRI worker and expose command telemetry

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -17,6 +17,7 @@ struct IIWAStateSnapshot
   std::array<double, 7> measured_tau{};  // measured torque [Nm]
   std::array<double, 7> external_tau{};  // estimated external torque [Nm]
   std::array<double, 7> ipo_pos{};       // interpolator position [rad]
+  std::array<double, 7> command_pos{};   // position sent in the current FRI reply [rad]
   double sample_time{0.0};
   KUKA::FRI::ESessionState session{KUKA::FRI::IDLE};
   KUKA::FRI::EConnectionQuality quality{KUKA::FRI::POOR};

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -61,6 +61,7 @@ private:
   std::string robot_ip_;
   int fri_port_{30200};
   int fri_cycle_ms_{5};
+  int rt_prio_{80};
   bool simulate_{false};
   // Объекты FRI SDK
   std::unique_ptr<FRIClient> fri_client_;
@@ -86,6 +87,8 @@ private:
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_vel_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_eff_;
   std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ext_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_ipo_;
+  std::array<hardware_interface::StateInterface::SharedPtr, N_JOINTS> h_fri_cmd_;
 
   std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
 

src/iiwa_controller/src/FRIClient.cpp

@@ -29,6 +29,10 @@ void FRIClient::captureData(bool commanding)
   std::copy_n(state.getMeasuredJointPosition(), N_JOINTS, current_.measured_pos.begin());
   std::copy_n(state.getMeasuredTorque(), N_JOINTS, current_.measured_tau.begin());
   std::copy_n(state.getExternalTorque(), N_JOINTS, current_.external_tau.begin());
+  if (!commanding) {
+    // No application command exists outside a commanding FRI state.
+    current_.command_pos = current_.measured_pos;
+  }
   current_.sample_time = state.getSampleTime();
   current_.session = state.getSessionState();
   current_.quality = state.getConnectionQuality();
@@ -117,6 +121,7 @@ void FRIClient::waitForCommand()
   validateCommanding();
   LBRClient::waitForCommand();  // SDK 1.16 mirrors IPO, not measured position.
   sent_pos_ = target_pos_ = current_.ipo_pos;
+  current_.command_pos = sent_pos_;
   initialized_ = true;
   last_command_at_ = current_.received_at;
   publishState();
@@ -154,6 +159,7 @@ void FRIClient::command()
     sent_pos_[i] += std::clamp(step, -max_velocity_[i] * dt, max_velocity_[i] * dt);
   }
   robotCommand().setJointPosition(sent_pos_.data());
+  current_.command_pos = sent_pos_;
   publishState();
 }
 

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -15,6 +15,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
   iiwa_controller::IIWAHardwareInterface,
@@ -87,6 +88,11 @@ CallbackReturn IIWAHardwareInterface::on_init(
     if (used != cycle.size() || fri_cycle_ms_ < 1 || fri_cycle_ms_ > 100) {
       throw std::invalid_argument("Invalid fri_cycle_ms (expected 1..100)");
     }
+    const auto rt_prio = getParam(info, "rt_prio", "80");
+    rt_prio_ = std::stoi(rt_prio, &used);
+    if (used != rt_prio.size() || rt_prio_ < 1 || rt_prio_ > 99) {
+      throw std::invalid_argument("Invalid rt_prio (expected 1..99)");
+    }
     const auto simulation = getParam(info, "simulate", "false");
     if (simulation != "true" && simulation != "false") {
       throw std::invalid_argument("simulate must be true or false");
@@ -151,15 +157,17 @@ std::vector<hardware_interface::InterfaceDescription>
 IIWAHardwareInterface::export_unlisted_state_interface_descriptions()
 {
   std::vector<hardware_interface::InterfaceDescription> descs;
-  descs.reserve(N_JOINTS);
+  descs.reserve(3 * N_JOINTS);
 
   for (size_t i = 0; i < N_JOINTS; ++i) {
+    for (const auto * name : {"external_torque", "ipo_position", "fri_command_position"}) {
       hardware_interface::InterfaceInfo if_info;
-    if_info.name          = "external_torque";
+      if_info.name = name;
       if_info.data_type = "double";
       if_info.initial_value = "0.0";
       descs.emplace_back(info_.joints[i].name, if_info);
     }
+  }
 
   return descs;
 }
@@ -195,6 +203,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       h_vel_[i] = get_state_interface_handle(name + "/velocity");
       h_eff_[i] = get_state_interface_handle(name + "/effort");
       h_ext_[i] = get_state_interface_handle(name + "/external_torque");
+      h_ipo_[i] = get_state_interface_handle(name + "/ipo_position");
+      h_fri_cmd_[i] = get_state_interface_handle(name + "/fri_command_position");
       h_cmd_pos_[i] = get_command_interface_handle(name + "/position");
     }
     if (!simulate_) {
@@ -233,6 +243,8 @@ CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State
       set_state(h_vel_[i], 0.0, true);
       set_state(h_eff_[i], simulate_ ? 0.0 : last_snapshot_.measured_tau[i], true);
       set_state(h_ext_[i], simulate_ ? 0.0 : last_snapshot_.external_tau[i], true);
+      set_state(h_ipo_[i], position, true);
+      set_state(h_fri_cmd_[i], position, true);
       set_command(h_cmd_pos_[i], position, true);
     }
   } catch (const std::exception & e) {
@@ -291,6 +303,13 @@ CallbackReturn IIWAHardwareInterface::on_error(const rclcpp_lifecycle::State & s
 void IIWAHardwareInterface::friThreadFunc()
 {
   try {
+    if (!realtime_tools::configure_sched_fifo(rt_prio_)) {
+      RCLCPP_WARN(rclcpp::get_logger(LOG),
+        "FRI worker could not enable FIFO priority %d; configure realtime permissions for this user",
+        rt_prio_);
+    } else {
+      RCLCPP_INFO(rclcpp::get_logger(LOG), "FRI worker uses FIFO priority %d", rt_prio_);
+    }
     bool synchronized = false;
     const auto startup_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(15);
     while (fri_running_.load()) {
@@ -379,6 +398,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
       set_state(h_vel_[i], 0.0, false);
       set_state(h_eff_[i], 0.0, false);
       set_state(h_ext_[i], 0.0, false);
+      set_state(h_ipo_[i], pos, false);
+      set_state(h_fri_cmd_[i], pos, false);
     }
     return hardware_interface::return_type::OK;
   }
@@ -407,6 +428,8 @@ hardware_interface::return_type IIWAHardwareInterface::read(
     set_state(h_vel_[i], velocity_[i],         false);
     set_state(h_eff_[i], snap.measured_tau[i], false);
     set_state(h_ext_[i], snap.external_tau[i], false);
+    set_state(h_ipo_[i], snap.ipo_valid ? snap.ipo_pos[i] : snap.measured_pos[i], false);
+    set_state(h_fri_cmd_[i], snap.command_pos[i], false);
   }
 
   return hardware_interface::return_type::OK;

src/iiwa_controller/test/test_hardware.cpp

@@ -45,6 +45,14 @@ TEST(Hardware, RejectsMalformedPortWithoutThrowing)
   EXPECT_EQ(hardware.on_init(params), Return::ERROR);
 }
 
+TEST(Hardware, RejectsInvalidRealtimePriority)
+{
+  Hardware hardware;
+  auto params = parameters();
+  params.hardware_info.hardware_parameters["rt_prio"] = "0";
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
+
 TEST(Hardware, RejectsWrongJointOrderAndMissingInterfaces)
 {
   auto params = parameters();

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -145,6 +145,7 @@
                 <param name="robot_ip">$(arg robot_ip)</param>
                 <param name="fri_port">$(arg fri_port)</param>
                 <param name="fri_cycle_ms">$(arg fri_cycle_ms)</param>
+                <param name="rt_prio">80</param>
                 <param name="simulate">false</param>
             </hardware>