diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 4a49af9d82..326f05a579 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -55,18 +55,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 7eb0f8eea0..896163ef91 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_.get().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_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); @@ -264,17 +264,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 4f915d204c..e19daefb3e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -563,21 +563,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(); + } } } @@ -613,23 +629,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 46302a1451..b10ec445b7 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -45,14 +45,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 4fa5ed3251..b6009e1d7a 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_.get().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_.get().twist.linear.x)); @@ -246,9 +248,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8464867c49..cb4f2edcee 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -578,11 +578,15 @@ void DiffDriveController::reset_buffers() void DiffDriveController::halt() { - const auto halt_wheels = [](auto & wheel_handles) + auto logger = get_node()->get_logger(); + const auto halt_wheels = [&](auto & wheel_handles) { for (const auto & wheel_handle : wheel_handles) { - wheel_handle.velocity.get().set_value(0.0); + if (!wheel_handle.velocity.get().set_value(0.0)) + { + RCLCPP_WARN(logger, "Failed to set wheel velocity to value 0.0"); + } } }; 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 dda7a755db..5fa4394250 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 forward_command_controller::CmdType command; @@ -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 053eec35d1..4d3e62379b 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -337,7 +337,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])); } } @@ -407,7 +407,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipl for (size_t i = 0; i < 6; ++i) { ASSERT_EQ( - exported_state_interfaces[i]->get_value(), + exported_state_interfaces[i]->get_optional().value(), sensor_values_[i] * (i < 3 ? force_multipliers[i] : torque_multipliers[i - 3])); } } @@ -445,8 +445,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) @@ -495,7 +495,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 769763bd67..df43736694 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -176,10 +176,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 2a28a76d7c..50f2f7cf20 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 forward_command_controller::CmdType command_msg; @@ -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); @@ -468,9 +468,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_.set(command); @@ -481,7 +481,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 9407118598..814d7b6218 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 @@ -190,9 +190,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) @@ -210,9 +210,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) @@ -225,9 +225,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) @@ -245,9 +245,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) @@ -277,9 +277,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) @@ -305,9 +305,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); @@ -353,9 +353,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_.set(command); @@ -366,7 +366,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 bdabbb3f13..34222928c3 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -368,11 +368,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) { @@ -412,8 +419,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 c252ed4bf9..9bf26e91d9 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_.set(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_.set(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( @@ -539,9 +539,9 @@ TEST_F( controller_->rt_command_.set(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( @@ -563,9 +563,9 @@ TEST_F( controller_->rt_command_.set(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( @@ -590,9 +590,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/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 65c4709b5f..3d1f2c1616 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -143,21 +143,26 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + auto logger = this->get_node()->get_logger(); if (scaling_state_interface_.has_value()) { - scaling_factor_ = scaling_state_interface_->get().get_value(); + auto scaling_state_interface_op = scaling_state_interface_->get().get_optional(); + if (!scaling_state_interface_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve scaling state interface value"); + return controller_interface::return_type::OK; + } + scaling_factor_ = scaling_state_interface_op.value(); } if (scaling_command_interface_.has_value()) { if (!scaling_command_interface_->get().set_value(scaling_factor_cmd_.load())) { - RCLCPP_ERROR( - get_node()->get_logger(), "Could not set speed scaling factor through command interfaces."); + RCLCPP_ERROR(logger, "Could not set speed scaling factor through command interfaces."); } } - auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->try_update_params(params_)) { @@ -454,12 +459,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 = @@ -470,8 +485,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(); + } } }; @@ -515,16 +539,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. @@ -600,16 +637,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. @@ -1153,6 +1201,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; @@ -1167,24 +1216,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; } } @@ -1418,17 +1482,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 5bfc01c9da..288d00b9a7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -698,9 +698,10 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + position.at(i) - pos_state_interfaces_[i].get_optional().value(), joint_vel_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + << pos_state_interfaces_[i].get_optional().value() << ", velocity command is " + << joint_vel_[i]; } } if (traj_controller_->has_effort_command_interface()) @@ -708,9 +709,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value() + effort.at(i), joint_eff_[i])) + position.at(i) - pos_state_interfaces_[i].get_optional().value() + effort.at(i), + joint_eff_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + << pos_state_interfaces_[i].get_optional().value() << ", effort command is " + << joint_eff_[i]; } } } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index b96cacc5b4..c320b258be 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -412,10 +412,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) && @@ -534,14 +550,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 66bc2d6ab9..c1466bf6f3 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -338,11 +338,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)), @@ -564,7 +564,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); } msg_2.header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); @@ -603,7 +603,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); } } @@ -662,7 +662,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); } } @@ -817,10 +817,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 87bd2c34a5..6281ad85d4 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -59,26 +59,50 @@ controller_interface::CallbackReturn GripperActionController::on_init() controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + auto logger = get_node()->get_logger(); auto command_struct_rt_op = command_.try_get(); if (command_struct_rt_op.has_value()) { command_struct_rt_ = command_struct_rt_op.value(); } + const auto current_position_op = joint_position_state_interface_->get().get_optional(); + if (!current_position_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current position value"); + return controller_interface::return_type::OK; + } + const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional(); + if (!current_velocity_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current velocity value"); + return controller_interface::return_type::OK; + } - const double 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; @@ -176,7 +200,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_.set(command_struct_); @@ -327,8 +356,16 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + // Command - non RT version + 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_.try_set(command_struct_); diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c35a637d51..d8caac42c1 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -480,7 +480,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(); } } @@ -582,7 +590,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 8aa4abe2cf..fabf5680ba 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)), @@ -243,7 +243,8 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain) // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30102.301020; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } @@ -298,7 +299,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 const double expected_command_value = 1173.978; - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -324,7 +325,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // check the result of the commands - the values are not wrapped const double expected_command_value = 2679.078; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } /** @@ -354,7 +356,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // Check the command value const double expected_command_value = 787.713559; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -486,7 +489,7 @@ TEST_F(PidControllerTest, test_update_chained_non_zero_feedforward_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); } /** @@ -549,7 +552,8 @@ TEST_F(PidControllerTest, test_update_chained_changing_feedforward_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), first_expected_command_value); + ASSERT_EQ( + controller_->command_interfaces_[0].get_optional().value(), first_expected_command_value); // turn on feedforward gain for (const auto & dof_name : dof_names_) @@ -565,7 +569,8 @@ TEST_F(PidControllerTest, test_update_chained_changing_feedforward_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), second_expected_command_value); + ASSERT_EQ( + controller_->command_interfaces_[0].get_optional().value(), second_expected_command_value); // Check updated parameters for (const auto & dof_name : dof_names_) @@ -606,7 +611,8 @@ TEST_F(PidControllerTest, test_save_i_term_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -619,7 +625,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); } @@ -655,7 +661,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -668,7 +675,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 c95cc1bd49..bfb7054c10 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -90,8 +90,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i // check the commands const double joint1_expected_cmd = 8.915; const double joint2_expected_cmd = 9.915; - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); - ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd); } int main(int argc, char ** argv) diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 7d575e7d6a..3d38671cba 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 forward_command_controller::CmdType command; @@ -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 2a117a171d..be67eb9cdd 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -453,7 +453,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; } @@ -507,6 +514,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 @@ -521,18 +529,34 @@ 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(traction_commands[i]); + const auto & value = 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; + } } } else { for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(0.0); + if (!command_interfaces_[i].set_value(0.0)) + { + RCLCPP_WARN(logger, "Unable to set command interface to value 0.0"); + return controller_interface::return_type::OK; + } } } @@ -582,24 +606,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 { - controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - state_interfaces_[i].get_value()); + 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_command.push_back( + velocity_command_interface_op.value()); } - controller_state_publisher_->msg_.traction_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 a88b84874c..f215526ee2 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,10 +133,10 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // are the command_itfs updated? - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // adjusting to achieve age_of_last_command > ref_timeout msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -177,14 +177,13 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // 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); } - // Tests controller update_reference_from_subscribers and // for position_feedback=false behavior // when too old msg is sent i.e age_of_last_command > ref_timeout case @@ -240,10 +239,10 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // are the command_itfs updated? - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // adjusting to achieve age_of_last_command > ref_timeout msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -273,12 +272,12 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // 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 e7433b59be..380a70cf37 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; } @@ -432,8 +453,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 55f41801b5..124088de6e 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -284,15 +284,15 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); executor.cancel(); } @@ -312,8 +312,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -328,8 +328,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_optional().value()); // deactivated // wait so controller process the second point when deactivated @@ -340,14 +340,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->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 605c5d8fbc..43fdcd4421 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -51,11 +51,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 2803c1394e..816a2181e8 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_.get().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_.get().twist.linear.x)); @@ -249,13 +249,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 fd7dec4cb2..9d7a5f2b19 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 forward_command_controller::CmdType command; @@ -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); }