Skip to content

Commit 0c2e827

Browse files
mhubiigithub-actions[bot]
mhubii
authored and
github-actions[bot]
committed
finalize interface handles
(cherry picked from commit c5181f7)
1 parent 154bb09 commit 0c2e827

2 files changed

Lines changed: 69 additions & 58 deletions

File tree

‎lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp‎

Lines changed: 51 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -86,22 +86,22 @@ class SystemInterface : public hardware_interface::SystemInterface {
8686

8787
void nan_interfaces() const {
8888
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());
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());
9191
}
9292
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
93-
wrench[i]->set_value(std::numeric_limits<double>::quiet_NaN());
93+
std::ignore = wrench[i]->set_value(std::numeric_limits<double>::quiet_NaN());
9494
}
9595
}
9696

9797
void pull(lbr_fri_idl::msg::LBRCommand &lbr_command) const {
9898
// populate command message
9999
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();
100+
lbr_command.joint_position[i] = joint_position[i]->get_optional().value();
101+
lbr_command.torque[i] = torque[i]->get_optional().value();
102102
}
103103
for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) {
104-
lbr_command.wrench[i] = wrench[i]->get_value();
104+
lbr_command.wrench[i] = wrench[i]->get_optional().value();
105105
}
106106
}
107107
};
@@ -168,28 +168,29 @@ class SystemInterface : public hardware_interface::SystemInterface {
168168
void nan_interfaces() const {
169169
// joint state interfaces
170170
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
171-
position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
171+
std::ignore = position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
172172
#if FRI_CLIENT_VERSION_MAJOR == 1
173-
commanded_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
173+
std::ignore =
174+
commanded_joint_position[i]->set_value(std::numeric_limits<double>::quiet_NaN());
174175
#endif
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());
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());
180181
}
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());
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());
193194
}
194195

195196
void
@@ -198,31 +199,34 @@ class SystemInterface : public hardware_interface::SystemInterface {
198199
// set the joint state interfaces
199200
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
200201
#if FRI_CLIENT_VERSION_MAJOR == 1
201-
commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]);
202+
std::ignore = commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]);
202203
#endif
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]);
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]);
209210
}
210211

211212
// state interfaces without cast
212-
sample_time->set_value(lbr_state.sample_time);
213-
tracking_performance->set_value(lbr_state.tracking_performance);
213+
std::ignore = sample_time->set_value(lbr_state.sample_time);
214+
std::ignore = tracking_performance->set_value(lbr_state.tracking_performance);
214215

215216
// 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));
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));
226230
}
227231
};
228232

@@ -286,6 +290,9 @@ class SystemInterface : public hardware_interface::SystemInterface {
286290
std::shared_ptr<lbr_fri_ros2::AsyncClient> async_client_ptr_;
287291
std::unique_ptr<lbr_fri_ros2::App> app_ptr_;
288292

293+
// session state tracking
294+
KUKA::FRI::ESessionState previous_session_state_;
295+
289296
// velocity computation
290297
lbr_fri_idl::msg::LBRState::_measured_joint_position_type last_measured_joint_position_,
291298
velocity_;

‎lbr_ros2_control/src/system_interface.cpp‎

Lines changed: 18 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,8 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
160160
}
161161
std::this_thread::sleep_for(std::chrono::seconds(1));
162162
}
163+
// initialize the previous session state
164+
previous_session_state_ = static_cast<KUKA::FRI::ESessionState>(state.session_state);
163165
return controller_interface::CallbackReturn::SUCCESS;
164166
}
165167

@@ -191,9 +193,8 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim
191193
}
192194

193195
// exit once robot exits COMMANDING_ACTIVE (for safety)
194-
if (exit_commanding_active_(
195-
static_cast<KUKA::FRI::ESessionState>(state_if_handles_.session_state->get_value()),
196-
static_cast<KUKA::FRI::ESessionState>(lbr_state_.session_state))) {
196+
auto current_session_state = static_cast<KUKA::FRI::ESessionState>(lbr_state_.session_state);
197+
if (exit_commanding_active_(previous_session_state_, current_session_state)) {
197198
RCLCPP_ERROR_STREAM(get_node()->get_logger(),
198199
lbr_fri_ros2::ColorScheme::ERROR
199200
<< "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup"
@@ -202,20 +203,21 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim
202203
app_ptr_->close_udp_socket();
203204
return hardware_interface::return_type::ERROR;
204205
}
206+
previous_session_state_ = current_session_state;
207+
208+
// compute velocity
209+
compute_velocity_();
205210

206211
// set the joint state interfaces
207212
state_if_handles_.push(lbr_state_, velocity_);
208213

209-
// additional velocity state interface
210-
compute_velocity_();
211214
update_last_states_();
212215
return hardware_interface::return_type::OK;
213216
}
214217

215218
hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*time*/,
216219
const rclcpp::Duration & /*period*/) {
217-
if (static_cast<KUKA::FRI::ESessionState>(state_if_handles_.session_state->get_value()) !=
218-
KUKA::FRI::COMMANDING_ACTIVE) {
220+
if (lbr_state_.session_state != KUKA::FRI::COMMANDING_ACTIVE) {
219221
return hardware_interface::return_type::OK;
220222
}
221223

@@ -496,10 +498,10 @@ void SystemInterface::nan_last_states_() {
496498

497499
void SystemInterface::update_last_states_() {
498500
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
499-
last_measured_joint_position_[i] = state_if_handles_.position[i]->get_value();
501+
last_measured_joint_position_[i] = lbr_state_.measured_joint_position[i];
500502
}
501-
last_time_stamp_sec_ = state_if_handles_.time_stamp_sec->get_value();
502-
last_time_stamp_nano_sec_ = state_if_handles_.time_stamp_nano_sec->get_value();
503+
last_time_stamp_sec_ = lbr_state_.time_stamp_sec;
504+
last_time_stamp_nano_sec_ = lbr_state_.time_stamp_nano_sec;
503505
}
504506

505507
void SystemInterface::compute_velocity_() {
@@ -508,8 +510,8 @@ void SystemInterface::compute_velocity_() {
508510
return;
509511
}
510512

511-
auto time_stamp_sec = state_if_handles_.time_stamp_sec->get_value();
512-
auto time_stamp_nano_sec = state_if_handles_.time_stamp_nano_sec->get_value();
513+
const double time_stamp_sec = lbr_state_.time_stamp_sec;
514+
const double time_stamp_nano_sec = lbr_state_.time_stamp_nano_sec;
513515

514516
// state wasn't updated
515517
if (last_time_stamp_sec_ == time_stamp_sec && last_time_stamp_nano_sec_ == time_stamp_nano_sec) {
@@ -518,9 +520,11 @@ void SystemInterface::compute_velocity_() {
518520

519521
double dt = time_stamps_to_sec_(time_stamp_sec, time_stamp_nano_sec) -
520522
time_stamps_to_sec_(last_time_stamp_sec_, last_time_stamp_nano_sec_);
523+
if (dt <= 0) {
524+
return;
525+
}
521526
for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
522-
velocity_[i] =
523-
(state_if_handles_.position[i]->get_value() - last_measured_joint_position_[i]) / dt;
527+
velocity_[i] = (lbr_state_.measured_joint_position[i] - last_measured_joint_position_[i]) / dt;
524528
}
525529
}
526530

0 commit comments

Comments
 (0)