Skip to content
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

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

Open
wants to merge 13 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
13 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
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 @@ -56,18 +56,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 @@ -143,9 +143,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 @@ -176,17 +176,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 @@ -216,17 +216,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 @@ -266,17 +266,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 @@ -526,21 +526,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 @@ -576,23 +592,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 @@ -54,14 +54,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 @@ -127,9 +127,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 @@ -159,9 +159,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 @@ -191,9 +192,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 @@ -246,9 +248,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
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;
}

return ret;
Expand Down
Loading
Loading