Skip to content

Commit 959a92b

Browse files
mhubiimhubii
authored
Merge pull request #378 from lbr-stack/feature/lbr-fri-ros2-stack-350/interface_handles
New ros2_control interface handles
2 parents 667d462 + c5181f7 commit 959a92b

3 files changed

Lines changed: 209 additions & 145 deletions

File tree

‎CHANGELOG.rst‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,28 +3,38 @@ Changelog for package LBR FRI ROS 2 Stack
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44
Jazzy v2.x (TBD)
55
--------------------------
6-
* ``lbr_demos``: Replace ``ament_target_dependencies`` with ``target_link_libraries``.
7-
* ``lbr_fri_ros2``:
6+
* ``lbr_bringup``:
87

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

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

1616
* Sets the ``k_velocity`` to fix the safety limits for effort client command modes.
1717
* Removes the controller-related parts from the ``*_description.xacro`` files, now named ``*.urdf.xacro``.
1818
* Fix tests.
1919
* Depend on ``ament_cmake_pytest`` for tests only.
20-
* ``lbr_ros2_control``: Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ``<command_interface>`` tags.
20+
* ``lbr_fri_ros2``:
21+
22+
* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
23+
* Provide default for ``StateInterfaceParameters`` to fix test.
24+
* ``lbr_ros2_control``:
25+
26+
* Migrates to the new ``ros2_control`` interface handles: https://github.com/ros-controls/ros2_control/pull/2831.
27+
* Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ``<command_interface>`` tags.
28+
* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
2129
* Related pull requests:
2230

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

2939
Jazzy v2.4.3 (2025-12-09)
3040
--------------------------

‎lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp‎

Lines changed: 164 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_
33

44
#include <algorithm>
5+
#include <array>
56
#include <cstring>
67
#include <memory>
78
#include <stdexcept>
@@ -55,61 +56,177 @@ class SystemInterface : public hardware_interface::SystemInterface {
5556
bool open_loop{true};
5657
};
5758

58-
struct CommandKeys {
59-
lbr_fri_ros2::jnt_name_array_t joint_position, torque;
60-
lbr_fri_ros2::cart_name_array_t wrench;
59+
struct CommandInterfaceHandles {
60+
std::array<hardware_interface::CommandInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
61+
joint_position, torque;
62+
std::array<hardware_interface::CommandInterface::SharedPtr, lbr_fri_ros2::CARTESIAN_DOF> wrench;
6163

62-
void populate_keys(const hardware_interface::HardwareInfo &info) {
64+
void populate(const hardware_interface::SystemInterface &si) {
65+
const auto &info = si.get_hardware_info();
6366
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
64-
auto joint_name = info.joints[i].name;
65-
joint_position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION;
66-
torque[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT;
67+
const auto &joint_name = info.joints[i].name;
68+
joint_position[i] =
69+
si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION);
70+
torque[i] =
71+
si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT);
72+
}
73+
wrench[0] =
74+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X);
75+
wrench[1] =
76+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y);
77+
wrench[2] =
78+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z);
79+
wrench[3] =
80+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X);
81+
wrench[4] =
82+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y);
83+
wrench[5] =
84+
si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z);
85+
}
86+
87+
void nan_interfaces() const {
88+
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
89+
std::ignore = joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
90+
std::ignore = torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
91+
}
92+
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
93+
std::ignore = wrench[i]->set_value(std::numeric_limits<double>::quiet_NaN());
94+
}
95+
}
96+
97+
void pull(lbr_fri_idl::msg::LBRCommand &lbr_command) const {
98+
// populate command message
99+
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
100+
lbr_command.joint_position[i] = joint_position[i]->get_optional().value();
101+
lbr_command.torque[i] = torque[i]->get_optional().value();
102+
}
103+
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
104+
lbr_command.wrench[i] = wrench[i]->get_optional().value();
67105
}
68-
wrench[0] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X;
69-
wrench[1] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y;
70-
wrench[2] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z;
71-
wrench[3] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X;
72-
wrench[4] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y;
73-
wrench[5] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z;
74106
}
75107
};
76108

77-
struct StateKeys {
109+
struct StateInterfaceHandles {
78110
#if FRI_CLIENT_VERSION_MAJOR == 1
79-
lbr_fri_ros2::jnt_name_array_t commanded_joint_position;
111+
std::array<hardware_interface::StateInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
112+
commanded_joint_position;
80113
#endif
81-
lbr_fri_ros2::jnt_name_array_t commanded_torque, ipo_joint_position, position, external_torque,
82-
effort, velocity;
83-
std::string sample_time, session_state, connection_quality, safety_state, operation_mode,
84-
drive_state, client_command_mode, overlay_type, control_mode, time_stamp_sec,
85-
time_stamp_nano_sec, tracking_performance;
114+
std::array<hardware_interface::StateInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
115+
commanded_torque, ipo_joint_position, position, external_torque, effort, velocity;
116+
hardware_interface::StateInterface::SharedPtr sample_time, session_state, connection_quality,
117+
safety_state, operation_mode, drive_state, client_command_mode, overlay_type, control_mode,
118+
time_stamp_sec, time_stamp_nano_sec, tracking_performance;
86119

87-
void populate_keys(const hardware_interface::HardwareInfo &info) {
120+
void populate(const hardware_interface::SystemInterface &si) {
121+
const auto &info = si.get_hardware_info();
88122
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
89-
auto joint_name = info.joints[i].name;
123+
const auto &joint_name = info.joints[i].name;
90124
#if FRI_CLIENT_VERSION_MAJOR == 1
91-
commanded_joint_position[i] = joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION;
125+
commanded_joint_position[i] =
126+
si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION);
92127
#endif
93-
commanded_torque[i] = joint_name + "/" + HW_IF_COMMANDED_TORQUE;
94-
ipo_joint_position[i] = joint_name + "/" + HW_IF_IPO_JOINT_POSITION;
95-
position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION;
96-
external_torque[i] = joint_name + "/" + HW_IF_EXTERNAL_TORQUE;
97-
effort[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT;
98-
velocity[i] = joint_name + "/" + hardware_interface::HW_IF_VELOCITY;
128+
commanded_torque[i] =
129+
si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_TORQUE);
130+
ipo_joint_position[i] =
131+
si.get_state_interface_handle(joint_name + "/" + HW_IF_IPO_JOINT_POSITION);
132+
position[i] =
133+
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION);
134+
external_torque[i] =
135+
si.get_state_interface_handle(joint_name + "/" + HW_IF_EXTERNAL_TORQUE);
136+
effort[i] =
137+
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT);
138+
velocity[i] =
139+
si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_VELOCITY);
99140
}
100141

101-
sample_time = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAMPLE_TIME;
102-
session_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SESSION_STATE;
103-
connection_quality = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONNECTION_QUALITY;
104-
safety_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAFETY_STATE;
105-
operation_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OPERATION_MODE;
106-
drive_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_DRIVE_STATE;
107-
client_command_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CLIENT_COMMAND_MODE;
108-
overlay_type = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OVERLAY_TYPE;
109-
control_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONTROL_MODE;
110-
time_stamp_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_SEC;
111-
time_stamp_nano_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_NANO_SEC;
112-
tracking_performance = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TRACKING_PERFORMANCE;
142+
sample_time = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
143+
HW_IF_SAMPLE_TIME);
144+
session_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
145+
HW_IF_SESSION_STATE);
146+
connection_quality = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
147+
HW_IF_CONNECTION_QUALITY);
148+
safety_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
149+
HW_IF_SAFETY_STATE);
150+
operation_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
151+
HW_IF_OPERATION_MODE);
152+
drive_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
153+
HW_IF_DRIVE_STATE);
154+
client_command_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
155+
"/" + HW_IF_CLIENT_COMMAND_MODE);
156+
overlay_type = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
157+
HW_IF_OVERLAY_TYPE);
158+
control_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
159+
HW_IF_CONTROL_MODE);
160+
time_stamp_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" +
161+
HW_IF_TIME_STAMP_SEC);
162+
time_stamp_nano_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
163+
"/" + HW_IF_TIME_STAMP_NANO_SEC);
164+
tracking_performance = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) +
165+
"/" + HW_IF_TRACKING_PERFORMANCE);
166+
}
167+
168+
void nan_interfaces() const {
169+
// joint state interfaces
170+
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
171+
std::ignore = position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
172+
#if FRI_CLIENT_VERSION_MAJOR == 1
173+
std::ignore =
174+
commanded_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
175+
#endif
176+
std::ignore = effort[i]->set_value(std::numeric_limits<double>::quiet_NaN());
177+
std::ignore = commanded_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
178+
std::ignore = external_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
179+
std::ignore = ipo_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
180+
std::ignore = velocity[i]->set_value(std::numeric_limits<double>::quiet_NaN());
181+
}
182+
std::ignore = sample_time->set_value(std::numeric_limits<double>::quiet_NaN());
183+
std::ignore = tracking_performance->set_value(std::numeric_limits<double>::quiet_NaN());
184+
std::ignore = session_state->set_value(std::numeric_limits<double>::quiet_NaN());
185+
std::ignore = connection_quality->set_value(std::numeric_limits<double>::quiet_NaN());
186+
std::ignore = safety_state->set_value(std::numeric_limits<double>::quiet_NaN());
187+
std::ignore = operation_mode->set_value(std::numeric_limits<double>::quiet_NaN());
188+
std::ignore = drive_state->set_value(std::numeric_limits<double>::quiet_NaN());
189+
std::ignore = client_command_mode->set_value(std::numeric_limits<double>::quiet_NaN());
190+
std::ignore = overlay_type->set_value(std::numeric_limits<double>::quiet_NaN());
191+
std::ignore = control_mode->set_value(std::numeric_limits<double>::quiet_NaN());
192+
std::ignore = time_stamp_sec->set_value(std::numeric_limits<double>::quiet_NaN());
193+
std::ignore = time_stamp_nano_sec->set_value(std::numeric_limits<double>::quiet_NaN());
194+
}
195+
196+
void
197+
push(const lbr_fri_idl::msg::LBRState &lbr_state,
198+
const lbr_fri_idl::msg::LBRState::_measured_joint_position_type &velocity_estimate) const {
199+
// set the joint state interfaces
200+
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
201+
#if FRI_CLIENT_VERSION_MAJOR == 1
202+
std::ignore = commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]);
203+
#endif
204+
std::ignore = commanded_torque[i]->set_value(lbr_state.commanded_torque[i]);
205+
std::ignore = ipo_joint_position[i]->set_value(lbr_state.ipo_joint_position[i]);
206+
std::ignore = position[i]->set_value(lbr_state.measured_joint_position[i]);
207+
std::ignore = external_torque[i]->set_value(lbr_state.external_torque[i]);
208+
std::ignore = effort[i]->set_value(lbr_state.measured_torque[i]);
209+
std::ignore = velocity[i]->set_value(velocity_estimate[i]);
210+
}
211+
212+
// state interfaces without cast
213+
std::ignore = sample_time->set_value(lbr_state.sample_time);
214+
std::ignore = tracking_performance->set_value(lbr_state.tracking_performance);
215+
216+
// state interfaces with cast
217+
std::ignore = session_state->set_value(static_cast<double>(lbr_state.session_state));
218+
std::ignore =
219+
connection_quality->set_value(static_cast<double>(lbr_state.connection_quality));
220+
std::ignore = safety_state->set_value(static_cast<double>(lbr_state.safety_state));
221+
std::ignore = operation_mode->set_value(static_cast<double>(lbr_state.operation_mode));
222+
std::ignore = drive_state->set_value(static_cast<double>(lbr_state.drive_state));
223+
std::ignore =
224+
client_command_mode->set_value(static_cast<double>(lbr_state.client_command_mode));
225+
std::ignore = overlay_type->set_value(static_cast<double>(lbr_state.overlay_type));
226+
std::ignore = control_mode->set_value(static_cast<double>(lbr_state.control_mode));
227+
std::ignore = time_stamp_sec->set_value(static_cast<double>(lbr_state.time_stamp_sec));
228+
std::ignore =
229+
time_stamp_nano_sec->set_value(static_cast<double>(lbr_state.time_stamp_nano_sec));
113230
}
114231
};
115232

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

293+
// session state tracking
294+
KUKA::FRI::ESessionState previous_session_state_;
295+
176296
// velocity computation
177297
lbr_fri_idl::msg::LBRState::_measured_joint_position_type last_measured_joint_position_,
178298
velocity_;
@@ -189,9 +309,9 @@ class SystemInterface : public hardware_interface::SystemInterface {
189309
lbr_fri_idl::msg::LBRCommand lbr_command_;
190310
lbr_fri_idl::msg::LBRState lbr_state_;
191311

192-
// keys for command / state interfaces
193-
CommandKeys command_keys_;
194-
StateKeys state_keys_;
312+
// interface handles for commands / states
313+
CommandInterfaceHandles command_if_handles_;
314+
StateInterfaceHandles state_if_handles_;
195315
};
196316
} // namespace lbr_ros2_control
197317
#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_

0 commit comments

Comments
 (0)