Skip to content

Backport 378 to jazzy #384

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
mhubii merged 5 commits into from
Mar 27, 2026
Merged

Backport 378 to jazzy #384

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6eeee22
updating changelog
mhubii Mar 4, 2026
7d4b791
sort CHANGELOG
mhubii Mar 10, 2026
352e09f
updated changelog
mhubii Mar 26, 2026
154bb09
initial interface handle refactor
mhubii Mar 26, 2026
0c2e827
finalize interface handles
mhubii Mar 27, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 17 additions & 7 deletions CHANGELOG.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,28 +3,38 @@ Changelog for package LBR FRI ROS 2 Stack
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Jazzy v2.x (TBD)
--------------------------
* ``lbr_demos``: Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* ``lbr_fri_ros2``:
* ``lbr_bringup``:

* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* Provide default for ``StateInterfaceParameters`` to fix test.
* ``lbr_ros2_control``: Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* Adds a dedicated ``namespace`` argument to mock / hardware launch files.
* Fix the MoveIt servo node name.
* ``lbr_demos``:

* ``lbr_bringup``: Adds a dedicated ``namespace`` argument to mock / hardware launch files.
* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses.
* ``lbr_description``:

* Sets the ``k_velocity`` to fix the safety limits for effort client command modes.
* Removes the controller-related parts from the ``*_description.xacro`` files, now named ``*.urdf.xacro``.
* Fix tests.
* Depend on ``ament_cmake_pytest`` for tests only.
* ``lbr_ros2_control``: Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ``<command_interface>`` tags.
* ``lbr_fri_ros2``:

* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* Provide default for ``StateInterfaceParameters`` to fix test.
* ``lbr_ros2_control``:

* Migrates to the new ``ros2_control`` interface handles: https://github.com/ros-controls/ros2_control/pull/2831.
* Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ``<command_interface>`` tags.
* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
* Related pull requests:

* MoveIt fix: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/379
* Namespace argument: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/344, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/353
* Effort limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/357
* URDF de-coupling: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/359
* Ament target dependencies: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/364
* Test fixes: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/367, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/373
* Interface migration: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/378

Jazzy v2.4.3 (2025-12-09)
--------------------------
Expand Down
208 changes: 164 additions & 44 deletions lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_

#include <algorithm>
#include <array>
#include <cstring>
#include <memory>
#include <stdexcept>
Expand Down Expand Up @@ -55,61 +56,177 @@ class SystemInterface : public hardware_interface::SystemInterface {
bool open_loop{true};
};

struct CommandKeys {
lbr_fri_ros2::jnt_name_array_t joint_position, torque;
lbr_fri_ros2::cart_name_array_t wrench;
struct CommandInterfaceHandles {
std::array<hardware_interface::CommandInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
joint_position, torque;
std::array<hardware_interface::CommandInterface::SharedPtr, lbr_fri_ros2::CARTESIAN_DOF> wrench;

void populate_keys(const hardware_interface::HardwareInfo &info) {
void populate(const hardware_interface::SystemInterface &si) {
const auto &info = si.get_hardware_info();
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
auto joint_name = info.joints[i].name;
joint_position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION;
torque[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT;
const auto &joint_name = info.joints[i].name;
joint_position[i] =
si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION);
torque[i] =
si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT);
}
wrench[0] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X);
wrench[1] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y);
wrench[2] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z);
wrench[3] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X);
wrench[4] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y);
wrench[5] =
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z);
}

void nan_interfaces() const {
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
std::ignore = joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
}
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
std::ignore = wrench[i]->set_value(std::numeric_limits<double>::quiet_NaN());
}
}

void pull(lbr_fri_idl::msg::LBRCommand &lbr_command) const {
// populate command message
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
lbr_command.joint_position[i] = joint_position[i]->get_optional().value();
lbr_command.torque[i] = torque[i]->get_optional().value();
}
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
lbr_command.wrench[i] = wrench[i]->get_optional().value();
}
wrench[0] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X;
wrench[1] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y;
wrench[2] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z;
wrench[3] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X;
wrench[4] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y;
wrench[5] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z;
}
};

struct StateKeys {
struct StateInterfaceHandles {
#if FRI_CLIENT_VERSION_MAJOR == 1
lbr_fri_ros2::jnt_name_array_t commanded_joint_position;
std::array<hardware_interface::StateInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
commanded_joint_position;
#endif
lbr_fri_ros2::jnt_name_array_t commanded_torque, ipo_joint_position, position, external_torque,
effort, velocity;
std::string sample_time, session_state, connection_quality, safety_state, operation_mode,
drive_state, client_command_mode, overlay_type, control_mode, time_stamp_sec,
time_stamp_nano_sec, tracking_performance;
std::array<hardware_interface::StateInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
commanded_torque, ipo_joint_position, position, external_torque, effort, velocity;
hardware_interface::StateInterface::SharedPtr sample_time, session_state, connection_quality,
safety_state, operation_mode, drive_state, client_command_mode, overlay_type, control_mode,
time_stamp_sec, time_stamp_nano_sec, tracking_performance;

void populate_keys(const hardware_interface::HardwareInfo &info) {
void populate(const hardware_interface::SystemInterface &si) {
const auto &info = si.get_hardware_info();
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
auto joint_name = info.joints[i].name;
const auto &joint_name = info.joints[i].name;
#if FRI_CLIENT_VERSION_MAJOR == 1
commanded_joint_position[i] = joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION;
commanded_joint_position[i] =
si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION);
#endif
commanded_torque[i] = joint_name + "/" + HW_IF_COMMANDED_TORQUE;
ipo_joint_position[i] = joint_name + "/" + HW_IF_IPO_JOINT_POSITION;
position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION;
external_torque[i] = joint_name + "/" + HW_IF_EXTERNAL_TORQUE;
effort[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT;
velocity[i] = joint_name + "/" + hardware_interface::HW_IF_VELOCITY;
commanded_torque[i] =
si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_TORQUE);
ipo_joint_position[i] =
si.get_state_interface_handle(joint_name + "/" + HW_IF_IPO_JOINT_POSITION);
position[i] =
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION);
external_torque[i] =
si.get_state_interface_handle(joint_name + "/" + HW_IF_EXTERNAL_TORQUE);
effort[i] =
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT);
velocity[i] =
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_VELOCITY);
}

sample_time = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAMPLE_TIME;
session_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SESSION_STATE;
connection_quality = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONNECTION_QUALITY;
safety_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAFETY_STATE;
operation_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OPERATION_MODE;
drive_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_DRIVE_STATE;
client_command_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CLIENT_COMMAND_MODE;
overlay_type = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OVERLAY_TYPE;
control_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONTROL_MODE;
time_stamp_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_SEC;
time_stamp_nano_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_NANO_SEC;
tracking_performance = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TRACKING_PERFORMANCE;
sample_time = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_SAMPLE_TIME);
session_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_SESSION_STATE);
connection_quality = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_CONNECTION_QUALITY);
safety_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_SAFETY_STATE);
operation_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_OPERATION_MODE);
drive_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_DRIVE_STATE);
client_command_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
"/" + HW_IF_CLIENT_COMMAND_MODE);
overlay_type = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_OVERLAY_TYPE);
control_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_CONTROL_MODE);
time_stamp_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
HW_IF_TIME_STAMP_SEC);
time_stamp_nano_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
"/" + HW_IF_TIME_STAMP_NANO_SEC);
tracking_performance = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
"/" + HW_IF_TRACKING_PERFORMANCE);
}

void nan_interfaces() const {
// joint state interfaces
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
std::ignore = position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
#if FRI_CLIENT_VERSION_MAJOR == 1
std::ignore =
commanded_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
#endif
std::ignore = effort[i]->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = commanded_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = external_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = ipo_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = velocity[i]->set_value(std::numeric_limits<double>::quiet_NaN());
}
std::ignore = sample_time->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = tracking_performance->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = session_state->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = connection_quality->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = safety_state->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = operation_mode->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = drive_state->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = client_command_mode->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = overlay_type->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = control_mode->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = time_stamp_sec->set_value(std::numeric_limits<double>::quiet_NaN());
std::ignore = time_stamp_nano_sec->set_value(std::numeric_limits<double>::quiet_NaN());
}

void
push(const lbr_fri_idl::msg::LBRState &lbr_state,
const lbr_fri_idl::msg::LBRState::_measured_joint_position_type &velocity_estimate) const {
// set the joint state interfaces
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
#if FRI_CLIENT_VERSION_MAJOR == 1
std::ignore = commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]);
#endif
std::ignore = commanded_torque[i]->set_value(lbr_state.commanded_torque[i]);
std::ignore = ipo_joint_position[i]->set_value(lbr_state.ipo_joint_position[i]);
std::ignore = position[i]->set_value(lbr_state.measured_joint_position[i]);
std::ignore = external_torque[i]->set_value(lbr_state.external_torque[i]);
std::ignore = effort[i]->set_value(lbr_state.measured_torque[i]);
std::ignore = velocity[i]->set_value(velocity_estimate[i]);
}

// state interfaces without cast
std::ignore = sample_time->set_value(lbr_state.sample_time);
std::ignore = tracking_performance->set_value(lbr_state.tracking_performance);

// state interfaces with cast
std::ignore = session_state->set_value(static_cast<double>(lbr_state.session_state));
std::ignore =
connection_quality->set_value(static_cast<double>(lbr_state.connection_quality));
std::ignore = safety_state->set_value(static_cast<double>(lbr_state.safety_state));
std::ignore = operation_mode->set_value(static_cast<double>(lbr_state.operation_mode));
std::ignore = drive_state->set_value(static_cast<double>(lbr_state.drive_state));
std::ignore =
client_command_mode->set_value(static_cast<double>(lbr_state.client_command_mode));
std::ignore = overlay_type->set_value(static_cast<double>(lbr_state.overlay_type));
std::ignore = control_mode->set_value(static_cast<double>(lbr_state.control_mode));
std::ignore = time_stamp_sec->set_value(static_cast<double>(lbr_state.time_stamp_sec));
std::ignore =
time_stamp_nano_sec->set_value(static_cast<double>(lbr_state.time_stamp_nano_sec));
}
};

Expand Down Expand Up @@ -173,6 +290,9 @@ class SystemInterface : public hardware_interface::SystemInterface {
std::shared_ptr<lbr_fri_ros2::AsyncClient> async_client_ptr_;
std::unique_ptr<lbr_fri_ros2::App> app_ptr_;

// session state tracking
KUKA::FRI::ESessionState previous_session_state_;

// velocity computation
lbr_fri_idl::msg::LBRState::_measured_joint_position_type last_measured_joint_position_,
velocity_;
Expand All @@ -189,9 +309,9 @@ class SystemInterface : public hardware_interface::SystemInterface {
lbr_fri_idl::msg::LBRCommand lbr_command_;
lbr_fri_idl::msg::LBRState lbr_state_;

// keys for command / state interfaces
CommandKeys command_keys_;
StateKeys state_keys_;
// interface handles for commands / states
CommandInterfaceHandles command_if_handles_;
StateInterfaceHandles state_if_handles_;
};
} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_
Loading
Loading