Skip to content

Commit c8511bb

Browse files
kumar-sanjeeevchristophfroehlichskasaikishor
committed
Use new handles API in ros2_controllers to fix deprecation warnings (#1566)
Co-authored-by: Christoph Fröhlich <[email protected]> Co-authored-by: ska <[email protected]> Co-authored-by: Christoph Froehlich <[email protected]> Co-authored-by: Sai Kishor Kothakota <[email protected]>
1 parent 825470c commit c8511bb

File tree

31 files changed

+728
-361
lines changed

31 files changed

+728
-361
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp‎

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

115115
bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
116116
{
117+
auto logger = get_node()->get_logger();
118+
117119
if (params_.open_loop)
118120
{
119121
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
120122
}
121123
else
122124
{
123-
const double traction_right_wheel_value =
124-
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
125-
const double traction_left_wheel_value =
126-
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
127-
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
128-
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
125+
const auto traction_right_wheel_value_op =
126+
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional();
127+
const auto traction_left_wheel_value_op =
128+
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional();
129+
const auto steering_right_position_op =
130+
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional();
131+
const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional();
132+
133+
if (
134+
!traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() ||
135+
!steering_right_position_op.has_value() || !steering_left_position_op.has_value())
136+
{
137+
RCLCPP_DEBUG(
138+
logger,
139+
"Unable to retrieve the data from right wheel or left wheel or right steering position or "
140+
"left steering position!");
141+
142+
return true;
143+
}
144+
145+
const double traction_right_wheel_value = traction_right_wheel_value_op.value();
146+
const double traction_left_wheel_value = traction_left_wheel_value_op.value();
147+
const double steering_right_position = steering_right_position_op.value();
148+
const double steering_left_position = steering_left_position_op.value();
149+
129150
if (
130151
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
131152
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_.readFromRT()))->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_.readFromRT()))->twist.linear.x));
229229
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
@@ -263,17 +263,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
263263

264264
// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
265265
EXPECT_NEAR(
266-
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
267-
COMMON_THRESHOLD);
266+
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
267+
0.22222222222222224, COMMON_THRESHOLD);
268268
EXPECT_NEAR(
269-
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
270-
COMMON_THRESHOLD);
269+
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
270+
0.22222222222222224, COMMON_THRESHOLD);
271271
EXPECT_NEAR(
272-
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
273-
COMMON_THRESHOLD);
272+
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
273+
1.4179821977774734, COMMON_THRESHOLD);
274274
EXPECT_NEAR(
275-
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
276-
COMMON_THRESHOLD);
275+
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
276+
1.4179821977774734, COMMON_THRESHOLD);
277277

278278
subscribe_and_get_messages(msg);
279279

‎admittance_controller/src/admittance_controller.cpp‎

Lines changed: 36 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -528,21 +528,37 @@ void AdmittanceController::read_state_from_hardware(
528528
{
529529
if (has_position_state_interface_)
530530
{
531-
state_current.positions[joint_ind] =
532-
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
533-
nan_position |= std::isnan(state_current.positions[joint_ind]);
531+
const auto state_current_position_op =
532+
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_optional();
533+
nan_position |=
534+
!state_current_position_op.has_value() || std::isnan(state_current_position_op.value());
535+
if (state_current_position_op.has_value())
536+
{
537+
state_current.positions[joint_ind] = state_current_position_op.value();
538+
}
534539
}
535540
if (has_velocity_state_interface_)
536541
{
537-
state_current.velocities[joint_ind] =
538-
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
539-
nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
542+
auto state_current_velocity_op =
543+
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_optional();
544+
nan_velocity |=
545+
!state_current_velocity_op.has_value() || std::isnan(state_current_velocity_op.value());
546+
547+
if (state_current_velocity_op.has_value())
548+
{
549+
state_current.velocities[joint_ind] = state_current_velocity_op.value();
550+
}
540551
}
541552
if (has_acceleration_state_interface_)
542553
{
543-
state_current.accelerations[joint_ind] =
544-
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
545-
nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]);
554+
auto state_current_acceleration_op =
555+
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_optional();
556+
nan_acceleration |= !state_current_acceleration_op.has_value() ||
557+
std::isnan(state_current_acceleration_op.value());
558+
if (state_current_acceleration_op.has_value())
559+
{
560+
state_current.accelerations[joint_ind] = state_current_acceleration_op.value();
561+
}
546562
}
547563
}
548564

@@ -578,23 +594,31 @@ void AdmittanceController::write_state_to_hardware(
578594
size_t vel_ind =
579595
(has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind;
580596
size_t acc_ind = vel_ind + has_acceleration_command_interface_;
597+
598+
auto logger = get_node()->get_logger();
599+
581600
for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
582601
{
602+
bool success = true;
583603
if (has_position_command_interface_)
584604
{
585-
command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
605+
success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
586606
state_commanded.positions[joint_ind]);
587607
}
588608
if (has_velocity_command_interface_)
589609
{
590-
command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
610+
success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
591611
state_commanded.velocities[joint_ind]);
592612
}
593613
if (has_acceleration_command_interface_)
594614
{
595-
command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
615+
success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
596616
state_commanded.accelerations[joint_ind]);
597617
}
618+
if (!success)
619+
{
620+
RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind);
621+
}
598622
}
599623
last_commanded_ = state_commanded;
600624
}

‎bicycle_steering_controller/src/bicycle_steering_controller.cpp‎

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

6565
bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
6666
{
67+
auto logger = get_node()->get_logger();
68+
6769
if (params_.open_loop)
6870
{
6971
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
7072
}
7173
else
7274
{
73-
const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
74-
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
75+
const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional();
76+
const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional();
77+
78+
if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value())
79+
{
80+
RCLCPP_DEBUG(
81+
logger, "Unable to retrieve the data from the traction wheel or steering position!");
82+
return true;
83+
}
84+
85+
const double traction_wheel_value = traction_wheel_value_op.value();
86+
const double steering_position = steering_position_op.value();
87+
7588
if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
7689
{
7790
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_.readFromRT()))->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_.readFromRT()))->twist.linear.x));
@@ -245,9 +247,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
245247
controller_interface::return_type::OK);
246248

247249
EXPECT_NEAR(
248-
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
250+
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
251+
COMMON_THRESHOLD);
249252
EXPECT_NEAR(
250-
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
253+
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
251254
COMMON_THRESHOLD);
252255

253256
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
@@ -645,11 +645,15 @@ void DiffDriveController::reset_buffers()
645645

646646
void DiffDriveController::halt()
647647
{
648-
const auto halt_wheels = [](auto & wheel_handles)
648+
auto logger = get_node()->get_logger();
649+
const auto halt_wheels = [&](auto & wheel_handles)
649650
{
650651
for (const auto & wheel_handle : wheel_handles)
651652
{
652-
wheel_handle.velocity.get().set_value(0.0);
653+
if (!wheel_handle.velocity.get().set_value(0.0))
654+
{
655+
RCLCPP_WARN(logger, "Failed to set wheel velocity to value 0.0");
656+
}
653657
}
654658
};
655659

‎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)