Skip to content

Commit 154bb09

Browse files
mhubiigithub-actions[bot]
mhubii
authored and
github-actions[bot]
committed
initial interface handle refactor
(cherry picked from commit ce3925c)
1 parent 352e09f commit 154bb09

3 files changed

Lines changed: 179 additions & 133 deletions

File tree

‎CHANGELOG.rst‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,10 @@ Jazzy v2.x (TBD)
77

88
* Adds a dedicated ``namespace`` argument to mock / hardware launch files.
99
* Fix the MoveIt servo node name.
10-
* ``lbr_demos``: Replace ``ament_target_dependencies`` with ``target_link_libraries``.
10+
* ``lbr_demos``:
11+
12+
* Replace ``ament_target_dependencies`` with ``target_link_libraries``.
13+
* ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses.
1114
* ``lbr_description``:
1215

1316
* Sets the ``k_velocity`` to fix the safety limits for effort client command modes.

‎lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp‎

Lines changed: 157 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,173 @@ 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+
joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
90+
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+
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_value();
101+
lbr_command.torque[i] = torque[i]->get_value();
102+
}
103+
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
104+
lbr_command.wrench[i] = wrench[i]->get_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 {
110+
#if FRI_CLIENT_VERSION_MAJOR == 1
111+
std::array<hardware_interface::StateInterface::SharedPtr, lbr_fri_ros2::N_JNTS>
112+
commanded_joint_position;
113+
#endif
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;
119+
120+
void populate(const hardware_interface::SystemInterface &si) {
121+
const auto &info = si.get_hardware_info();
122+
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
123+
const auto &joint_name = info.joints[i].name;
124+
#if FRI_CLIENT_VERSION_MAJOR == 1
125+
commanded_joint_position[i] =
126+
si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION);
127+
#endif
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);
140+
}
141+
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+
position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
78172
#if FRI_CLIENT_VERSION_MAJOR == 1
79-
lbr_fri_ros2::jnt_name_array_t commanded_joint_position;
173+
commanded_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
80174
#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;
175+
effort[i]->set_value(std::numeric_limits<double>::quiet_NaN());
176+
commanded_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
177+
external_torque[i]->set_value(std::numeric_limits<double>::quiet_NaN());
178+
ipo_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
179+
velocity[i]->set_value(std::numeric_limits<double>::quiet_NaN());
180+
}
181+
sample_time->set_value(std::numeric_limits<double>::quiet_NaN());
182+
tracking_performance->set_value(std::numeric_limits<double>::quiet_NaN());
183+
session_state->set_value(std::numeric_limits<double>::quiet_NaN());
184+
connection_quality->set_value(std::numeric_limits<double>::quiet_NaN());
185+
safety_state->set_value(std::numeric_limits<double>::quiet_NaN());
186+
operation_mode->set_value(std::numeric_limits<double>::quiet_NaN());
187+
drive_state->set_value(std::numeric_limits<double>::quiet_NaN());
188+
client_command_mode->set_value(std::numeric_limits<double>::quiet_NaN());
189+
overlay_type->set_value(std::numeric_limits<double>::quiet_NaN());
190+
control_mode->set_value(std::numeric_limits<double>::quiet_NaN());
191+
time_stamp_sec->set_value(std::numeric_limits<double>::quiet_NaN());
192+
time_stamp_nano_sec->set_value(std::numeric_limits<double>::quiet_NaN());
193+
}
86194

87-
void populate_keys(const hardware_interface::HardwareInfo &info) {
195+
void
196+
push(const lbr_fri_idl::msg::LBRState &lbr_state,
197+
const lbr_fri_idl::msg::LBRState::_measured_joint_position_type &velocity_estimate) const {
198+
// set the joint state interfaces
88199
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
89-
auto joint_name = info.joints[i].name;
90200
#if FRI_CLIENT_VERSION_MAJOR == 1
91-
commanded_joint_position[i] = joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION;
201+
commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]);
92202
#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;
203+
commanded_torque[i]->set_value(lbr_state.commanded_torque[i]);
204+
ipo_joint_position[i]->set_value(lbr_state.ipo_joint_position[i]);
205+
position[i]->set_value(lbr_state.measured_joint_position[i]);
206+
external_torque[i]->set_value(lbr_state.external_torque[i]);
207+
effort[i]->set_value(lbr_state.measured_torque[i]);
208+
velocity[i]->set_value(velocity_estimate[i]);
99209
}
100210

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;
211+
// state interfaces without cast
212+
sample_time->set_value(lbr_state.sample_time);
213+
tracking_performance->set_value(lbr_state.tracking_performance);
214+
215+
// state interfaces with cast
216+
session_state->set_value(static_cast<double>(lbr_state.session_state));
217+
connection_quality->set_value(static_cast<double>(lbr_state.connection_quality));
218+
safety_state->set_value(static_cast<double>(lbr_state.safety_state));
219+
operation_mode->set_value(static_cast<double>(lbr_state.operation_mode));
220+
drive_state->set_value(static_cast<double>(lbr_state.drive_state));
221+
client_command_mode->set_value(static_cast<double>(lbr_state.client_command_mode));
222+
overlay_type->set_value(static_cast<double>(lbr_state.overlay_type));
223+
control_mode->set_value(static_cast<double>(lbr_state.control_mode));
224+
time_stamp_sec->set_value(static_cast<double>(lbr_state.time_stamp_sec));
225+
time_stamp_nano_sec->set_value(static_cast<double>(lbr_state.time_stamp_nano_sec));
113226
}
114227
};
115228

@@ -189,9 +302,9 @@ class SystemInterface : public hardware_interface::SystemInterface {
189302
lbr_fri_idl::msg::LBRCommand lbr_command_;
190303
lbr_fri_idl::msg::LBRState lbr_state_;
191304

192-
// keys for command / state interfaces
193-
CommandKeys command_keys_;
194-
StateKeys state_keys_;
305+
// interface handles for commands / states
306+
CommandInterfaceHandles command_if_handles_;
307+
StateInterfaceHandles state_if_handles_;
195308
};
196309
} // namespace lbr_ros2_control
197310
#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_

0 commit comments

Comments
 (0)