Skip to content

Commit 195d57d

Browse files
mhubiimhubii
authored
Merge pull request #312 from lbr-stack/backport-311-to-jazzy
Backport 311 to jazzy
2 parents c361620 + b951a5f commit 195d57d

File tree

9 files changed

+55
-43
lines changed

9 files changed

+55
-43
lines changed

‎lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,8 @@ class AdmittanceController : public controller_interface::ControllerInterface {
4949
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
5050

5151
protected:
52-
bool reference_state_interfaces_();
53-
void clear_state_interfaces_();
52+
bool assign_state_interfaces_();
53+
void release_state_interfaces_();
5454
void configure_joint_names_();
5555
void configure_admittance_impl_();
5656
void configure_inv_jac_ctrl_impl_();

‎lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,8 +44,8 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf
4444
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
4545

4646
protected:
47-
bool reference_command_interfaces_();
48-
void clear_command_interfaces_();
47+
bool assign_command_interfaces_();
48+
void release_command_interfaces_();
4949
void configure_joint_names_();
5050

5151
lbr_fri_ros2::jnt_name_array_t joint_names_;

‎lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -66,10 +66,10 @@ class LBRWrenchCommandController : public controller_interface::ChainableControl
6666
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
6767

6868
protected:
69-
bool reference_state_interfaces_();
70-
bool reference_command_interfaces_();
71-
void clear_state_interfaces_();
72-
void clear_command_interfaces_();
69+
bool assign_state_interfaces_();
70+
bool assign_command_interfaces_();
71+
void release_state_interfaces_();
72+
void release_command_interfaces_();
7373
void configure_joint_names_();
7474
void configure_parameters_();
7575
bool zero_wrench_commands_();

‎lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,8 +48,8 @@ class TwistController : public controller_interface::ControllerInterface {
4848
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
4949

5050
protected:
51-
bool reference_state_interfaces_();
52-
void clear_state_interfaces_();
51+
bool assign_state_interfaces_();
52+
void release_state_interfaces_();
5353
void reset_command_buffer_();
5454
void zero_joint_velocity_command_();
5555
void configure_joint_names_();

‎lbr_ros2_control/src/controllers/admittance_controller.cpp‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -183,7 +183,8 @@ AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_st
183183

184184
controller_interface::CallbackReturn
185185
AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
186-
if (!reference_state_interfaces_()) {
186+
if (!assign_state_interfaces_()) {
187+
release_state_interfaces_();
187188
return controller_interface::CallbackReturn::ERROR;
188189
}
189190
init_filters_with_update_rate_();
@@ -196,6 +197,7 @@ AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_sta
196197
<< "External force-torques detected during admittance controller activation. "
197198
"Please make sure load data was calibrated."
198199
<< lbr_fri_ros2::ColorScheme::ENDC);
200+
release_state_interfaces_();
199201
return controller_interface::CallbackReturn::ERROR;
200202
}
201203
} catch (const std::exception &e) {
@@ -205,21 +207,22 @@ AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_sta
205207
<< "Failed to check external force-torques during admittance controller activation "
206208
"with: "
207209
<< e.what() << lbr_fri_ros2::ColorScheme::ENDC);
210+
release_state_interfaces_();
208211
return controller_interface::CallbackReturn::ERROR;
209212
}
210213
return controller_interface::CallbackReturn::SUCCESS;
211214
}
212215

213216
controller_interface::CallbackReturn
214217
AdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
215-
clear_state_interfaces_();
218+
release_state_interfaces_();
216219
return controller_interface::CallbackReturn::SUCCESS;
217220
}
218221

219-
bool AdmittanceController::reference_state_interfaces_() {
222+
bool AdmittanceController::assign_state_interfaces_() {
220223
for (auto &state_interface : state_interfaces_) {
221224
if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
222-
joint_position_state_interfaces_.emplace_back(std::ref(state_interface));
225+
joint_position_state_interfaces_.push_back(std::ref(state_interface));
223226
}
224227
if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) {
225228
session_state_interface_ptr_ =
@@ -243,8 +246,9 @@ bool AdmittanceController::reference_state_interfaces_() {
243246
return true;
244247
}
245248

246-
void AdmittanceController::clear_state_interfaces_() {
249+
void AdmittanceController::release_state_interfaces_() {
247250
joint_position_state_interfaces_.clear();
251+
session_state_interface_ptr_.reset();
248252
estimated_ft_sensor_ptr_->release_interfaces();
249253
}
250254

‎lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -76,25 +76,26 @@ LBRTorqueCommandController::on_configure(const rclcpp_lifecycle::State & /*previ
7676

7777
controller_interface::CallbackReturn
7878
LBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
79-
if (!reference_command_interfaces_()) {
79+
if (!assign_command_interfaces_()) {
80+
release_command_interfaces_();
8081
return controller_interface::CallbackReturn::ERROR;
8182
}
8283
return controller_interface::CallbackReturn::SUCCESS;
8384
}
8485

8586
controller_interface::CallbackReturn
8687
LBRTorqueCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
87-
clear_command_interfaces_();
88+
release_command_interfaces_();
8889
return controller_interface::CallbackReturn::SUCCESS;
8990
}
9091

91-
bool LBRTorqueCommandController::reference_command_interfaces_() {
92+
bool LBRTorqueCommandController::assign_command_interfaces_() {
9293
for (auto &command_interface : command_interfaces_) {
9394
if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
94-
joint_position_command_interfaces_.emplace_back(std::ref(command_interface));
95+
joint_position_command_interfaces_.push_back(std::ref(command_interface));
9596
}
9697
if (command_interface.get_interface_name() == hardware_interface::HW_IF_EFFORT) {
97-
torque_command_interfaces_.emplace_back(std::ref(command_interface));
98+
torque_command_interfaces_.push_back(std::ref(command_interface));
9899
}
99100
}
100101
if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
@@ -120,7 +121,7 @@ bool LBRTorqueCommandController::reference_command_interfaces_() {
120121
return true;
121122
}
122123

123-
void LBRTorqueCommandController::clear_command_interfaces_() {
124+
void LBRTorqueCommandController::release_command_interfaces_() {
124125
joint_position_command_interfaces_.clear();
125126
torque_command_interfaces_.clear();
126127
}

‎lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp‎

Lines changed: 15 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -238,29 +238,32 @@ LBRWrenchCommandController::on_configure(const rclcpp_lifecycle::State & /*previ
238238
controller_interface::CallbackReturn
239239
LBRWrenchCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
240240
reference_interfaces_.assign(lbr_fri_ros2::N_JNTS, std::numeric_limits<double>::quiet_NaN());
241-
if (!reference_state_interfaces_()) {
241+
if (!assign_state_interfaces_()) {
242+
release_state_interfaces_();
242243
return controller_interface::CallbackReturn::ERROR;
243244
}
244-
if (!reference_command_interfaces_()) {
245+
if (!assign_command_interfaces_()) {
246+
release_state_interfaces_();
247+
release_command_interfaces_();
245248
return controller_interface::CallbackReturn::ERROR;
246249
}
247250
return controller_interface::CallbackReturn::SUCCESS;
248251
}
249252

250253
controller_interface::CallbackReturn
251254
LBRWrenchCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
252-
clear_state_interfaces_();
253-
clear_command_interfaces_();
255+
release_state_interfaces_();
256+
release_command_interfaces_();
254257
return controller_interface::CallbackReturn::SUCCESS;
255258
}
256259

257-
bool LBRWrenchCommandController::reference_state_interfaces_() {
260+
bool LBRWrenchCommandController::assign_state_interfaces_() {
258261
for (auto &state_interface : state_interfaces_) {
259262
if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
260-
joint_position_state_interfaces_.emplace_back(std::ref(state_interface));
263+
joint_position_state_interfaces_.push_back(std::ref(state_interface));
261264
}
262265
if (state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY) {
263-
joint_velocity_state_interfaces_.emplace_back(std::ref(state_interface));
266+
joint_velocity_state_interfaces_.push_back(std::ref(state_interface));
264267
}
265268
}
266269
if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
@@ -286,13 +289,13 @@ bool LBRWrenchCommandController::reference_state_interfaces_() {
286289
return true;
287290
}
288291

289-
bool LBRWrenchCommandController::reference_command_interfaces_() {
292+
bool LBRWrenchCommandController::assign_command_interfaces_() {
290293
for (auto &command_interface : command_interfaces_) {
291294
if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
292-
joint_position_command_interfaces_.emplace_back(std::ref(command_interface));
295+
joint_position_command_interfaces_.push_back(std::ref(command_interface));
293296
}
294297
if (command_interface.get_prefix_name() == HW_IF_WRENCH_PREFIX) {
295-
wrench_command_interfaces_.emplace_back(std::ref(command_interface));
298+
wrench_command_interfaces_.push_back(std::ref(command_interface));
296299
}
297300
}
298301
if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
@@ -317,12 +320,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() {
317320
return true;
318321
}
319322

320-
void LBRWrenchCommandController::clear_state_interfaces_() {
323+
void LBRWrenchCommandController::release_state_interfaces_() {
321324
joint_position_state_interfaces_.clear();
322325
joint_velocity_state_interfaces_.clear();
323326
}
324327

325-
void LBRWrenchCommandController::clear_command_interfaces_() {
328+
void LBRWrenchCommandController::release_command_interfaces_() {
326329
joint_position_command_interfaces_.clear();
327330
wrench_command_interfaces_.clear();
328331
}

‎lbr_ros2_control/src/controllers/twist_controller.cpp‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,8 @@ TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/
132132

133133
controller_interface::CallbackReturn
134134
TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
135-
if (!reference_state_interfaces_()) {
135+
if (!assign_state_interfaces_()) {
136+
release_state_interfaces_();
136137
return controller_interface::CallbackReturn::ERROR;
137138
}
138139
reset_command_buffer_();
@@ -142,16 +143,16 @@ TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
142143

143144
controller_interface::CallbackReturn
144145
TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
145-
clear_state_interfaces_();
146+
release_state_interfaces_();
146147
reset_command_buffer_();
147148
zero_joint_velocity_command_();
148149
return controller_interface::CallbackReturn::SUCCESS;
149150
}
150151

151-
bool TwistController::reference_state_interfaces_() {
152+
bool TwistController::assign_state_interfaces_() {
152153
for (auto &state_interface : state_interfaces_) {
153154
if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
154-
joint_position_state_interfaces_.emplace_back(std::ref(state_interface));
155+
joint_position_state_interfaces_.push_back(std::ref(state_interface));
155156
}
156157
if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) {
157158
session_state_interface_ptr_ =
@@ -170,7 +171,10 @@ bool TwistController::reference_state_interfaces_() {
170171
return true;
171172
}
172173

173-
void TwistController::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); }
174+
void TwistController::release_state_interfaces_() {
175+
joint_position_state_interfaces_.clear();
176+
session_state_interface_ptr_.reset();
177+
}
174178

175179
void TwistController::reset_command_buffer_() {
176180
rt_twist_ptr_ =

‎lbr_ros2_control/src/system_interface.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -208,11 +208,11 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim
208208
lbr_state_ = async_client_ptr_->get_state_interface()->get_state();
209209

210210
if (period.seconds() - lbr_state_.sample_time * 0.2 > lbr_state_.sample_time) {
211-
RCLCPP_WARN_STREAM(get_node()->get_logger(),
212-
lbr_fri_ros2::ColorScheme::WARNING
213-
<< "Increase update_rate parameter for controller_manager to "
214-
<< std::to_string(static_cast<int>(1. / lbr_state_.sample_time))
215-
<< " Hz or more" << lbr_fri_ros2::ColorScheme::ENDC);
211+
RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 500 /*ms*/,
212+
lbr_fri_ros2::ColorScheme::WARNING
213+
<< "Increase update_rate parameter for controller_manager to "
214+
<< std::to_string(static_cast<int>(1. / lbr_state_.sample_time))
215+
<< " Hz or more" << lbr_fri_ros2::ColorScheme::ENDC);
216216
}
217217

218218
// exit once robot exits COMMANDING_ACTIVE (for safety)

0 commit comments

Comments
 (0)