diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 9452339a81..9300fe1721 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -114,18 +114,39 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) { + auto logger = get_node()->get_logger(); + if (params_.open_loop) { odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); } else { - const double traction_right_wheel_value = - state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double traction_left_wheel_value = - state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const auto traction_right_wheel_value_op = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); + const auto traction_left_wheel_value_op = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional(); + const auto steering_right_position_op = + state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional(); + const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional(); + + if ( + !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || + !steering_right_position_op.has_value() || !steering_left_position_op.has_value()) + { + RCLCPP_DEBUG( + logger, + "Unable to retrieve the data from right wheel or left wheel or right steering position or " + "left steering position!"); + + return true; + } + + const double traction_right_wheel_value = traction_right_wheel_value_op.value(); + const double traction_left_wheel_value = traction_left_wheel_value_op.value(); + const double steering_right_position = steering_right_position_op.value(); + const double steering_left_position = steering_left_position_op.value(); + if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index f7cdca092f..da69fbf042 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -173,17 +173,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); @@ -213,17 +213,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); @@ -263,17 +263,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 77ff09cb01..db6a91700d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -528,21 +528,37 @@ void AdmittanceController::read_state_from_hardware( { if (has_position_state_interface_) { - state_current.positions[joint_ind] = - state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); - nan_position |= std::isnan(state_current.positions[joint_ind]); + 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.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.value()); + if (state_current_acceleration_op.has_value()) + { + state_current.accelerations[joint_ind] = state_current_acceleration_op.value(); + } } } @@ -578,23 +594,31 @@ void AdmittanceController::write_state_to_hardware( size_t vel_ind = (has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind; size_t acc_ind = vel_ind + has_acceleration_command_interface_; + + auto logger = get_node()->get_logger(); + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { + bool success = true; if (has_position_command_interface_) { - command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } if (has_velocity_command_interface_) { - command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( state_commanded.velocities[joint_ind]); } if (has_acceleration_command_interface_) { - command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( state_commanded.accelerations[joint_ind]); } + if (!success) + { + RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind); + } } last_commanded_ = state_commanded; } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index eb10e5ad00..03fbcfa172 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -64,14 +64,27 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) { + auto logger = get_node()->get_logger(); + if (params_.open_loop) { odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); } else { - const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional(); + const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional(); + + if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve the data from the traction wheel or steering position!"); + return true; + } + + const double traction_wheel_value = traction_wheel_value_op.value(); + const double steering_position = steering_position_op.value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 23103f7974..7036dd4102 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -126,9 +126,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -158,9 +158,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -190,9 +191,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -245,9 +247,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 3295df05a1..3dc5b1d035 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -645,11 +645,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"); + } } }; diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index e7d0f274a5..1d053f4c7f 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_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0"); + return controller_interface::CallbackReturn::SUCCESS; + } } 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 01125ac442..e88dfe63a6 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -106,9 +106,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(); @@ -121,9 +121,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) @@ -143,9 +143,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) @@ -160,9 +160,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) @@ -171,9 +171,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_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,9 +204,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) @@ -218,15 +218,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); } 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 99fa93d845..017928888f 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 @@ -340,7 +340,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])); } } @@ -411,7 +411,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])); } } @@ -450,8 +450,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) @@ -501,7 +501,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]); } } diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index ff765e6600..e8ccb3c6ca 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -151,10 +151,17 @@ controller_interface::return_type ForwardControllersBase::update( for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value((*joint_commands)->data[index]); + const auto & value = (*joint_commands)->data[index]; + + 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); + 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 e00a69c468..07f4e64e3a 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -213,9 +213,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(); @@ -228,9 +228,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) @@ -256,9 +256,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) @@ -278,9 +278,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) @@ -291,9 +291,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_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -324,9 +324,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, DropInfiniteCommandCallbackTest) @@ -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) @@ -383,9 +383,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); @@ -420,9 +420,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); @@ -472,9 +472,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); @@ -485,7 +485,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 7bd869677c..2be11101a9 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); } diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fdecf71c30..4250114e1f 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(), "Unable to set the command for interface '%s' with value '%f'.", + full_command_interface_name.c_str(), command_value); + } } catch (const std::exception & e) { @@ -394,8 +401,20 @@ void GpioCommandController::apply_state_value( state_msg.interface_values[gpio_index].interface_names[interface_index]; try { - state_msg.interface_values[gpio_index].values[interface_index] = - state_interfaces_map_.at(interface_name).get().get_value(); + 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()); + } + else + { + state_msg.interface_values[gpio_index].values[interface_index] = + 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 de085894f2..e8f720de3b 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -133,12 +133,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() @@ -492,9 +492,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( @@ -516,9 +516,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( @@ -540,9 +540,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( @@ -565,9 +565,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( @@ -592,9 +592,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) 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 a94e163d7b..8e60a1add0 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,8 @@ 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 double current_position = joint_position_state_interface_->get().get_optional().value(); + const double current_velocity = joint_velocity_state_interface_->get().get_optional().value(); const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; @@ -147,7 +147,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel template void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_ = joint_position_state_interface_->get().get_optional().value(); command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -285,7 +285,7 @@ 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(); + command_struct_.position_ = joint_position_state_interface_->get().get_optional().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..1c7d08baaf 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -83,7 +83,7 @@ class HardwareInterfaceAdapter double /* error_velocity */, double max_allowed_effort) { // Forward desired position to command - joint_handle_->get().set_value(desired_position); + std::ignore = joint_handle_->get().set_value(desired_position); return max_allowed_effort; } @@ -148,7 +148,7 @@ class HardwareInterfaceAdapter } // Reset PIDs, zero effort commands pid_->reset(); - joint_handle_->get().set_value(0.0); + std::ignore = joint_handle_->get().set_value(0.0); } void stopping(const rclcpp::Time & /* time */) {} @@ -168,7 +168,7 @@ 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); + std::ignore = joint_handle_->get().set_value(command); last_update_time_ = std::chrono::steady_clock::now(); return command; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 251dacc460..4c6717d338 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -282,7 +282,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa { for (size_t index = 0; index < num_cmd_joints_; ++index) { - joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); + if (!joint_interface[index].get().set_value( + trajectory_point_interface[map_cmd_to_joints_[index]])) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to set value for joint '%s' in command interface '%s'. ", + command_joint_names_[index].c_str(), joint_interface[index].get().get_name().c_str()); + return; + } } } }; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 117826ff3f..494ac8b11d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -430,12 +430,22 @@ 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); + } + else + { + trajectory_point_interface[index] = joint_state_interface_value_op.value(); + } } }; auto assign_point_from_command_interface = @@ -446,8 +456,17 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory std::numeric_limits::quiet_NaN()); 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_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); + } + else + { + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_command_interface_value_op.value(); + } } }; @@ -491,16 +510,29 @@ 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); + } + else + { + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value(); + } } }; 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. @@ -576,16 +608,27 @@ 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); + continue; + } + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_op.value(); } }; 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. @@ -1079,6 +1122,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_ = false; @@ -1093,24 +1137,39 @@ 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_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())) + { + RCLCPP_WARN( + logger, "Unable to set the 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; } } @@ -1342,17 +1401,38 @@ 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())) + if (has_position_command_interface_) { - // copy last command if cmd interface exists - it.positions.push_back(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); + } + 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 - 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); + } + else if (!std::isnan(position_state_value_op.value())) + { + it.positions.push_back(position_state_value_op.value()); + } } } if (!it.velocities.empty()) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c47e9ccd86..41a2ab8ece 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -393,13 +393,13 @@ class TrajectoryControllerTest : public ::testing::Test // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(initial_pos_joints[i]); + (void)cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(initial_vel_joints[i]); + (void)cmd_interfaces.back().set_value(initial_vel_joints[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(initial_acc_joints[i]); + (void)cmd_interfaces.back().set_value(initial_acc_joints[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(initial_eff_joints[i]); + (void)cmd_interfaces.back().set_value(initial_eff_joints[i]); if (separate_cmd_and_state_values) { joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; @@ -690,9 +690,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()) @@ -700,9 +701,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]; } } } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index ead080050f..acb3b5a5cf 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -400,10 +400,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_rear_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) && @@ -522,14 +538,12 @@ 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(); + + 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]; 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 748940b138..1278be5453 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -339,11 +339,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)), @@ -561,7 +561,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(); @@ -600,7 +600,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); } } @@ -659,7 +659,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); } } @@ -810,10 +810,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( 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 b6be291a8d..82037ed546 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 @@ -57,22 +57,46 @@ 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(); command_struct_rt_ = *(command_.readFromRT()); + 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 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; + 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()); - 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_)) { - speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + RCLCPP_WARN( + 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_); + + 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, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); + return controller_interface::return_type::OK; } return controller_interface::return_type::OK; @@ -168,7 +192,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_); @@ -317,7 +346,15 @@ 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"); + } + else + { + 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_); diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 05b534bf3e..15b3e77696 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -493,7 +493,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()); + continue; + } + measured_state_values_[i] = state_interface_value_op.value(); } } @@ -598,7 +606,18 @@ 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()); + } + else + { + 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 1900b172ac..633a6259ce 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -179,15 +179,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)), @@ -319,7 +319,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30100.301000; - 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); } @@ -379,7 +380,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30100.301000; - 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); } } @@ -434,10 +436,9 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // i_term = zero at first update // d_term = error_dot * d_gain = 390.0 * 3.0 = 1170.0 // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 - { - const double expected_command_value = 1173.9; - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); - } + const double expected_command_value = 1173.9; + + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -470,7 +471,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // d_term = error_dot * d_gain = 890.0 * 3.0 = 2670.0 // feedforward OFF -> cmd = p_term + i_term + d_term = 8.9 + 0.0 + 2670.0 = 2678.9 const double expected_command_value = 2678.9; - 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); } /** @@ -506,7 +508,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // first update d_term = error_dot * d_gain = 261.6814 * 3.0 = 785.0444079 feedforward OFF -> cmd // = p_term + i_term + d_term = 2.616814, + 0.0 + 785.0444079 = 787.6612219 const double expected_command_value = 787.6612219; - 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) @@ -644,7 +647,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); } /** @@ -702,7 +705,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); } /** @@ -738,7 +741,8 @@ TEST_F(PidControllerTest, test_save_i_term_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30100.301 const double expected_command_value = 30100.3010; - 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 @@ -751,7 +755,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); } @@ -788,7 +792,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.301 const double expected_command_value = 30100.3010; - 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 @@ -801,7 +806,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 e8847ecd2d..9099d11a75 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -95,8 +95,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i // check the commands const double joint1_expected_cmd = 8.90; const double joint2_expected_cmd = 9.90; - 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) 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); } diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f34b3a78e0..bbb779b60a 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -529,7 +529,14 @@ 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; + } } return controller_interface::CallbackReturn::SUCCESS; } @@ -552,6 +559,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 @@ -574,11 +582,23 @@ 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, "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++) { - 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, "Unable to set steering command at index %zu: value = %f", i, value); + return controller_interface::return_type::OK; + } } } @@ -628,24 +648,66 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { if (params_.position_feedback) { - controller_state_publisher_->msg_.traction_wheels_position.push_back( - state_interfaces_[i].get_value()); + 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); + } + else + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + 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); + } + 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(); + if (!velocity_command_interface_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve velocity command for traction wheel %zu", i); } else { - controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - state_interfaces_[i].get_value()); + controller_state_publisher_->msg_.linear_velocity_command.push_back( + velocity_command_interface_op.value()); } - controller_state_publisher_->msg_.linear_velocity_command.push_back( - command_interfaces_[i].get_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()); - controller_state_publisher_->msg_.steering_angle_command.push_back( - command_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); + } + 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(); 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) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 6d32b8265a..d0a9bd684e 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_WARN( + 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(), + "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)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to set the 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(), + "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(), + "Unable to set the 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 cdc97b0518..a933ef1fa8 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -285,15 +285,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(); } @@ -313,8 +313,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()); @@ -329,8 +329,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 @@ -341,14 +341,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_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 2c919cbfd8..c626301284 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -86,11 +86,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.has_value()) + { + RCLCPP_WARN( + 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 4f9b53e31f..f0d2f6d2db 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -133,9 +133,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)), @@ -165,13 +165,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)); @@ -202,13 +202,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)); @@ -248,13 +248,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); diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 97f6713d72..b2d0086254 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(), "Unable to set the 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 9a9af3fb81..880f664141 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_->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); }