Skip to content

Use new handles API in ros2_controllers to fix deprecation warnings #1566

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 28 commits into
base: master
Choose a base branch
from
Open

Use new handles API in ros2_controllers to fix deprecation warnings #1566

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
0bf0670
Fix deprecation warnings with use of new handles API in ackermann_ste…
kumar-sanjeeev Mar 2, 2025
1466fdc
Fix deprecation warnings with use of new handles API in bicycle_steer…
kumar-sanjeeev Mar 3, 2025
a447f31
Fix typo
kumar-sanjeeev Mar 3, 2025
d356e08
Fix deprecation warnings with use of new handles API in admittance_co…
kumar-sanjeeev Mar 5, 2025
e4f6aef
Fix deprecation warnings with use of new handles API in effort_contro…
kumar-sanjeeev Mar 5, 2025
44dcfcb
Fix deprecation warnings with use of new handles API in force_torque_…
kumar-sanjeeev Mar 5, 2025
39200aa
Fix deprecation warnings with use of new handles API in forward_comma…
kumar-sanjeeev Mar 5, 2025
0c044b8
Fix deprecation warnings with use of new handles API in gpio_controllers
kumar-sanjeeev Mar 5, 2025
3b4685e
Add more descriptive message for warning
kumar-sanjeeev Mar 5, 2025
12d9639
Change return from ERROR to SUCCESS to make it work in sychronous cal…
kumar-sanjeeev Mar 5, 2025
9592271
Fix deprecation warnings with use of new handles API in gripper_contr…
kumar-sanjeeev Mar 5, 2025
704096b
Fix deprecation warnings with use of new handles API in joint_state_b…
kumar-sanjeeev Mar 5, 2025
214dcd1
Fix deprecation warnings with use of new handles API in mecanum_drive…
kumar-sanjeeev Mar 5, 2025
eaec22b
Fix deprecation warnings with use of new handles API in parallel_grip…
kumar-sanjeeev Mar 5, 2025
a64136a
Fix deprecation warnings with use of new handles API in tricycle_cont…
kumar-sanjeeev Mar 7, 2025
bf43abd
Merge branch 'ros-controls:master' into fix_warnings_in_controllers/n…
kumar-sanjeeev Mar 15, 2025
663a131
Fix deprecation warnings with use of new handles API in velocity_comt…
kumar-sanjeeev Mar 16, 2025
2ef6d10
Fix deprecation warnings with use of new handles API in pid_controller
kumar-sanjeeev Mar 31, 2025
6628b14
Fix deprecation warnings with use of new handles API in mecanum_drive…
kumar-sanjeeev Mar 31, 2025
31d574c
Fix deprecation warnings with use of new handles API in tricycle_stee…
kumar-sanjeeev Mar 31, 2025
85a06ce
Merge branch 'master' into fix_warnings_in_controllers/new_api
christophfroehlich Apr 16, 2025
116cf22
Merge remote-tracking branch 'upstream/master' into fix_warnings_in_c…
kumar-sanjeeev May 11, 2025
96736c6
Fix deprecation warnings with use of new handles API in steering_cont…
May 11, 2025
9af0948
Discard implemented changes to resolve conflict
kumar-sanjeeev May 11, 2025
5d5567e
Fix deprecation warnings with use of new handles API in position_cont…
kumar-sanjeeev May 11, 2025
764ff05
Fix deprecation warnings with use of new handles API in diff_drive_co…
kumar-sanjeeev May 11, 2025
374538f
Fix [partially] deprecation warnings with use of new handles API in j…
kumar-sanjeeev May 11, 2025
4a45a4b
Merge remote-tracking branch 'origin/master' into fix_warnings_in_con…
christophfroehlich May 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 27 additions & 6 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,18 +114,39 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom

bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
{
auto logger = get_node()->get_logger();

if (params_.open_loop)
{
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
}
else
{
const double traction_right_wheel_value =
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
const double traction_left_wheel_value =
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
const auto traction_right_wheel_value_op =
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional();
const auto traction_left_wheel_value_op =
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional();
const auto steering_right_position_op =
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional();
const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional();

if (
!traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() ||
!steering_right_position_op.has_value() || !steering_left_position_op.has_value())
{
RCLCPP_DEBUG(
logger,
"Unable to retrieve the data from right wheel or left wheel or right steering position or "
"left steering position!");

return true;
}

const double traction_right_wheel_value = traction_right_wheel_value_op.value();
const double traction_left_wheel_value = traction_left_wheel_value_op.value();
const double steering_right_position = steering_right_position_op.value();
const double steering_left_position = steering_left_position_op.value();

if (
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
Expand Down
52 changes: 26 additions & 26 deletions ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));

ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
Expand Down Expand Up @@ -173,17 +173,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
Expand Down Expand Up @@ -213,17 +213,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
Expand Down Expand Up @@ -263,17 +263,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat

// we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
0.22222222222222224, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
1.4179821977774734, COMMON_THRESHOLD);

subscribe_and_get_messages(msg);

Expand Down
48 changes: 36 additions & 12 deletions admittance_controller/src/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -528,21 +528,37 @@ void AdmittanceController::read_state_from_hardware(
{
if (has_position_state_interface_)
{
state_current.positions[joint_ind] =
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
nan_position |= std::isnan(state_current.positions[joint_ind]);
auto state_current_position_op =
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_optional();
nan_position |=
!state_current_position_op.has_value() || std::isnan(state_current_position_op.has_value());
if (state_current_position_op.has_value())
{
state_current.positions[joint_ind] = state_current_position_op.value();
}
}
if (has_velocity_state_interface_)
{
state_current.velocities[joint_ind] =
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
auto state_current_velocity_op =
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_optional();
nan_velocity |=
!state_current_velocity_op.has_value() || std::isnan(state_current_velocity_op.value());

if (state_current_velocity_op.has_value())
{
state_current.velocities[joint_ind] = state_current_velocity_op.value();
}
}
if (has_acceleration_state_interface_)
{
state_current.accelerations[joint_ind] =
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]);
auto state_current_acceleration_op =
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_optional();
nan_acceleration |= !state_current_acceleration_op.has_value() ||
std::isnan(state_current_acceleration_op.has_value());
if (state_current_acceleration_op.has_value())
{
state_current.accelerations[joint_ind] = state_current_acceleration_op.value();
}
}
}

Expand Down Expand Up @@ -578,23 +594,31 @@ void AdmittanceController::write_state_to_hardware(
size_t vel_ind =
(has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind;
size_t acc_ind = vel_ind + has_acceleration_command_interface_;

auto logger = get_node()->get_logger();

for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
{
bool success = true;
if (has_position_command_interface_)
{
command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
state_commanded.positions[joint_ind]);
}
if (has_velocity_command_interface_)
{
command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
state_commanded.velocities[joint_ind]);
}
if (has_acceleration_command_interface_)
{
command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
state_commanded.accelerations[joint_ind]);
}
if (!success)
{
RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind);
}
}
last_commanded_ = state_commanded;
}
Expand Down
17 changes: 15 additions & 2 deletions bicycle_steering_controller/src/bicycle_steering_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,14 +64,27 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet

bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
{
auto logger = get_node()->get_logger();

if (params_.open_loop)
{
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
}
else
{
const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional();
const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional();

if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value())
{
RCLCPP_DEBUG(
logger, "Unable to retrieve the data from the traction wheel or steering position!");
return true;
}

const double traction_wheel_value = traction_wheel_value_op.value();
const double steering_position = steering_position_op.value();

if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
{
if (params_.position_feedback)
Expand Down
19 changes: 11 additions & 8 deletions bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,9 +126,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success)
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));

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

EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
COMMON_THRESHOLD);

EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
Expand Down Expand Up @@ -190,9 +191,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
controller_interface::return_type::OK);

EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
COMMON_THRESHOLD);

EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
Expand Down Expand Up @@ -245,9 +247,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
controller_interface::return_type::OK);

EXPECT_NEAR(
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
COMMON_THRESHOLD);
EXPECT_NEAR(
controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
COMMON_THRESHOLD);

subscribe_and_get_messages(msg);
Expand Down
8 changes: 6 additions & 2 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -578,11 +578,15 @@ void DiffDriveController::reset_buffers()

void DiffDriveController::halt()
{
const auto halt_wheels = [](auto & wheel_handles)
auto logger = get_node()->get_logger();
const auto halt_wheels = [&](auto & wheel_handles)
{
for (const auto & wheel_handle : wheel_handles)
{
wheel_handle.velocity.get().set_value(0.0);
if (!wheel_handle.velocity.get().set_value(0.0))
{
RCLCPP_WARN(logger, "Failed to set wheel velocity to value 0.0");
}
}
};

Expand Down
7 changes: 5 additions & 2 deletions effort_controllers/src/joint_group_effort_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,14 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(
const rclcpp_lifecycle::State & previous_state)
{
auto ret = ForwardCommandController::on_deactivate(previous_state);

// stop all joints
for (auto & command_interface : command_interfaces_)
{
command_interface.set_value(0.0);
if (!command_interface.set_value(0.0))
{
RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value to 0.0");
}
return controller_interface::CallbackReturn::SUCCESS;
Comment on lines +61 to +65
Copy link
Contributor

@christophfroehlich christophfroehlich May 17, 2025 •
edited by saikishor
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this should go into the if branch, right?

Suggested change
if (!command_interface.set_value(0.0))
{
RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value to 0.0");
}
return controller_interface::CallbackReturn::SUCCESS;
if (!command_interface.set_value(0.0))
{
RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0");
return controller_interface::CallbackReturn::SUCCESS;
}

}

return ret;
Expand Down
Loading
Loading