Skip to content

Commit 76c07f9

Browse files
kumar-sanjeeevchristophfroehlichskasaikishor
authored
Use new handles API in ros2_controllers to fix deprecation warnings (#1566)
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com> Co-authored-by: ska <sanjeev.kumar@ipa.fraunhofer.de> Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at> Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
1 parent a10c564 commit 76c07f9

File tree

31 files changed

+752
-374
lines changed

31 files changed

+752
-374
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp

Lines changed: 27 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -55,18 +55,39 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
5555

5656
bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
5757
{
58+
auto logger = get_node()->get_logger();
59+
5860
if (params_.open_loop)
5961
{
6062
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
6163
}
6264
else
6365
{
64-
const double traction_right_wheel_value =
65-
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
66-
const double traction_left_wheel_value =
67-
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
68-
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
69-
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
66+
const auto traction_right_wheel_value_op =
67+
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional();
68+
const auto traction_left_wheel_value_op =
69+
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional();
70+
const auto steering_right_position_op =
71+
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional();
72+
const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional();
73+
74+
if (
75+
!traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() ||
76+
!steering_right_position_op.has_value() || !steering_left_position_op.has_value())
77+
{
78+
RCLCPP_DEBUG(
79+
logger,
80+
"Unable to retrieve the data from right wheel or left wheel or right steering position or "
81+
"left steering position!");
82+
83+
return true;
84+
}
85+
86+
const double traction_right_wheel_value = traction_right_wheel_value_op.value();
87+
const double traction_left_wheel_value = traction_left_wheel_value_op.value();
88+
const double steering_right_position = steering_right_position_op.value();
89+
const double steering_left_position = steering_left_position_op.value();
90+
7091
if (
7192
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
7293
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))

‎ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success)
140140
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
141141
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
142142
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
143-
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
143+
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
144144
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
145-
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
145+
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
146146

147147
ASSERT_EQ(
148148
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -173,17 +173,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
173173

174174
// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
175175
EXPECT_NEAR(
176-
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
177-
COMMON_THRESHOLD);
176+
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
177+
0.22222222222222224, COMMON_THRESHOLD);
178178
EXPECT_NEAR(
179-
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
180-
COMMON_THRESHOLD);
179+
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
180+
0.22222222222222224, COMMON_THRESHOLD);
181181
EXPECT_NEAR(
182-
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
183-
COMMON_THRESHOLD);
182+
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
183+
1.4179821977774734, COMMON_THRESHOLD);
184184
EXPECT_NEAR(
185-
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
186-
COMMON_THRESHOLD);
185+
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
186+
1.4179821977774734, COMMON_THRESHOLD);
187187

188188
EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
189189
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
@@ -213,17 +213,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
213213

214214
// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
215215
EXPECT_NEAR(
216-
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
217-
COMMON_THRESHOLD);
216+
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional().value(),
217+
0.22222222222222224, COMMON_THRESHOLD);
218218
EXPECT_NEAR(
219-
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
220-
COMMON_THRESHOLD);
219+
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional().value(),
220+
0.22222222222222224, COMMON_THRESHOLD);
221221
EXPECT_NEAR(
222-
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
223-
COMMON_THRESHOLD);
222+
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional().value(),
223+
1.4179821977774734, COMMON_THRESHOLD);
224224
EXPECT_NEAR(
225-
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
226-
COMMON_THRESHOLD);
225+
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(),
226+
1.4179821977774734, COMMON_THRESHOLD);
227227

228228
EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
229229
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
@@ -264,17 +264,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
264264

265265
// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
266266
EXPECT_NEAR(
267-
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
268-
COMMON_THRESHOLD);
267+
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
268+
0.22222222222222224, COMMON_THRESHOLD);
269269
EXPECT_NEAR(
270-
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
271-
COMMON_THRESHOLD);
270+
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
271+
0.22222222222222224, COMMON_THRESHOLD);
272272
EXPECT_NEAR(
273-
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
274-
COMMON_THRESHOLD);
273+
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
274+
1.4179821977774734, COMMON_THRESHOLD);
275275
EXPECT_NEAR(
276-
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
277-
COMMON_THRESHOLD);
276+
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
277+
1.4179821977774734, COMMON_THRESHOLD);
278278

279279
subscribe_and_get_messages(msg);
280280

‎admittance_controller/src/admittance_controller.cpp

Lines changed: 36 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -563,21 +563,37 @@ void AdmittanceController::read_state_from_hardware(
563563
{
564564
if (has_position_state_interface_)
565565
{
566-
state_current.positions[joint_ind] =
567-
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
568-
nan_position |= std::isnan(state_current.positions[joint_ind]);
566+
const auto state_current_position_op =
567+
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_optional();
568+
nan_position |=
569+
!state_current_position_op.has_value() || std::isnan(state_current_position_op.value());
570+
if (state_current_position_op.has_value())
571+
{
572+
state_current.positions[joint_ind] = state_current_position_op.value();
573+
}
569574
}
570575
if (has_velocity_state_interface_)
571576
{
572-
state_current.velocities[joint_ind] =
573-
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
574-
nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
577+
auto state_current_velocity_op =
578+
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_optional();
579+
nan_velocity |=
580+
!state_current_velocity_op.has_value() || std::isnan(state_current_velocity_op.value());
581+
582+
if (state_current_velocity_op.has_value())
583+
{
584+
state_current.velocities[joint_ind] = state_current_velocity_op.value();
585+
}
575586
}
576587
if (has_acceleration_state_interface_)
577588
{
578-
state_current.accelerations[joint_ind] =
579-
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
580-
nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]);
589+
auto state_current_acceleration_op =
590+
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_optional();
591+
nan_acceleration |= !state_current_acceleration_op.has_value() ||
592+
std::isnan(state_current_acceleration_op.value());
593+
if (state_current_acceleration_op.has_value())
594+
{
595+
state_current.accelerations[joint_ind] = state_current_acceleration_op.value();
596+
}
581597
}
582598
}
583599

@@ -613,23 +629,31 @@ void AdmittanceController::write_state_to_hardware(
613629
size_t vel_ind =
614630
(has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind;
615631
size_t acc_ind = vel_ind + has_acceleration_command_interface_;
632+
633+
auto logger = get_node()->get_logger();
634+
616635
for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
617636
{
637+
bool success = true;
618638
if (has_position_command_interface_)
619639
{
620-
command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
640+
success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
621641
state_commanded.positions[joint_ind]);
622642
}
623643
if (has_velocity_command_interface_)
624644
{
625-
command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
645+
success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
626646
state_commanded.velocities[joint_ind]);
627647
}
628648
if (has_acceleration_command_interface_)
629649
{
630-
command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
650+
success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
631651
state_commanded.accelerations[joint_ind]);
632652
}
653+
if (!success)
654+
{
655+
RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind);
656+
}
633657
}
634658
last_commanded_ = state_commanded;
635659
}

‎bicycle_steering_controller/src/bicycle_steering_controller.cpp

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,14 +45,27 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet
4545

4646
bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
4747
{
48+
auto logger = get_node()->get_logger();
49+
4850
if (params_.open_loop)
4951
{
5052
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
5153
}
5254
else
5355
{
54-
const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
55-
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
56+
const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional();
57+
const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional();
58+
59+
if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value())
60+
{
61+
RCLCPP_DEBUG(
62+
logger, "Unable to retrieve the data from the traction wheel or steering position!");
63+
return true;
64+
}
65+
66+
const double traction_wheel_value = traction_wheel_value_op.value();
67+
const double steering_position = steering_position_op.value();
68+
5669
if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
5770
{
5871
if (params_.position_feedback)

‎bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -126,9 +126,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success)
126126
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
127127
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
128128
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
129-
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
129+
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
130130
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
131-
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
131+
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
132132

133133
ASSERT_EQ(
134134
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -158,9 +158,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
158158
controller_interface::return_type::OK);
159159

160160
EXPECT_NEAR(
161-
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
161+
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
162+
COMMON_THRESHOLD);
162163
EXPECT_NEAR(
163-
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
164+
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
164165
COMMON_THRESHOLD);
165166

166167
EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
@@ -190,9 +191,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
190191
controller_interface::return_type::OK);
191192

192193
EXPECT_NEAR(
193-
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
194+
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
195+
COMMON_THRESHOLD);
194196
EXPECT_NEAR(
195-
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
197+
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
196198
COMMON_THRESHOLD);
197199

198200
EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
@@ -246,9 +248,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
246248
controller_interface::return_type::OK);
247249

248250
EXPECT_NEAR(
249-
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
251+
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
252+
COMMON_THRESHOLD);
250253
EXPECT_NEAR(
251-
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
254+
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
252255
COMMON_THRESHOLD);
253256

254257
subscribe_and_get_messages(msg);

‎diff_drive_controller/src/diff_drive_controller.cpp

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -578,11 +578,15 @@ void DiffDriveController::reset_buffers()
578578

579579
void DiffDriveController::halt()
580580
{
581-
const auto halt_wheels = [](auto & wheel_handles)
581+
auto logger = get_node()->get_logger();
582+
const auto halt_wheels = [&](auto & wheel_handles)
582583
{
583584
for (const auto & wheel_handle : wheel_handles)
584585
{
585-
wheel_handle.velocity.get().set_value(0.0);
586+
if (!wheel_handle.velocity.get().set_value(0.0))
587+
{
588+
RCLCPP_WARN(logger, "Failed to set wheel velocity to value 0.0");
589+
}
586590
}
587591
};
588592

‎effort_controllers/src/joint_group_effort_controller.cpp

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,11 +55,14 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(
5555
const rclcpp_lifecycle::State & previous_state)
5656
{
5757
auto ret = ForwardCommandController::on_deactivate(previous_state);
58-
5958
// stop all joints
6059
for (auto & command_interface : command_interfaces_)
6160
{
62-
command_interface.set_value(0.0);
61+
if (!command_interface.set_value(0.0))
62+
{
63+
RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0");
64+
return controller_interface::CallbackReturn::SUCCESS;
65+
}
6366
}
6467

6568
return ret;

0 commit comments

Comments
 (0)