From 0bf0670461c0b83d0716e79fe1e48c1921f45140 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 2 Mar 2025 23:14:07 +0100 Subject: [PATCH 01/68] Fix deprecation warnings with use of new handles API in ackermann_steering_controller --- .../src/ackermann_steering_controller.cpp | 33 +++++++++--- .../test_ackermann_steering_controller.cpp | 52 +++++++++---------- 2 files changed, 53 insertions(+), 32 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 77ba55812a..436c3a5b0d 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -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_STEER_RIGHT_WHEEL].get_optional(); + const auto traction_left_wheel_value_op = + state_interfaces_[STATE_STEER_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)) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 15eb39f1a4..af8af22d7c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -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)), @@ -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()); @@ -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()); @@ -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); From 1466fdc4e4d0b0506262c9e22d6e9e8e033d7fb6 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 3 Mar 2025 20:20:21 +0100 Subject: [PATCH 02/68] Fix deprecation warnings with use of new handles API in bicycle_steering_controller --- .../src/bicycle_steering_controller.cpp | 17 +++++++++++++++-- .../test/test_bicycle_steering_controller.cpp | 19 +++++++++++-------- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 95eaf1965c..db8fa2f76f 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -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) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 9c41f5fd47..11a86ec01e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -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)), @@ -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)); @@ -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)); @@ -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); From a447f3138dc4b660c774a41402f067cf8f9ba77a Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 3 Mar 2025 20:30:24 +0100 Subject: [PATCH 03/68] Fix typo --- .../src/ackermann_steering_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 436c3a5b0d..a0d99f755d 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -65,9 +65,9 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio else { const auto traction_right_wheel_value_op = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional(); + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); const auto traction_left_wheel_value_op = - state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional(); + 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(); From d356e0876c8a69b22c5bb857dd58f46e2d63180c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 12:46:33 +0100 Subject: [PATCH 04/68] Fix deprecation warnings with use of new handles API in admittance_controller --- .../src/admittance_controller.cpp | 48 ++++++++++++++----- 1 file changed, 36 insertions(+), 12 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b4c0586473..deb228c0c8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -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(); + } } } @@ -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; } From e4f6aef86cc4cb3fe62f822cb16630531ec6cf3a Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 13:07:44 +0100 Subject: [PATCH 05/68] Fix deprecation warnings with use of new handles API in effort_controllers --- .../src/joint_group_effort_controller.cpp | 7 ++- .../test_joint_group_effort_controller.cpp | 48 +++++++++---------- 2 files changed, 29 insertions(+), 26 deletions(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index e7d0f274a5..652a03be66 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -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"); + } + return controller_interface::CallbackReturn::ERROR; } return ret; diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index c19cbff9cf..de1f0eb1e5 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -105,9 +105,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -120,9 +120,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -142,9 +142,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -159,9 +159,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -170,9 +170,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -203,9 +203,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -217,15 +217,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); } From 44dcfcb640d22275d32a6eefdea8da621ca58ff0 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 15:02:29 +0100 Subject: [PATCH 06/68] Fix deprecation warnings with use of new handles API in force_torque_sensor_broadcaster tests --- .../test/test_force_torque_sensor_broadcaster.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 46c396f7b1..89de8cd3f1 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -335,7 +335,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets ASSERT_EQ( exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_); ASSERT_EQ( - exported_state_interfaces[i]->get_value(), + exported_state_interfaces[i]->get_optional().value(), sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); } } @@ -373,8 +373,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name); ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z"); - ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); - ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); + ASSERT_EQ(exported_state_interfaces[0]->get_optional().value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_optional().value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -423,7 +423,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) for (size_t i = 0; i < 6; ++i) { ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); - ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + ASSERT_EQ(exported_state_interfaces[i]->get_optional().value(), sensor_values_[i]); } } From 39200aac4441b75b300e200233cbfa2579c61f5b Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 15:33:41 +0100 Subject: [PATCH 07/68] Fix deprecation warnings with use of new handles API in forward_command_controller --- .../src/forward_controllers_base.cpp | 11 +++- .../test/test_forward_command_controller.cpp | 60 +++++++++---------- ...i_interface_forward_command_controller.cpp | 48 +++++++-------- 3 files changed, 63 insertions(+), 56 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 331d3f9eea..86c4ba74de 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -118,6 +118,8 @@ controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + auto logger = get_node()->get_logger(); + auto joint_commands = rt_command_ptr_.readFromRT(); // no command received yet @@ -129,7 +131,7 @@ controller_interface::return_type ForwardControllersBase::update( if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *(get_node()->get_clock()), 1000, + logger, *(get_node()->get_clock()), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; @@ -137,7 +139,12 @@ controller_interface::return_type ForwardControllersBase::update( for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value((*joint_commands)->data[index]); + if (!command_interfaces_[index].set_value((*joint_commands)->data[index])) + { + RCLCPP_WARN(logger, "Error while setting the command interface value"); + } + + return controller_interface::return_type::OK; } return controller_interface::return_type::OK; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index fcfa65ee1c..3c1886bcd2 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -212,9 +212,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -227,9 +227,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -255,9 +255,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -277,9 +277,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -290,9 +290,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -323,9 +323,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -336,9 +336,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -373,9 +373,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -425,9 +425,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -438,7 +438,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 7.7); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index c52f4f3ab6..7dcf1f35c5 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -276,9 +276,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -304,9 +304,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -355,9 +355,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -368,7 +368,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 7.7); } From 0c044b8c4ec325964b123dedb4482041cadfc58c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 17:53:58 +0100 Subject: [PATCH 08/68] Fix deprecation warnings with use of new handles API in gpio_controllers --- .../src/gpio_command_controller.cpp | 25 +++++++++-- .../test/test_gpio_command_controller.cpp | 42 +++++++++---------- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fdecf71c30..f7d6353578 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -350,11 +350,18 @@ void GpioCommandController::apply_command( const auto full_command_interface_name = gpio_commands.interface_groups[gpio_index] + '/' + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + + const auto & command_value = + gpio_commands.interface_values[gpio_index].values[command_interface_index]; + try { - command_interfaces_map_.at(full_command_interface_name) - .get() - .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + if (!command_interfaces_map_.at(full_command_interface_name).get().set_value(command_value)) + { + RCLCPP_WARN( + get_node()->get_logger(), "Error while setting command for interface '%s' with value '%f'.", + full_command_interface_name.c_str(), command_value); + } } catch (const std::exception & e) { @@ -394,8 +401,18 @@ void GpioCommandController::apply_state_value( state_msg.interface_values[gpio_index].interface_names[interface_index]; try { + auto state_msg_interface_value_op = + state_interfaces_map_.at(interface_name).get().get_optional(); + + if (!state_msg_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the data from state: %s \n", + interface_name.c_str()); + } + state_msg.interface_values[gpio_index].values[interface_index] = - state_interfaces_map_.at(interface_name).get().get_value(); + state_msg_interface_value_op.value(); } catch (const std::exception & e) { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index ec0e0f1228..885ee42a46 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -127,12 +127,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test void assert_default_command_and_state_values() { - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); - ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); - ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); - ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_optional().value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_optional().value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_optional().value(), gpio_states.at(2)); } void update_controller_loop() @@ -482,9 +482,9 @@ TEST_F( controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F( @@ -505,9 +505,9 @@ TEST_F( controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F( @@ -528,9 +528,9 @@ TEST_F( controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands[2]); } TEST_F( @@ -552,9 +552,9 @@ TEST_F( controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); } TEST_F( @@ -578,9 +578,9 @@ TEST_F( wait_one_miliseconds_for_timeout(); update_controller_loop(); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) From 3b4685e8e2358aa3ec3f559a5eb8907d6a4deaa7 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 18:09:17 +0100 Subject: [PATCH 09/68] Add more descriptive message for warning --- .../src/forward_controllers_base.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 86c4ba74de..1522e767f6 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -137,11 +137,16 @@ controller_interface::return_type ForwardControllersBase::update( return controller_interface::return_type::ERROR; } + const auto & data = (*joint_commands)->data; + for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - if (!command_interfaces_[index].set_value((*joint_commands)->data[index])) + if (!command_interfaces_[index].set_value(data[index])) { - RCLCPP_WARN(logger, "Error while setting the command interface value"); + RCLCPP_WARN( + logger, "Error while setting command interface value at index %zu: value = %f", index, + data[index]); + return controller_interface::return_type::OK; } return controller_interface::return_type::OK; From 12d96398923540daea95d452c4f3e518ad7bcf9a Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 18:23:59 +0100 Subject: [PATCH 10/68] Change return from ERROR to SUCCESS to make it work in sychronous call, without failures --- effort_controllers/src/joint_group_effort_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 652a03be66..b0e2890ec1 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -60,9 +60,9 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( { if (!command_interface.set_value(0.0)) { - RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value"); + RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value to 0.0"); } - return controller_interface::CallbackReturn::ERROR; + return controller_interface::CallbackReturn::SUCCESS; } return ret; From 9592271f63b2c0b4e2cde0495aea5197fb2e1515 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 20:10:32 +0100 Subject: [PATCH 11/68] Fix deprecation warnings with use of new handles API in gripper_controllers --- .../gripper_action_controller_impl.hpp | 31 ++++++++++++++++--- .../hardware_interface_adapter.hpp | 23 +++++++++++--- 2 files changed, 46 insertions(+), 8 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 034f7e43dd..e40b1cdfce 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -65,8 +65,19 @@ controller_interface::return_type GripperActionController::up { command_struct_rt_ = *(command_.readFromRT()); - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const auto current_position_op = joint_position_state_interface_->get().get_optional(); + const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional(); + + if (!current_position_op.has_value() || !current_velocity_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve the data from current position or current velocity"); + return controller_interface::return_type::OK; + } + + const double current_position = current_position_op.value(); + const double current_velocity = current_velocity_op.value(); const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; @@ -147,7 +158,12 @@ rclcpp_action::CancelResponse GripperActionController::cancel template void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + const auto command_struct_position_op = joint_position_state_interface_->get().get_optional(); + if (!command_struct_position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); + } + command_struct_.position_ = command_struct_position_op.value(); command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -287,7 +303,14 @@ controller_interface::CallbackReturn GripperActionController: hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + auto command_struct_position_op = joint_position_state_interface_->get().get_optional(); + if (!command_struct_position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve the data about joint position"); + return controller_interface::CallbackReturn::SUCCESS; + } + + command_struct_.position_ = command_struct_position_op.value(); command_struct_.max_effort_ = params_.max_effort; command_.initRT(command_struct_); diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 66da3d869a..2ae1ba911a 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -69,9 +69,10 @@ class HardwareInterfaceAdapter public: bool init( std::optional> joint_handle, - const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { joint_handle_ = joint_handle; + node_ = node; return true; } @@ -83,12 +84,18 @@ class HardwareInterfaceAdapter double /* error_velocity */, double max_allowed_effort) { // Forward desired position to command - joint_handle_->get().set_value(desired_position); + if (!joint_handle_->get().set_value(desired_position)) + { + RCLCPP_WARN( + node_->get_logger(), "Error while setting desired position to value = %lf", + desired_position); + } return max_allowed_effort; } private: std::optional> joint_handle_; + std::shared_ptr node_; }; /** @@ -129,6 +136,7 @@ class HardwareInterfaceAdapter const std::shared_ptr & node) { joint_handle_ = joint_handle; + node_ = node; // Init PID gains from ROS parameter server const std::string prefix = "gains." + joint_handle_->get().get_prefix_name(); const auto k_p = auto_declare(node, prefix + ".p", 0.0); @@ -148,7 +156,10 @@ class HardwareInterfaceAdapter } // Reset PIDs, zero effort commands pid_->reset(); - joint_handle_->get().set_value(0.0); + if (!joint_handle_->get().set_value(0.0)) + { + RCLCPP_WARN(node_->get_logger(), "Error while setting joint handle to value 0.0"); + } } void stopping(const rclcpp::Time & /* time */) {} @@ -168,7 +179,10 @@ class HardwareInterfaceAdapter double command = pid_->compute_command(error_position, error_velocity, period); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); - joint_handle_->get().set_value(command); + if (!joint_handle_->get().set_value(command)) + { + RCLCPP_WARN(node_->get_logger(), "Error while setting command value = %lf", command); + } last_update_time_ = std::chrono::steady_clock::now(); return command; } @@ -178,6 +192,7 @@ class HardwareInterfaceAdapter PidPtr pid_; std::optional> joint_handle_; std::chrono::steady_clock::time_point last_update_time_; + std::shared_ptr node_; }; #endif // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_ From 704096b6b41f82e82aa29f8dc67f5ba2a292526d Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 21:06:04 +0100 Subject: [PATCH 12/68] Fix deprecation warnings with use of new handles API in joint_state_broadcaster --- .../src/joint_state_broadcaster.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5a9d1caeef..993385a2e3 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -352,11 +352,18 @@ controller_interface::return_type JointStateBroadcaster::update( { interface_name = map_interface_to_joint_state_[interface_name]; } + const auto state_interface_op = state_interface.get_optional(); + if (!state_interface_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve data from state interface %s: ", state_interface.get_name().c_str()); + } name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = - state_interface.get_value(); + state_interface_op.value(); RCLCPP_DEBUG( get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), - state_interface.get_value()); + state_interface_op.value()); } if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) From 214dcd1b8ebd2d69c7d7683d0471bdedaa14c9ea Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 22:06:52 +0100 Subject: [PATCH 13/68] Fix deprecation warnings with use of new handles API in mecanum_drive_controller --- .../src/mecanum_drive_controller.cpp | 53 ++++++++++++++----- .../test/test_mecanum_drive_controller.cpp | 12 ++--- 2 files changed, 47 insertions(+), 18 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 93e607bd08..867e1f4a87 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -375,10 +375,26 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma const rclcpp::Time & time, const rclcpp::Duration & period) { // FORWARD KINEMATICS (odometry). - const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); - const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); - const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); - const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + const auto wheel_front_left_state_vel_op = state_interfaces_[FRONT_LEFT].get_optional(); + const auto wheel_front_right_state_vel_op = state_interfaces_[FRONT_RIGHT].get_optional(); + const auto wheel_rear_right_state_vel_op = state_interfaces_[REAR_RIGHT].get_optional(); + const auto wheel_rear_left_state_vel_op = state_interfaces_[REAR_LEFT].get_optional(); + + if ( + !wheel_front_left_state_vel_op.has_value() || !wheel_front_right_state_vel_op.has_value() || + !wheel_rear_right_state_vel_op.has_value() || !wheel_front_left_state_vel_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve data from front left wheel or front right wheel or rear left wheel or " + "rear right wheel"); + return controller_interface::return_type::OK; + } + + const double wheel_front_left_state_vel = wheel_front_left_state_vel_op.value(); + const double wheel_front_right_state_vel = wheel_front_right_state_vel_op.value(); + const double wheel_rear_right_state_vel = wheel_rear_right_state_vel_op.value(); + const double wheel_rear_left_state_vel = wheel_rear_left_state_vel_op.value(); if ( !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && @@ -497,14 +513,27 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.front_left_wheel_velocity = - state_interfaces_[FRONT_LEFT].get_value(); - controller_state_publisher_->msg_.front_right_wheel_velocity = - state_interfaces_[FRONT_RIGHT].get_value(); - controller_state_publisher_->msg_.back_right_wheel_velocity = - state_interfaces_[REAR_RIGHT].get_value(); - controller_state_publisher_->msg_.back_left_wheel_velocity = - state_interfaces_[REAR_LEFT].get_value(); + const auto front_left_op = state_interfaces_[FRONT_LEFT].get_optional(); + const auto front_right_op = state_interfaces_[FRONT_RIGHT].get_optional(); + const auto rear_right_op = state_interfaces_[REAR_RIGHT].get_optional(); + const auto rear_left_op = state_interfaces_[REAR_LEFT].get_optional(); + + if ( + !front_left_op.has_value() || !front_right_op.has_value() || !rear_right_op.has_value() || + !rear_left_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve the data of state interface of front left or front right or rear left " + "or rear right"); + return controller_interface::return_type::OK; + } + + controller_state_publisher_->msg_.front_left_wheel_velocity = front_left_op.value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = front_right_op.value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = rear_right_op.value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = rear_left_op.value(); + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 079a431e03..6d295423d2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -161,11 +161,11 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value())); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -383,7 +383,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 0.0); } std::shared_ptr msg_2 = std::make_shared(); @@ -422,7 +422,7 @@ TEST_F( for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 3.0); } } @@ -481,7 +481,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 6.0); } } From eaec22bb1991f0b0a27637ea8847f59e2d612f8c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Wed, 5 Mar 2025 22:46:27 +0100 Subject: [PATCH 14/68] Fix deprecation warnings with use of new handles API in parallel_gripper_controller --- ...arallel_gripper_action_controller_impl.hpp | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 5b7e760ca2..8380798031 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -58,21 +58,47 @@ controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { command_struct_rt_ = *(command_.readFromRT()); + auto logger = get_node()->get_logger(); + + const auto current_position_op = joint_position_state_interface_->get().get_optional(); + const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional(); + + if (!current_position_op.has_value() || !current_velocity_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve data of current position or current velocity"); + return controller_interface::return_type::OK; + } + + const double current_position = current_position_op.value(); + const double current_velocity = current_velocity_op.value(); - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); const double error_position = command_struct_rt_.position_cmd_ - current_position; check_for_success(get_node()->now(), error_position, current_position, current_velocity); - joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); - if (speed_interface_.has_value()) + if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) + { + RCLCPP_WARN( + logger, "Error while setting joint position command to: %f", + command_struct_rt_.position_cmd_); + return controller_interface::return_type::OK; + } + if ( + speed_interface_.has_value() && + !speed_interface_->get().set_value(command_struct_rt_.max_velocity_)) { - speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + RCLCPP_WARN( + logger, "Error while setting speed command to: %f", command_struct_rt_.max_velocity_); + + return controller_interface::return_type::OK; } - if (effort_interface_.has_value()) + if ( + effort_interface_.has_value() && + !effort_interface_->get().set_value(command_struct_rt_.max_effort_)) { - effort_interface_->get().set_value(command_struct_rt_.max_effort_); + RCLCPP_WARN( + logger, "Error while setting effort command to: %f", command_struct_rt_.max_effort_); + return controller_interface::return_type::OK; } return controller_interface::return_type::OK; @@ -168,7 +194,12 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( void GripperActionController::set_hold_position() { - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + const auto position_op = joint_position_state_interface_->get().get_optional(); + if (!position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); + } + command_struct_.position_cmd_ = position_op.value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; command_.writeFromNonRT(command_struct_); @@ -318,7 +349,12 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } // Command - non RT version - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + const auto position_op = joint_position_state_interface_->get().get_optional(); + if (!position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); + } + command_struct_.position_cmd_ = position_op.value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; command_.initRT(command_struct_); From a64136a69f90314e36513fc0965ac401fd7af218 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Fri, 7 Mar 2025 19:22:36 +0100 Subject: [PATCH 15/68] Fix deprecation warnings with use of new handles API in tricycle_controller --- .../src/tricycle_controller.cpp | 43 ++++++++++++++++--- .../test/test_tricycle_controller.cpp | 26 +++++------ 2 files changed, 51 insertions(+), 18 deletions(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 02f5449b4c..88a62f35a8 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -110,8 +110,19 @@ controller_interface::return_type TricycleController::update( TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; - double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s - double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + auto Ws_read_op = traction_joint_[0].velocity_state.get().get_optional(); + auto alpha_read_op = steering_joint_[0].position_state.get().get_optional(); + + if (!Ws_read_op.has_value() || !alpha_read_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve the data of traction joint velocity or steering joint position"); + return controller_interface::return_type::OK; + } + + double Ws_read = Ws_read_op.value(); // in radians/s + double alpha_read = alpha_read_op.value(); // in radians if (params_.open_loop) { @@ -211,8 +222,18 @@ controller_interface::return_type TricycleController::update( realtime_ackermann_command_publisher_->unlockAndPublish(); } - traction_joint_[0].velocity_command.get().set_value(Ws_write); - steering_joint_[0].position_command.get().set_value(alpha_write); + if (!traction_joint_[0].velocity_command.get().set_value(Ws_write)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Error while setting velocity command for traction joint to value: '%f'.", Ws_write); + } + if (!steering_joint_[0].position_command.get().set_value(alpha_write)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Error while setting position command for steering joint to value: '%f'.", alpha_write); + } return controller_interface::return_type::OK; } @@ -433,8 +454,18 @@ bool TricycleController::reset() void TricycleController::halt() { - traction_joint_[0].velocity_command.get().set_value(0.0); - steering_joint_[0].position_command.get().set_value(0.0); + if (!traction_joint_[0].velocity_command.get().set_value(0.0)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Error while setting velocity command for traction joint to value 0.0"); + } + if (!steering_joint_[0].position_command.get().set_value(0.0)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Error while setting position command for steering joint to value 0.0"); + } } CallbackReturn TricycleController::get_traction( diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index d8d4f2cf63..f084da12a5 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -284,15 +284,15 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); executor.cancel(); } @@ -312,8 +312,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -328,8 +328,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_optional().value()); // deactivated // wait so controller process the second point when deactivated @@ -340,14 +340,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); From 663a131454d89fc83ed725cb7f3361262d93b5a1 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 16 Mar 2025 08:18:28 +0100 Subject: [PATCH 16/68] Fix deprecation warnings with use of new handles API in velocity_comtrollers --- .../src/joint_group_velocity_controller.cpp | 6 ++- .../test_joint_group_velocity_controller.cpp | 48 +++++++++---------- 2 files changed, 29 insertions(+), 25 deletions(-) diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 97f6713d72..4e4bb18db4 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -59,7 +59,11 @@ controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate // 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 to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; + } } return ret; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 31ba67ec60..1346b3fe70 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -106,9 +106,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -121,9 +121,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -143,9 +143,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -160,9 +160,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -171,9 +171,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,9 +204,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -218,15 +218,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); } From 2ef6d102d35b1e5b0391d38d2ddb6e8d967b8b76 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 31 Mar 2025 14:06:23 +0200 Subject: [PATCH 17/68] Fix deprecation warnings with use of new handles API in pid_controller --- pid_controller/src/pid_controller.cpp | 22 ++++++++++-- pid_controller/test/test_pid_controller.cpp | 34 +++++++++++-------- .../test_pid_controller_dual_interface.cpp | 4 +-- 3 files changed, 42 insertions(+), 18 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 782d39dfdc..0199b6b7e2 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -494,7 +494,15 @@ controller_interface::return_type PidController::update_and_write_commands( { for (size_t i = 0; i < measured_state_values_.size(); ++i) { - measured_state_values_[i] = state_interfaces_[i].get_value(); + const auto state_interface_value_op = state_interfaces_[i].get_optional(); + if (!state_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the state interface value for %s", + state_interfaces_[i].get_name().c_str()); + return controller_interface::return_type::OK; + } + measured_state_values_[i] = state_interface_value_op.value(); } } @@ -599,7 +607,17 @@ controller_interface::return_type PidController::update_and_write_commands( state_publisher_->msg_.dof_states[i].time_step = period.seconds(); // Command can store the old calculated values. This should be obvious because at least one // another value is NaN. - state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + const auto command_interface_value_op = command_interfaces_[i].get_optional(); + + if (!command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the command interface value for %s", + command_interfaces_[i].get_name().c_str()); + return controller_interface::return_type::OK; + } + + state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value(); } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 1c0494a002..14db82f0e3 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -181,15 +181,15 @@ TEST_F(PidControllerTest, reactivate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -272,7 +272,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } @@ -331,7 +332,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30102.301020; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } @@ -388,7 +390,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 const double expected_command_value = 1173.978; - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -415,7 +417,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // check the result of the commands - the values are not wrapped const double expected_command_value = 2679.078; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } /** @@ -446,7 +449,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // Check the command value const double expected_command_value = 787.713559; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -583,7 +587,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -641,7 +645,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -677,7 +681,8 @@ TEST_F(PidControllerTest, test_save_i_term_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -690,7 +695,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 0.0, 1e-5); } @@ -727,7 +732,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -740,7 +746,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index e006986473..f5b1442a57 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -96,8 +96,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i // check the commands const double joint1_expected_cmd = 8.915; const double joint2_expected_cmd = 9.915; - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); - ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd); } int main(int argc, char ** argv) From 6628b1477df4529a68187ba66ecc3cce8d6afdb6 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 31 Mar 2025 14:11:14 +0200 Subject: [PATCH 18/68] Fix deprecation warnings with use of new handles API in mecanum_drive_controller --- .../test/test_mecanum_drive_controller.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 7a84397f10..6cb0364b28 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -632,10 +632,14 @@ TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) size_t fr_index = controller_->get_front_right_wheel_index(); size_t rl_index = controller_->get_rear_left_wheel_index(); size_t rr_index = controller_->get_rear_right_wheel_index(); - joint_state_values_[fl_index] = controller_->command_interfaces_[fl_index].get_value(); - joint_state_values_[fr_index] = controller_->command_interfaces_[fr_index].get_value(); - joint_state_values_[rl_index] = controller_->command_interfaces_[rl_index].get_value(); - joint_state_values_[rr_index] = controller_->command_interfaces_[rr_index].get_value(); + joint_state_values_[fl_index] = + controller_->command_interfaces_[fl_index].get_optional().value(); + joint_state_values_[fr_index] = + controller_->command_interfaces_[fr_index].get_optional().value(); + joint_state_values_[rl_index] = + controller_->command_interfaces_[rl_index].get_optional().value(); + joint_state_values_[rr_index] = + controller_->command_interfaces_[rr_index].get_optional().value(); } RCLCPP_INFO( From 31d574cf75a33c004e9c2e64162a2633144c4188 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 31 Mar 2025 14:55:52 +0200 Subject: [PATCH 19/68] Fix deprecation warnings with use of new handles API in tricycle_steering_controller --- .../src/tricycle_steering_controller.cpp | 23 ++++++++++--- .../test_tricycle_steering_controller.cpp | 34 +++++++++---------- 2 files changed, 35 insertions(+), 22 deletions(-) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..a8adc8a13d 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,11 +60,24 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } 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_position = state_interfaces_[STATE_STEER_AXIS].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_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional(); + + if ( + !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || + !steering_position_op.value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve the data from right wheel or left wheel or 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_position = steering_position_op.value(); if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_position)) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 93c92c865f..50b14d5dc4 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -135,9 +135,9 @@ TEST_F(TricycleSteeringControllerTest, 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)), @@ -167,13 +167,13 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); 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_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)); @@ -204,13 +204,13 @@ TEST_F(TricycleSteeringControllerTest, 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_[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_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)); @@ -250,13 +250,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu // 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_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); From 96736c62041dfb8edd272f25f764e51b58f0afb4 Mon Sep 17 00:00:00 2001 From: ska Date: Sun, 11 May 2025 13:46:52 +0200 Subject: [PATCH 20/68] Fix deprecation warnings with use of new handles API in steering_controllers_library --- .../src/steering_controllers_library.cpp | 73 ++++++++++++++++--- .../test_steering_controllers_library.cpp | 16 ++-- 2 files changed, 72 insertions(+), 17 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3840e2803b..adf6e2a05a 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -533,7 +533,13 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( { for (size_t i = 0; i < nr_cmd_itfs_; ++i) { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + if (!command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to set NaN value for command interface '%s' (index %zu) during deactivation.", + command_interfaces_[i].get_name().c_str(), i); + } } return controller_interface::CallbackReturn::SUCCESS; } @@ -556,6 +562,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c const rclcpp::Time & time, const rclcpp::Duration & period) { update_odometry(period); + auto logger = get_node()->get_logger(); // MOVE ROBOT @@ -578,11 +585,25 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + const auto value = timeout ? 0.0 : traction_commands[i]; + + if (!command_interfaces_[i].set_value(value)) + { + RCLCPP_WARN( + logger, "Failed to set traction command %.3f for interface '%s' (index %zu).", value, + command_interfaces_[i].get_name().c_str(), i); + } } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { - command_interfaces_[i + params_.traction_joints_names.size()].set_value(steering_commands[i]); + const auto value = steering_commands[i]; + + if (!command_interfaces_[i + params_.traction_joints_names.size()].set_value(value)) + { + RCLCPP_WARN( + logger, "Failed to set steering command %.3f for interface '%s' (index %zu).", value, + command_interfaces_[i].get_name().c_str(), i); + } } } @@ -632,24 +653,58 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { if (params_.position_feedback) { + auto position_state_interface_op = state_interfaces_[i].get_optional(); + if (!position_state_interface_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve position feedback data for traction wheel %zu", i); + return controller_interface::return_type::OK; + } controller_state_publisher_->msg_.traction_wheels_position.push_back( - state_interfaces_[i].get_value()); + position_state_interface_op.value()); } else { + auto velocity_state_interface_op = state_interfaces_[i].get_optional(); + if (!velocity_state_interface_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve velocity feedback data for traction wheel %zu", i); + return controller_interface::return_type::OK; + } controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - state_interfaces_[i].get_value()); + velocity_state_interface_op.value()); + } + auto linear_velocity_command_interface_op = command_interfaces_[i].get_optional(); + if (!linear_velocity_command_interface_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); + return controller_interface::return_type::OK; } controller_state_publisher_->msg_.linear_velocity_command.push_back( - command_interfaces_[i].get_value()); + linear_velocity_command_interface_op.value()); } for (size_t i = 0; i < number_of_steering_wheels; ++i) { - controller_state_publisher_->msg_.steer_positions.push_back( - state_interfaces_[number_of_traction_wheels + i].get_value()); + const auto state_interface_value_op = + state_interfaces_[number_of_traction_wheels + i].get_optional(); + const auto command_interface_value_op = + command_interfaces_[number_of_traction_wheels + i].get_optional(); + if (!state_interface_value_op.has_value() || !command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve %s for steering wheel %zu", + !state_interface_value_op.has_value() ? "state interface value" + : "command interface value", + i); + + return controller_interface::return_type::OK; + } + controller_state_publisher_->msg_.steer_positions.push_back(state_interface_value_op.value()); controller_state_publisher_->msg_.steering_angle_command.push_back( - command_interfaces_[number_of_traction_wheels + i].get_value()); + command_interface_value_op.value()); } controller_state_publisher_->unlockAndPublish(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 26750777c9..b661b85671 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -146,12 +146,12 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // Wheel velocities should reset to 0 - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); // Steer angles should not reset - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -190,12 +190,12 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // Wheel velocities should reset to 0 - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); // Steer angles should not reset - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); } int main(int argc, char ** argv) From 9af094882b1dffe1d9f5a0796ba24d20dfceadf7 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 11 May 2025 13:54:45 +0200 Subject: [PATCH 21/68] Discard implemented changes to resolve conflict --- .../src/joint_state_broadcaster.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 0201c9147e..c6857cf245 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -411,18 +411,6 @@ controller_interface::return_type JointStateBroadcaster::update( { *mapped_values_[i] = opt.value(); } - const auto state_interface_op = state_interface.get_optional(); - if (!state_interface_op.has_value()) - { - RCLCPP_DEBUG( - get_node()->get_logger(), - "Unable to retrieve data from state interface %s: ", state_interface.get_name().c_str()); - } - name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = - state_interface_op.value(); - RCLCPP_DEBUG( - get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), - state_interface_op.value()); } if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) From 5d5567e241624d1a5ffe710ee2251e3ec16a052d Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 11 May 2025 13:58:26 +0200 Subject: [PATCH 22/68] Fix deprecation warnings with use of new handles API in position_controllers --- .../test_joint_group_position_controller.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index ea7005c543..380450a6d1 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -106,9 +106,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -121,9 +121,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -143,9 +143,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -160,9 +160,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -171,9 +171,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,7 +204,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } From 764ff05db729c59f937e934351a08ea277958cc6 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 11 May 2025 21:06:28 +0200 Subject: [PATCH 23/68] Fix deprecation warnings with use of new handles API in diff_drive_controllers --- diff_drive_controller/src/diff_drive_controller.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c3554dfecb..a94c8ef794 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -639,11 +639,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"); + } } }; From 374538f732792fe6c1cc1befe9a4e6a4ed95020d Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 11 May 2025 21:07:09 +0200 Subject: [PATCH 24/68] Fix [partially] deprecation warnings with use of new handles API in joint_trajectory_controllers --- .../src/joint_trajectory_controller.cpp | 92 +++++++++++++++---- 1 file changed, 72 insertions(+), 20 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6890cea55e..ba1b711362 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -431,12 +431,19 @@ controller_interface::return_type JointTrajectoryController::update( void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { + auto logger = get_node()->get_logger(); auto assign_point_from_state_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) { for (size_t index = 0; index < dof_; ++index) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + const auto joint_state_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_state_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve joint state interface value for joint at index %zu", index); + } + trajectory_point_interface[index] = joint_state_interface_value_op.value(); } }; auto assign_point_from_command_interface = @@ -447,8 +454,14 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory std::numeric_limits::quiet_NaN()); for (size_t index = 0; index < num_cmd_joints_; ++index) { + const auto joint_command_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve joint command interface value for joint at index %zu", index); + } trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + joint_command_interface_value_op.value(); } }; @@ -492,8 +505,14 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto { for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + const auto joint_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve value of joint interface for joint at index %zu", index); + } + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value(); } }; @@ -1018,8 +1037,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( resize_joint_trajectory_point(state, dof_); // read from cmd joints only if all joints have command interface // otherwise it leaves the entries of joints without command interface NaN. - // if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and - // future trajectory sampling will always give NaN for these joints + // if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` + // and future trajectory sampling will always give NaN for these joints if ( params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state)) @@ -1067,6 +1086,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { const auto active_goal = *rt_active_goal_.readFromNonRT(); + auto logger = get_node()->get_logger(); if (active_goal) { rt_has_pending_goal_.writeFromNonRT(false); @@ -1081,24 +1101,41 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( { if (has_position_command_interface_) { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + const auto joint_position_value_op = joint_command_interface_[0][index].get().get_optional(); + if (!joint_position_value_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve joint position value"); + return controller_interface::CallbackReturn::SUCCESS; + } + if (!joint_command_interface_[0][index].get().set_value(joint_position_value_op.value())) + ; + { + RCLCPP_WARN( + logger, "Error while setting joint position to value %f", + joint_position_value_op.value()); + return controller_interface::CallbackReturn::SUCCESS; + } } - if (has_velocity_command_interface_) + if (has_velocity_command_interface_ && !joint_command_interface_[1][index].get().set_value(0.0)) { - joint_command_interface_[1][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint velocity to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } - if (has_acceleration_command_interface_) + if ( + has_acceleration_command_interface_ && + !joint_command_interface_[2][index].get().set_value(0.0)) { - joint_command_interface_[2][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint acceleration to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } // TODO(anyone): How to halt when using effort commands? - if (has_effort_command_interface_) + if (has_effort_command_interface_ && !joint_command_interface_[3][index].get().set_value(0.0)) { - joint_command_interface_[3][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint effort to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } } @@ -1331,17 +1368,31 @@ void JointTrajectoryController::fill_partial_goal( // Assume hold position with 0 velocity and acceleration for missing joints if (!it.positions.empty()) { - if ( - has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) + const auto position_command_value_op = + joint_command_interface_[0][index].get().get_optional(); + if (!position_command_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position command value of joint at index %zu", index); + } + if (has_position_command_interface_) { // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + it.positions.push_back(position_command_value_op.has_value()); } else if (has_position_state_interface_) { // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + const auto position_state_value_op = + joint_state_interface_[0][index].get().get_optional(); + if (!position_state_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position state value of joint at index %zu", index); + } + it.positions.push_back(position_state_value_op.value()); } } if (!it.velocities.empty()) @@ -1517,7 +1568,8 @@ bool JointTrajectoryController::validate_trajectory_msg( { RCLCPP_ERROR( get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", + "Time between points %zu and %zu is not strictly increasing, it is %f and %f " + "respectively", i - 1, i, previous_traj_time.seconds(), rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); return false; From a465cef8ff7a7c72475434efe7bfbe33ae4076a4 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 11:58:40 +0200 Subject: [PATCH 25/68] Update effort_controllers/src/joint_group_effort_controller.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- effort_controllers/src/joint_group_effort_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index b0e2890ec1..1d053f4c7f 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -60,9 +60,9 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( { if (!command_interface.set_value(0.0)) { - RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value to 0.0"); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } - return controller_interface::CallbackReturn::SUCCESS; } return ret; From f8acfbac02e2157f61d2bd7f68fdf0f584dc4af9 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 11:59:54 +0200 Subject: [PATCH 26/68] Update forward_command_controller/src/forward_controllers_base.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- forward_command_controller/src/forward_controllers_base.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 1522e767f6..fd48b82488 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -148,8 +148,6 @@ controller_interface::return_type ForwardControllersBase::update( data[index]); return controller_interface::return_type::OK; } - - return controller_interface::return_type::OK; } return controller_interface::return_type::OK; From 7f8a4f015ccb73c77a5406138506e479fe7375e9 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:04:50 +0200 Subject: [PATCH 27/68] Update gpio_controllers/src/gpio_command_controller.cpp Co-authored-by: Sai Kishor Kothakota --- gpio_controllers/src/gpio_command_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index f7d6353578..45a77460b5 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -359,7 +359,7 @@ void GpioCommandController::apply_command( if (!command_interfaces_map_.at(full_command_interface_name).get().set_value(command_value)) { RCLCPP_WARN( - get_node()->get_logger(), "Error while setting command for interface '%s' with value '%f'.", + get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.", full_command_interface_name.c_str(), command_value); } } From 93a09eebe6bd45c3cd24693711a505e3827b090b Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:18:12 +0200 Subject: [PATCH 28/68] Update velocity_controllers/src/joint_group_velocity_controller.cpp Co-authored-by: Sai Kishor Kothakota --- velocity_controllers/src/joint_group_velocity_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 4e4bb18db4..b2d0086254 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -61,7 +61,7 @@ controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate { if (!command_interface.set_value(0.0)) { - RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface to value 0.0"); + RCLCPP_WARN(get_node()->get_logger(), "Unable to set the command interface to value 0.0"); return controller_interface::CallbackReturn::SUCCESS; } } From b59a11115ab97840ee28c4fec6bd16ed7db04d85 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:19:12 +0200 Subject: [PATCH 29/68] Update tricycle_controller/src/tricycle_controller.cpp Co-authored-by: Sai Kishor Kothakota --- tricycle_controller/src/tricycle_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index d6ac79774e..df88fdc3a0 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -458,13 +458,13 @@ void TricycleController::halt() { RCLCPP_WARN( get_node()->get_logger(), - "Error while setting velocity command for traction joint to value 0.0"); + "Unable to set the velocity command for traction joint to value 0.0"); } if (!steering_joint_[0].position_command.get().set_value(0.0)) { RCLCPP_WARN( get_node()->get_logger(), - "Error while setting position command for steering joint to value 0.0"); + "Unable to set the position command for steering joint to value 0.0"); } } From 95e9a23468979373b7d089d3cb2cfaffd00aaed9 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:21:15 +0200 Subject: [PATCH 30/68] Update tricycle_steering_controller/src/tricycle_steering_controller.cpp Co-authored-by: Sai Kishor Kothakota --- .../src/tricycle_steering_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 68304a8e3a..d5dc3fb97e 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -96,7 +96,7 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || !steering_position_op.value()) { - RCLCPP_DEBUG( + RCLCPP_WARN( get_node()->get_logger(), "Unable to retrieve the data from right wheel or left wheel or steering position"); return true; From ef39442a0a59b6ff933afbe18eb97769e78ea5d9 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:21:40 +0200 Subject: [PATCH 31/68] Update tricycle_controller/src/tricycle_controller.cpp Co-authored-by: Sai Kishor Kothakota --- tricycle_controller/src/tricycle_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index df88fdc3a0..8bd4a533dd 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -232,7 +232,7 @@ controller_interface::return_type TricycleController::update( { RCLCPP_WARN( get_node()->get_logger(), - "Error while setting position command for steering joint to value: '%f'.", alpha_write); + "Unable to set the position command for steering joint to value: '%f'.", alpha_write); } return controller_interface::return_type::OK; } From f5b3d3065e9bd8631466a5ebafb02542a1fa3b80 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 12:21:54 +0200 Subject: [PATCH 32/68] Update tricycle_controller/src/tricycle_controller.cpp Co-authored-by: Sai Kishor Kothakota --- tricycle_controller/src/tricycle_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 8bd4a533dd..6f3cf29b57 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -226,7 +226,7 @@ controller_interface::return_type TricycleController::update( { RCLCPP_WARN( get_node()->get_logger(), - "Error while setting velocity command for traction joint to value: '%f'.", Ws_write); + "Unable to set the velocity command for traction joint to value: '%f'.", Ws_write); } if (!steering_joint_[0].position_command.get().set_value(alpha_write)) { From 86299a4517e25f1777de8f5ab61052a5d6c01b19 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:07:44 +0200 Subject: [PATCH 33/68] Update tricycle_controller/src/tricycle_controller.cpp Co-authored-by: Sai Kishor Kothakota --- tricycle_controller/src/tricycle_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 6f3cf29b57..d0a9bd684e 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -115,7 +115,7 @@ controller_interface::return_type TricycleController::update( if (!Ws_read_op.has_value() || !alpha_read_op.has_value()) { - RCLCPP_DEBUG( + RCLCPP_WARN( get_node()->get_logger(), "Unable to retrieve the data of traction joint velocity or steering joint position"); return controller_interface::return_type::OK; From 8faa405eff937da8756d3c69acb4165c56d2e0dc Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:09:12 +0200 Subject: [PATCH 34/68] Update steering_controllers_library/src/steering_controllers_library.cpp Co-authored-by: Sai Kishor Kothakota --- .../src/steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index adf6e2a05a..13113bd1db 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -694,7 +694,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c command_interfaces_[number_of_traction_wheels + i].get_optional(); if (!state_interface_value_op.has_value() || !command_interface_value_op.has_value()) { - RCLCPP_DEBUG( + RCLCPP_WARN( logger, "Unable to retrieve %s for steering wheel %zu", !state_interface_value_op.has_value() ? "state interface value" : "command interface value", From a2a502480c86c6ed45c8838544ae4a065317c28f Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:11:30 +0200 Subject: [PATCH 35/68] Update steering_controllers_library/src/steering_controllers_library.cpp Co-authored-by: Sai Kishor Kothakota --- .../src/steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 13113bd1db..8d895f9403 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -678,7 +678,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto linear_velocity_command_interface_op = command_interfaces_[i].get_optional(); if (!linear_velocity_command_interface_op.has_value()) { - RCLCPP_DEBUG( + RCLCPP_WARN( logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); return controller_interface::return_type::OK; } From d8529e2a3a46b5315a550daf7f3a4936dd421aa9 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:16:26 +0200 Subject: [PATCH 36/68] Update parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp Co-authored-by: Sai Kishor Kothakota --- .../parallel_gripper_action_controller_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 9292602ed9..444021fc3e 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -65,7 +65,7 @@ controller_interface::return_type GripperActionController::update( if (!current_position_op.has_value() || !current_velocity_op.has_value()) { - RCLCPP_DEBUG(logger, "Unable to retrieve data of current position or current velocity"); + RCLCPP_WARN(logger, "Unable to retrieve data of current position or current velocity"); return controller_interface::return_type::OK; } From 085e2f50b3588fdb446d69546ac2c35badb93ba7 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:17:02 +0200 Subject: [PATCH 37/68] Update joint_trajectory_controller/src/joint_trajectory_controller.cpp Co-authored-by: Sai Kishor Kothakota --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ba1b711362..31454f6c6c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1104,7 +1104,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const auto joint_position_value_op = joint_command_interface_[0][index].get().get_optional(); if (!joint_position_value_op.has_value()) { - RCLCPP_DEBUG(logger, "Unable to retrieve joint position value"); + RCLCPP_WARN(logger, "Unable to retrieve joint position value"); return controller_interface::CallbackReturn::SUCCESS; } if (!joint_command_interface_[0][index].get().set_value(joint_position_value_op.value())) From 6c865a5cf38d3562dbd5edb900209989593f3c93 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:17:53 +0200 Subject: [PATCH 38/68] Update forward_command_controller/src/forward_controllers_base.cpp Co-authored-by: Sai Kishor Kothakota --- forward_command_controller/src/forward_controllers_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index fd48b82488..ea571fdd3f 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -144,7 +144,7 @@ controller_interface::return_type ForwardControllersBase::update( if (!command_interfaces_[index].set_value(data[index])) { RCLCPP_WARN( - logger, "Error while setting command interface value at index %zu: value = %f", index, + logger, "Unable to set the command interface value at index %zu: value = %f", index, data[index]); return controller_interface::return_type::OK; } From f382dbb56ddbb38d1351117ceddd4b644ff0535a Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:19:06 +0200 Subject: [PATCH 39/68] Update joint_trajectory_controller/src/joint_trajectory_controller.cpp Co-authored-by: Sai Kishor Kothakota --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31454f6c6c..d233c848e5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1111,7 +1111,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( ; { RCLCPP_WARN( - logger, "Error while setting joint position to value %f", + logger, "Unable to set the joint position to value: %f", joint_position_value_op.value()); return controller_interface::CallbackReturn::SUCCESS; } From 4c18e854ad7649287e93595ff7476c48db8e79d4 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:19:45 +0200 Subject: [PATCH 40/68] Update parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp Co-authored-by: Sai Kishor Kothakota --- .../parallel_gripper_action_controller_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 444021fc3e..e04e8b5340 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -79,7 +79,7 @@ controller_interface::return_type GripperActionController::update( if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) { RCLCPP_WARN( - logger, "Error while setting joint position command to: %f", + logger, "Unable to set the joint position command to: %f", command_struct_rt_.position_cmd_); return controller_interface::return_type::OK; } From 895ad290b22b0798e69111bd0bcb9014907c5f9c Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:20:20 +0200 Subject: [PATCH 41/68] Update parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp Co-authored-by: Sai Kishor Kothakota --- .../parallel_gripper_action_controller_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index e04e8b5340..0046a12058 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -88,7 +88,7 @@ controller_interface::return_type GripperActionController::update( !speed_interface_->get().set_value(command_struct_rt_.max_velocity_)) { RCLCPP_WARN( - logger, "Error while setting speed command to: %f", command_struct_rt_.max_velocity_); + logger, "Unable to set the speed command to: %f", command_struct_rt_.max_velocity_); return controller_interface::return_type::OK; } From c2d6b5cff7acb4decfbab4b3f939498859c65b22 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 13:21:10 +0200 Subject: [PATCH 42/68] Update parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp Co-authored-by: Sai Kishor Kothakota --- .../parallel_gripper_action_controller_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 0046a12058..864a32c917 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -97,7 +97,7 @@ controller_interface::return_type GripperActionController::update( !effort_interface_->get().set_value(command_struct_rt_.max_effort_)) { RCLCPP_WARN( - logger, "Error while setting effort command to: %f", command_struct_rt_.max_effort_); + logger, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); return controller_interface::return_type::OK; } From 14c672a350fa3a4fd77a56ed929e6d262987e13b Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 17:15:01 +0200 Subject: [PATCH 43/68] Update joint_trajectory_controller/src/joint_trajectory_controller.cpp Co-authored-by: Sai Kishor Kothakota --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d233c848e5..ad58114064 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1108,7 +1108,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } if (!joint_command_interface_[0][index].get().set_value(joint_position_value_op.value())) - ; { RCLCPP_WARN( logger, "Unable to set the joint position to value: %f", From b473a2272cd2bce2aaef6dae6c6e0b7d5edbca07 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Sun, 8 Jun 2025 17:30:37 +0200 Subject: [PATCH 44/68] Update mecanum_drive_controller/src/mecanum_drive_controller.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 297107e3ef..dea946d19c 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -389,7 +389,7 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if ( !wheel_front_left_state_vel_op.has_value() || !wheel_front_right_state_vel_op.has_value() || - !wheel_rear_right_state_vel_op.has_value() || !wheel_front_left_state_vel_op.has_value()) + !wheel_rear_right_state_vel_op.has_value() || !wheel_rear_left_state_vel_op.has_value()) { RCLCPP_DEBUG( get_node()->get_logger(), From 4ce4139b0f668b251bc42165219c25973bd5683c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 8 Jun 2025 18:20:49 +0200 Subject: [PATCH 45/68] Update joint_trajectory_controller/src/joint_trajectory_controller.cpp --- .../src/joint_trajectory_controller.cpp | 29 ++++++++++++------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ad58114064..e3717f61b0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1367,19 +1367,23 @@ void JointTrajectoryController::fill_partial_goal( // Assume hold position with 0 velocity and acceleration for missing joints if (!it.positions.empty()) { - const auto position_command_value_op = - joint_command_interface_[0][index].get().get_optional(); - if (!position_command_value_op.has_value()) - { - RCLCPP_DEBUG( - get_node()->get_logger(), - "Unable to retrieve position command value of joint at index %zu", index); - } if (has_position_command_interface_) { - // copy last command if cmd interface exists - it.positions.push_back(position_command_value_op.has_value()); + const auto position_command_value_op = + joint_command_interface_[0][index].get().get_optional(); + + if (!position_command_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position command value of joint at index %zu", index); + } + else if (!std::isnan(position_command_value_op.value())) + { + it.positions.push_back(position_command_value_op.value()); + } } + else if (has_position_state_interface_) { // copy current state if state interface exists @@ -1391,7 +1395,10 @@ void JointTrajectoryController::fill_partial_goal( get_node()->get_logger(), "Unable to retrieve position state value of joint at index %zu", index); } - it.positions.push_back(position_state_value_op.value()); + else if (!std::isnan(position_state_value_op.value())) + { + it.positions.push_back(position_state_value_op.value()); + } } } if (!it.velocities.empty()) From 330818c57c010adee76625b30e3b545f250f8ddb Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 9 Jun 2025 15:54:54 +0200 Subject: [PATCH 46/68] steering_controllers_library/src/steering_controllers_library.cpp --- .../src/steering_controllers_library.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 8d895f9403..2cdfe7c3c9 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -592,6 +592,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c RCLCPP_WARN( logger, "Failed to set traction command %.3f for interface '%s' (index %zu).", value, command_interfaces_[i].get_name().c_str(), i); + + return controller_interface::return_type::OK; } } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) @@ -603,6 +605,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c RCLCPP_WARN( logger, "Failed to set steering command %.3f for interface '%s' (index %zu).", value, command_interfaces_[i].get_name().c_str(), i); + + return controller_interface::return_type::OK; } } } @@ -678,8 +682,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto linear_velocity_command_interface_op = command_interfaces_[i].get_optional(); if (!linear_velocity_command_interface_op.has_value()) { - RCLCPP_WARN( - logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); + RCLCPP_WARN(logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); return controller_interface::return_type::OK; } controller_state_publisher_->msg_.linear_velocity_command.push_back( From fa07219eee9e3cd8f59cd5221a29c08e2a74155c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 10 Jun 2025 15:20:31 +0200 Subject: [PATCH 47/68] Remove return statement --- pid_controller/src/pid_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 4be3a249e9..5887500f4c 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -616,7 +616,6 @@ controller_interface::return_type PidController::update_and_write_commands( RCLCPP_DEBUG( get_node()->get_logger(), "Unable to retrieve the command interface value for %s", command_interfaces_[i].get_name().c_str()); - return controller_interface::return_type::OK; } state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value(); From b387a61a7e4de729f5aa962d316999c86b39086b Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 10 Jun 2025 15:21:33 +0200 Subject: [PATCH 48/68] Code formatting pre-commit check --- .../src/joint_trajectory_controller.cpp | 3 +-- .../parallel_gripper_action_controller_impl.hpp | 9 +++------ 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e3717f61b0..7368f763bc 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1110,8 +1110,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( if (!joint_command_interface_[0][index].get().set_value(joint_position_value_op.value())) { RCLCPP_WARN( - logger, "Unable to set the joint position to value: %f", - joint_position_value_op.value()); + logger, "Unable to set the joint position to value: %f", joint_position_value_op.value()); return controller_interface::CallbackReturn::SUCCESS; } } diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 864a32c917..32ebe79bc1 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -79,16 +79,14 @@ controller_interface::return_type GripperActionController::update( if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) { RCLCPP_WARN( - logger, "Unable to set the joint position command to: %f", - command_struct_rt_.position_cmd_); + logger, "Unable to set the joint position command to: %f", command_struct_rt_.position_cmd_); return controller_interface::return_type::OK; } if ( speed_interface_.has_value() && !speed_interface_->get().set_value(command_struct_rt_.max_velocity_)) { - RCLCPP_WARN( - logger, "Unable to set the speed command to: %f", command_struct_rt_.max_velocity_); + RCLCPP_WARN(logger, "Unable to set the speed command to: %f", command_struct_rt_.max_velocity_); return controller_interface::return_type::OK; } @@ -96,8 +94,7 @@ controller_interface::return_type GripperActionController::update( effort_interface_.has_value() && !effort_interface_->get().set_value(command_struct_rt_.max_effort_)) { - RCLCPP_WARN( - logger, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); + RCLCPP_WARN(logger, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); return controller_interface::return_type::OK; } From 38c9e4a552f699a3daa22be38ea4b24214033422 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 10 Jun 2025 15:31:47 +0200 Subject: [PATCH 49/68] Remove return statement --- .../src/steering_controllers_library.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 2cdfe7c3c9..21e6f463dd 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -662,7 +662,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { RCLCPP_DEBUG( logger, "Unable to retrieve position feedback data for traction wheel %zu", i); - return controller_interface::return_type::OK; } controller_state_publisher_->msg_.traction_wheels_position.push_back( position_state_interface_op.value()); @@ -674,7 +673,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { RCLCPP_DEBUG( logger, "Unable to retrieve velocity feedback data for traction wheel %zu", i); - return controller_interface::return_type::OK; } controller_state_publisher_->msg_.traction_wheels_velocity.push_back( velocity_state_interface_op.value()); @@ -682,8 +680,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto linear_velocity_command_interface_op = command_interfaces_[i].get_optional(); if (!linear_velocity_command_interface_op.has_value()) { - RCLCPP_WARN(logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); - return controller_interface::return_type::OK; + RCLCPP_DEBUG( + logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); } controller_state_publisher_->msg_.linear_velocity_command.push_back( linear_velocity_command_interface_op.value()); @@ -697,13 +695,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c command_interfaces_[number_of_traction_wheels + i].get_optional(); if (!state_interface_value_op.has_value() || !command_interface_value_op.has_value()) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger, "Unable to retrieve %s for steering wheel %zu", !state_interface_value_op.has_value() ? "state interface value" : "command interface value", i); - - return controller_interface::return_type::OK; } controller_state_publisher_->msg_.steer_positions.push_back(state_interface_value_op.value()); controller_state_publisher_->msg_.steering_angle_command.push_back( From db48e139e0bb79ae133ef72ed1b4da4bb2472821 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Thu, 12 Jun 2025 09:33:41 +0200 Subject: [PATCH 50/68] Update return type from void to bool --- .../include/gpio_controllers/gpio_command_controller.hpp | 2 +- gpio_controllers/src/gpio_command_controller.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 35cf7c283d..8a2297f04f 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -76,7 +76,7 @@ class GpioCommandController : public controller_interface::ControllerInterface const InterfacesNames & interfaces_from_params, const T & configured_interfaces); void apply_state_value( StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; - void apply_command( + bool apply_command( const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const; bool should_broadcast_all_interfaces_of_configured_gpios() const; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 45a77460b5..6b33926a3d 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -344,7 +344,7 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() return controller_interface::return_type::OK; } -void GpioCommandController::apply_command( +bool GpioCommandController::apply_command( const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const { const auto full_command_interface_name = @@ -361,6 +361,7 @@ void GpioCommandController::apply_command( RCLCPP_WARN( get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.", full_command_interface_name.c_str(), command_value); + return true; } } catch (const std::exception & e) @@ -368,7 +369,10 @@ void GpioCommandController::apply_command( fprintf( stderr, "Exception thrown during applying command stage of %s with message: %s \n", full_command_interface_name.c_str(), e.what()); + return true; } + + return true; } void GpioCommandController::update_gpios_states() From 7fae4ca65ff65f1e176c819ad1f1d7a1c624d171 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 11:42:18 +0200 Subject: [PATCH 51/68] Fix: missing closing bracket --- .../test/test_steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 71ddf2dde1..0b2a67d668 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -183,7 +183,7 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) // Steer angles should not reset EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); - +} // Tests controller update_reference_from_subscribers and // for position_feedback=false behavior // when too old msg is sent i.e age_of_last_command > ref_timeout case From 108a12b83ad673bceeaea3a57a695da6a03fb885 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 11:44:46 +0200 Subject: [PATCH 52/68] Update gpio_controllers/src/gpio_command_controller.cpp --- gpio_controllers/src/gpio_command_controller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index d5921df6bd..bf7162b949 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -432,9 +432,11 @@ void GpioCommandController::apply_state_value( get_node()->get_logger(), "Unable to retrieve the data from state: %s \n", interface_name.c_str()); } - - state_msg.interface_values[gpio_index].values[interface_index] = - state_msg_interface_value_op.value(); + else + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_msg_interface_value_op.value(); + } } catch (const std::exception & e) { From c0df970a5de9c69f51e5d571a669757cd2f533cc Mon Sep 17 00:00:00 2001 From: ska Date: Sun, 20 Jul 2025 14:03:13 +0200 Subject: [PATCH 53/68] Update forward_command_controller to fix deprecations warnings --- .../src/forward_controllers_base.cpp | 12 ++++++------ .../test/test_forward_command_controller.cpp | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index cc0e15224c..bfc42e3185 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -174,20 +174,20 @@ controller_interface::return_type ForwardControllersBase::update( return controller_interface::return_type::ERROR; } - const auto & data = (*joint_commands)->data; - for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - if (!command_interfaces_[index].set_value(data[index])) + const auto & value = joint_commands_.data[index]; + bool success = command_interfaces_[index].set_value(value); + + if (!success) { RCLCPP_WARN( - logger, "Unable to set the command interface value at index %zu: value = %f", index, - data[index]); + get_node()->get_logger(), + "Unable to set the command interface value at index %zu: value = %f", index, value); return controller_interface::return_type::OK; } } return controller_interface::return_type::OK; } - } // namespace forward_command_controller diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 1f5b74102e..50f2f7cf20 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -337,9 +337,9 @@ TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -370,9 +370,9 @@ TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) controller_interface::return_type::OK); // check message containing infinite command value was rejected - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) From c73171ad6240595de62c351ed95b314073ccd4f2 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 16:10:58 +0200 Subject: [PATCH 54/68] Update steering_controllers_library to fix deprecations warnings --- .../src/steering_controllers_library.cpp | 39 ++++++++++--------- .../test_steering_controllers_library.cpp | 16 ++++---- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index d595c3107e..7148711d85 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -459,6 +459,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( get_node()->get_logger(), "Failed to set NaN value for command interface '%s' (index %zu) during deactivation.", command_interfaces_[i].get_name().c_str(), i); + return controller_interface::CallbackReturn::SUCCESS; } } return controller_interface::CallbackReturn::SUCCESS; @@ -528,27 +529,23 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - const auto value = timeout ? 0.0 : traction_commands[i]; + const auto & value = traction_commands[i]; + bool success = command_interfaces_[i].set_value(value); - if (!command_interfaces_[i].set_value(value)) + if (!success) { - RCLCPP_WARN( - logger, "Failed to set traction command %.3f for interface '%s' (index %zu).", value, - command_interfaces_[i].get_name().c_str(), i); - + RCLCPP_WARN(logger, "Unable to set traction command at index %zu: value = %f", i, value); return controller_interface::return_type::OK; } } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { - const auto value = steering_commands[i]; + const auto & value = steering_commands[i]; + bool success = command_interfaces_[i + params_.traction_joints_names.size()].set_value(value); - if (!command_interfaces_[i + params_.traction_joints_names.size()].set_value(value)) + if (!success) { - RCLCPP_WARN( - logger, "Failed to set steering command %.3f for interface '%s' (index %zu).", value, - command_interfaces_[i].get_name().c_str(), i); - + RCLCPP_WARN(logger, "Unable to set steering command at index %zu: value = %f", i, value); return controller_interface::return_type::OK; } } @@ -557,7 +554,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(0.0); + if (!command_interfaces_[i].set_value(0.0)) + { + RCLCPP_WARN(logger, "Unable to set command interface to value 0.0"); + return controller_interface::return_type::OK; + } } } @@ -627,14 +628,14 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_publisher_->msg_.traction_wheels_velocity.push_back( velocity_state_interface_op.value()); } - auto linear_velocity_command_interface_op = command_interfaces_[i].get_optional(); - if (!linear_velocity_command_interface_op.has_value()) + + auto velocity_command_interface_op = command_interfaces_[i].get_optional(); + if (!velocity_command_interface_op.has_value()) { - RCLCPP_DEBUG( - logger, "Unable to retrieve linear velocity command for traction wheel %zu", i); + RCLCPP_DEBUG(logger, "Unable to retrieve velocity command for traction wheel %zu", i); } - controller_state_publisher_->msg_.linear_velocity_command.push_back( - linear_velocity_command_interface_op.value()); + controller_state_publisher_->msg_.traction_command.push_back( + velocity_command_interface_op.value()); } for (size_t i = 0; i < number_of_steering_wheels; ++i) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0b2a67d668..f215526ee2 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,10 +133,10 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // are the command_itfs updated? - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // adjusting to achieve age_of_last_command > ref_timeout msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -239,10 +239,10 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // are the command_itfs updated? - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // adjusting to achieve age_of_last_command > ref_timeout msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - From 2144ba3f63cdf54e5d50524eb382fa5491b74878 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 16:26:54 +0200 Subject: [PATCH 55/68] Update parallel_gripper_controller to fix deprecations warnings --- ...parallel_gripper_action_controller_impl.hpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index d27e124f03..0e2ee76e5c 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -59,15 +59,29 @@ controller_interface::CallbackReturn GripperActionController::on_init() controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + auto logger = get_node()->get_logger(); auto command_struct_rt_op = command_.try_get(); if (command_struct_rt_op.has_value()) { command_struct_rt_ = command_struct_rt_op.value(); } + const auto current_position_op = joint_position_state_interface_->get().get_optional(); + if (!current_position_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current position value"); + return controller_interface::return_type::OK; + } + const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional(); + if (!current_velocity_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current velocity value"); + return controller_interface::return_type::OK; + } - const double error_position = command_struct_rt_.position_cmd_ - current_position; + const double error_position = command_struct_rt_.position_cmd_ - current_position_op.value(); - check_for_success(get_node()->now(), error_position, current_position, current_velocity); + check_for_success( + get_node()->now(), error_position, current_position_op.value(), current_velocity_op.value()); if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) { From db03a469e623f1fbba7f8539e482a8dfbea19ae1 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 17:11:37 +0200 Subject: [PATCH 56/68] Update pid_controller to fix deprecations warnings --- pid_controller/src/pid_controller.cpp | 2 +- pid_controller/test/test_pid_controller.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index cd33a7edc3..e06d5e460f 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -486,7 +486,7 @@ controller_interface::return_type PidController::update_and_write_commands( RCLCPP_DEBUG( get_node()->get_logger(), "Unable to retrieve the state interface value for %s", state_interfaces_[i].get_name().c_str()); - return controller_interface::return_type::OK; + continue; } measured_state_values_[i] = state_interface_value_op.value(); } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 39214606d4..fabf5680ba 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -552,7 +552,8 @@ TEST_F(PidControllerTest, test_update_chained_changing_feedforward_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), first_expected_command_value); + ASSERT_EQ( + controller_->command_interfaces_[0].get_optional().value(), first_expected_command_value); // turn on feedforward gain for (const auto & dof_name : dof_names_) @@ -568,7 +569,8 @@ TEST_F(PidControllerTest, test_update_chained_changing_feedforward_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), second_expected_command_value); + ASSERT_EQ( + controller_->command_interfaces_[0].get_optional().value(), second_expected_command_value); // Check updated parameters for (const auto & dof_name : dof_names_) From 8869f17edccb3a801ad98f0488bba5d8e6f50d74 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 19:01:48 +0200 Subject: [PATCH 57/68] Update joint_trajectory_controller to fix deprecations warnings --- .../src/joint_trajectory_controller.cpp | 41 ++++++++++++++----- .../test/test_trajectory_controller_utils.hpp | 11 +++-- 2 files changed, 38 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 48bde70ce6..131700cd0e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -143,21 +143,26 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + auto logger = this->get_node()->get_logger(); if (scaling_state_interface_.has_value()) { - scaling_factor_ = scaling_state_interface_->get().get_value(); + auto scaling_state_interface_op = scaling_state_interface_->get().get_optional(); + if (!scaling_state_interface_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve scaling state interface value"); + return controller_interface::return_type::OK; + } + scaling_factor_ = scaling_state_interface_op.value(); } if (scaling_command_interface_.has_value()) { if (!scaling_command_interface_->get().set_value(scaling_factor_cmd_.load())) { - RCLCPP_ERROR( - get_node()->get_logger(), "Could not set speed scaling factor through command interfaces."); + RCLCPP_ERROR(logger, "Could not set speed scaling factor through command interfaces."); } } - auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->try_update_params(params_)) { @@ -542,8 +547,12 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) - { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { + auto interface_op = interface.get().get_optional(); + return !interface_op.has_value() || std::isnan(interface_op.value()); + }) == joint_interface.end(); }; // Assign values from the command interfaces as state. Therefore needs check for both. @@ -619,16 +628,28 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( { for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + auto joint_interface_op = joint_interface[index].get().get_optional(); + if (!joint_interface_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve value of joint interface at index %zu", + index); + return true; + } + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_op.value(); } + return true; }; auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) - { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { + auto interface_op = interface.get().get_optional(); + return !interface_op.has_value() || std::isnan(interface_op.value()); + }) == joint_interface.end(); }; // Assign values from the command interfaces as command. diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5bfc01c9da..288d00b9a7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -698,9 +698,10 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + position.at(i) - pos_state_interfaces_[i].get_optional().value(), joint_vel_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + << pos_state_interfaces_[i].get_optional().value() << ", velocity command is " + << joint_vel_[i]; } } if (traj_controller_->has_effort_command_interface()) @@ -708,9 +709,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value() + effort.at(i), joint_eff_[i])) + position.at(i) - pos_state_interfaces_[i].get_optional().value() + effort.at(i), + joint_eff_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + << pos_state_interfaces_[i].get_optional().value() << ", effort command is " + << joint_eff_[i]; } } } From f71e58a11ff785ac85a8edfb7d2a8d6f2a8d503f Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 20 Jul 2025 19:09:36 +0200 Subject: [PATCH 58/68] Update force_torque_sensor_broadcaster to fix deprecations warnings --- .../test/test_force_torque_sensor_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index b88e8e0dc6..4d3e62379b 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -407,7 +407,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipl for (size_t i = 0; i < 6; ++i) { ASSERT_EQ( - exported_state_interfaces[i]->get_value(), + exported_state_interfaces[i]->get_optional().value(), sensor_values_[i] * (i < 3 ? force_multipliers[i] : torque_multipliers[i - 3])); } } From c47726aeeae4f1a1815a4192238f4356f27e0490 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Mon, 21 Jul 2025 10:33:31 +0200 Subject: [PATCH 59/68] Remove unused variable --- forward_command_controller/src/forward_controllers_base.cpp | 3 +-- .../src/steering_controllers_library.cpp | 6 ++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index bfc42e3185..2e32e14ee8 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -177,9 +177,8 @@ controller_interface::return_type ForwardControllersBase::update( for (auto index = 0ul; index < command_interfaces_.size(); ++index) { const auto & value = joint_commands_.data[index]; - bool success = command_interfaces_[index].set_value(value); - if (!success) + if (!command_interfaces_[index].set_value(value)) { RCLCPP_WARN( get_node()->get_logger(), diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 7148711d85..e059a7ff2d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -530,9 +530,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { const auto & value = traction_commands[i]; - bool success = command_interfaces_[i].set_value(value); - if (!success) + if (!command_interfaces_[i].set_value(value)) { RCLCPP_WARN(logger, "Unable to set traction command at index %zu: value = %f", i, value); return controller_interface::return_type::OK; @@ -541,9 +540,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { const auto & value = steering_commands[i]; - bool success = command_interfaces_[i + params_.traction_joints_names.size()].set_value(value); - if (!success) + if (!command_interfaces_[i + params_.traction_joints_names.size()].set_value(value)) { RCLCPP_WARN(logger, "Unable to set steering command at index %zu: value = %f", i, value); return controller_interface::return_type::OK; From ce04f35dde04a8996b4b11fa77160f55f629d884 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 22:47:45 +0200 Subject: [PATCH 60/68] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- admittance_controller/src/admittance_controller.cpp | 6 +++--- .../src/forward_controllers_base.cpp | 2 +- .../src/joint_trajectory_controller.cpp | 8 +++++++- .../parallel_gripper_action_controller_impl.hpp | 5 ++++- pid_controller/src/pid_controller.cpp | 6 ++++-- 5 files changed, 19 insertions(+), 8 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 20169d2c58..e19daefb3e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -563,10 +563,10 @@ void AdmittanceController::read_state_from_hardware( { if (has_position_state_interface_) { - auto state_current_position_op = + const 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()); + !state_current_position_op.has_value() || std::isnan(state_current_position_op.value()); if (state_current_position_op.has_value()) { state_current.positions[joint_ind] = state_current_position_op.value(); @@ -589,7 +589,7 @@ void AdmittanceController::read_state_from_hardware( 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()); + std::isnan(state_current_acceleration_op.value()); if (state_current_acceleration_op.has_value()) { state_current.accelerations[joint_ind] = state_current_acceleration_op.value(); diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 2e32e14ee8..2aa7fc3073 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -182,7 +182,7 @@ controller_interface::return_type ForwardControllersBase::update( { RCLCPP_WARN( get_node()->get_logger(), - "Unable to set the command interface value at index %zu: value = %f", index, value); + "Unable to set the command interface value %s: value = %f", command_interfaces_[index].get_name().c_str(), value); return controller_interface::return_type::OK; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 131700cd0e..73ae1de001 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -471,7 +471,10 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory RCLCPP_DEBUG( logger, "Unable to retrieve joint state interface value for joint at index %zu", index); } - trajectory_point_interface[index] = joint_state_interface_value_op.value(); + else + { + trajectory_point_interface[index] = joint_state_interface_value_op.value(); + } } }; auto assign_point_from_command_interface = @@ -488,8 +491,11 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory RCLCPP_DEBUG( logger, "Unable to retrieve joint command interface value for joint at index %zu", index); } + else + { trajectory_point_interface[map_cmd_to_joints_[index]] = joint_command_interface_value_op.value(); + } } }; diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 0e2ee76e5c..6281ad85d4 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -362,7 +362,10 @@ controller_interface::CallbackReturn GripperActionController::on_activate( { RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); } - command_struct_.position_cmd_ = position_op.value(); + else + { + command_struct_.position_cmd_ = position_op.value(); + } command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; command_.try_set(command_struct_); diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index e06d5e460f..d8caac42c1 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -598,8 +598,10 @@ controller_interface::return_type PidController::update_and_write_commands( get_node()->get_logger(), "Unable to retrieve the command interface value for %s", command_interfaces_[i].get_name().c_str()); } - - state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value(); + else + { + state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value(); + } } state_publisher_->unlockAndPublish(); } From 811ea2ec84b42ffdd03acc86a6b19ee8bd2a1282 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 21 Jul 2025 20:51:07 +0000 Subject: [PATCH 61/68] Fix format --- forward_command_controller/src/forward_controllers_base.cpp | 4 ++-- .../src/joint_trajectory_controller.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 2aa7fc3073..df43736694 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -181,8 +181,8 @@ controller_interface::return_type ForwardControllersBase::update( if (!command_interfaces_[index].set_value(value)) { RCLCPP_WARN( - get_node()->get_logger(), - "Unable to set the command interface value %s: value = %f", command_interfaces_[index].get_name().c_str(), value); + get_node()->get_logger(), "Unable to set the command interface value %s: value = %f", + command_interfaces_[index].get_name().c_str(), value); return controller_interface::return_type::OK; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 73ae1de001..68ab961669 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -493,9 +493,9 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory } else { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_command_interface_value_op.value(); - } + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_command_interface_value_op.value(); + } } }; From 75c3706d5a69062f1d7339f7c16459e8b19aea18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 22:53:41 +0200 Subject: [PATCH 62/68] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../src/joint_trajectory_controller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 68ab961669..f677da698e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -546,7 +546,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto get_node()->get_logger(), "Unable to retrieve value of joint interface for joint at index %zu", index); } - trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value(); + else + { + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value(); + } } }; @@ -1687,8 +1690,7 @@ bool JointTrajectoryController::validate_trajectory_msg( { RCLCPP_ERROR( get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f " - "respectively", + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", i - 1, i, previous_traj_time.seconds(), rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); return false; From a64615ea2836ac10efa8e349ba865bf70ce638ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 22:54:37 +0200 Subject: [PATCH 63/68] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../src/steering_controllers_library.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index e059a7ff2d..63fd2e8da6 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -612,8 +612,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c RCLCPP_DEBUG( logger, "Unable to retrieve position feedback data for traction wheel %zu", i); } + else + { controller_state_publisher_->msg_.traction_wheels_position.push_back( position_state_interface_op.value()); + } } else { @@ -623,8 +626,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c RCLCPP_DEBUG( logger, "Unable to retrieve velocity feedback data for traction wheel %zu", i); } + else + { controller_state_publisher_->msg_.traction_wheels_velocity.push_back( velocity_state_interface_op.value()); + } } auto velocity_command_interface_op = command_interfaces_[i].get_optional(); @@ -632,8 +638,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { RCLCPP_DEBUG(logger, "Unable to retrieve velocity command for traction wheel %zu", i); } + else + { controller_state_publisher_->msg_.traction_command.push_back( velocity_command_interface_op.value()); + } } for (size_t i = 0; i < number_of_steering_wheels; ++i) @@ -650,9 +659,12 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c : "command interface value", i); } + else + { controller_state_publisher_->msg_.steer_positions.push_back(state_interface_value_op.value()); controller_state_publisher_->msg_.steering_angle_command.push_back( command_interface_value_op.value()); + } } controller_state_publisher_->unlockAndPublish(); From 59c55354b23a2ae809354597929f74f6f0c7b871 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 22:55:32 +0200 Subject: [PATCH 64/68] Apply suggestions from code review for mecanum Co-authored-by: Sai Kishor Kothakota --- .../src/mecanum_drive_controller.cpp | 23 ++++--------------- 1 file changed, 4 insertions(+), 19 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 304f4e426f..c320b258be 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -550,26 +550,11 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - const auto front_left_op = state_interfaces_[FRONT_LEFT].get_optional(); - const auto front_right_op = state_interfaces_[FRONT_RIGHT].get_optional(); - const auto rear_right_op = state_interfaces_[REAR_RIGHT].get_optional(); - const auto rear_left_op = state_interfaces_[REAR_LEFT].get_optional(); - if ( - !front_left_op.has_value() || !front_right_op.has_value() || !rear_right_op.has_value() || - !rear_left_op.has_value()) - { - RCLCPP_DEBUG( - get_node()->get_logger(), - "Unable to retrieve the data of state interface of front left or front right or rear left " - "or rear right"); - return controller_interface::return_type::OK; - } - - controller_state_publisher_->msg_.front_left_wheel_velocity = front_left_op.value(); - controller_state_publisher_->msg_.front_right_wheel_velocity = front_right_op.value(); - controller_state_publisher_->msg_.back_right_wheel_velocity = rear_right_op.value(); - controller_state_publisher_->msg_.back_left_wheel_velocity = rear_left_op.value(); + controller_state_publisher_->msg_.front_left_wheel_velocity = wheel_front_left_state_vel; + controller_state_publisher_->msg_.front_right_wheel_velocity = wheel_front_right_state_vel; + controller_state_publisher_->msg_.back_right_wheel_velocity = wheel_rear_right_state_vel; + controller_state_publisher_->msg_.back_left_wheel_velocity = wheel_rear_left_state_vel; controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; From a1df42d8a4ed6ea81121b710b35fb022907f6d1e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 21 Jul 2025 20:58:29 +0000 Subject: [PATCH 65/68] Fix format --- .../src/steering_controllers_library.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 63fd2e8da6..be67eb9cdd 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -614,8 +614,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_wheels_position.push_back( - position_state_interface_op.value()); + controller_state_publisher_->msg_.traction_wheels_position.push_back( + position_state_interface_op.value()); } } else @@ -628,8 +628,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - velocity_state_interface_op.value()); + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + velocity_state_interface_op.value()); } } @@ -640,8 +640,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_command.push_back( - velocity_command_interface_op.value()); + controller_state_publisher_->msg_.traction_command.push_back( + velocity_command_interface_op.value()); } } @@ -661,9 +661,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.steer_positions.push_back(state_interface_value_op.value()); - controller_state_publisher_->msg_.steering_angle_command.push_back( - command_interface_value_op.value()); + controller_state_publisher_->msg_.steer_positions.push_back( + state_interface_value_op.value()); + controller_state_publisher_->msg_.steering_angle_command.push_back( + command_interface_value_op.value()); } } From 014a9f4f4619c9995fb626b723b28b183076abf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 23:01:52 +0200 Subject: [PATCH 66/68] Apply suggestions from code review for GPIO Co-authored-by: Sai Kishor Kothakota --- .../include/gpio_controllers/gpio_command_controller.hpp | 2 +- gpio_controllers/src/gpio_command_controller.cpp | 6 +----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 7c9d8130dc..f7d75e1c20 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -76,7 +76,7 @@ class GpioCommandController : public controller_interface::ControllerInterface const InterfacesNames & interfaces_from_params, const T & configured_interfaces); void apply_state_value( StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; - bool apply_command( + void apply_command( const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const; bool should_broadcast_all_interfaces_of_configured_gpios() const; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index bf7162b949..34222928c3 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -362,7 +362,7 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() return controller_interface::return_type::OK; } -bool GpioCommandController::apply_command( +void GpioCommandController::apply_command( const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const { const auto full_command_interface_name = @@ -379,7 +379,6 @@ bool GpioCommandController::apply_command( RCLCPP_WARN( get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.", full_command_interface_name.c_str(), command_value); - return true; } } catch (const std::exception & e) @@ -387,10 +386,7 @@ bool GpioCommandController::apply_command( fprintf( stderr, "Exception thrown during applying command stage of %s with message: %s \n", full_command_interface_name.c_str(), e.what()); - return true; } - - return true; } void GpioCommandController::update_gpios_states() From c58aeb98ce52f317e61eb82743556062bf7a44f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Jul 2025 23:02:52 +0200 Subject: [PATCH 67/68] Apply suggestions from code review JTC Co-authored-by: Sai Kishor Kothakota --- .../src/joint_trajectory_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f677da698e..3d1f2c1616 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -643,11 +643,10 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( RCLCPP_DEBUG( get_node()->get_logger(), "Unable to retrieve value of joint interface at index %zu", index); - return true; + continue; } trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_op.value(); } - return true; }; auto interface_has_values = [](const auto & joint_interface) From 3635b679cd4abf3f4a2042db40d03a971b7dc02a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 21 Jul 2025 23:49:56 +0200 Subject: [PATCH 68/68] fix has_value in the if condition --- .../src/tricycle_steering_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 1f71d4b534..43fdcd4421 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -59,7 +59,7 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period if ( !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || - !steering_position_op.value()) + !steering_position_op.has_value()) { RCLCPP_WARN( get_node()->get_logger(),