From fd66c8c7ecad104b4ec1f3861abe1244339e46ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 16 Sep 2026 14:54:44 +1000 Subject: [PATCH] fix: prioritize FRI worker and expose command telemetry --- .../include/iiwa_controller/FRIClient.h | 1 + .../iiwa_controller/IIWAHardwareInterface.hpp | 3 ++ src/iiwa_controller/src/FRIClient.cpp | 6 ++++ .../src/IIWAHardwareInterface.cpp | 35 +++++++++++++++---- src/iiwa_controller/test/test_hardware.cpp | 8 +++++ src/iiwa_description/urdf/iiwa7.urdf.xacro | 1 + 6 files changed, 48 insertions(+), 6 deletions(-) diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h index 7f5287d..4c8dc79 100644 --- a/src/iiwa_controller/include/iiwa_controller/FRIClient.h +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -17,6 +17,7 @@ struct IIWAStateSnapshot std::array measured_tau{}; // measured torque [Nm] std::array external_tau{}; // estimated external torque [Nm] std::array ipo_pos{}; // interpolator position [rad] + std::array 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}; diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp index 3b71043..611ed43 100644 --- a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp +++ b/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 fri_client_; @@ -86,6 +87,8 @@ private: std::array h_vel_; std::array h_eff_; std::array h_ext_; + std::array h_ipo_; + std::array h_fri_cmd_; std::array h_cmd_pos_; diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp index e31d7c7..9d70d25 100644 --- a/src/iiwa_controller/src/FRIClient.cpp +++ b/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(); } diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 7f4579a..aad6f28 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/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,14 +157,16 @@ std::vector IIWAHardwareInterface::export_unlisted_state_interface_descriptions() { std::vector descs; - descs.reserve(N_JOINTS); + descs.reserve(3 * N_JOINTS); for (size_t i = 0; i < N_JOINTS; ++i) { - hardware_interface::InterfaceInfo if_info; - if_info.name = "external_torque"; - if_info.data_type = "double"; - if_info.initial_value = "0.0"; - descs.emplace_back(info_.joints[i].name, if_info); + for (const auto * name : {"external_torque", "ipo_position", "fri_command_position"}) { + hardware_interface::InterfaceInfo if_info; + 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; diff --git a/src/iiwa_controller/test/test_hardware.cpp b/src/iiwa_controller/test/test_hardware.cpp index 75e6f11..a63a9fc 100644 --- a/src/iiwa_controller/test/test_hardware.cpp +++ b/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(); diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 9ca3b82..9294b61 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -145,6 +145,7 @@ $(arg robot_ip) $(arg fri_port) $(arg fri_cycle_ms) + 80 false