From 06ce8fff1b1d7dc876d88690583746c0aab6cc48 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 5 Mar 2025 16:35:07 +0100 Subject: [PATCH 1/2] Update ros2_control API to get_optional --- ur_controllers/CMakeLists.txt | 2 +- ur_controllers/src/force_mode_controller.cpp | 3 +- .../src/freedrive_mode_controller.cpp | 6 +- ur_controllers/src/gpio_controller.cpp | 112 +++++++++++------- .../src/passthrough_trajectory_controller.cpp | 30 +++-- .../scaled_joint_trajectory_controller.cpp | 2 +- .../src/speed_scaling_state_broadcaster.cpp | 3 +- .../src/ur_configuration_controller.cpp | 8 +- 8 files changed, 101 insertions(+), 65 deletions(-) diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 79a89acbd..3db6e8226 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -111,7 +111,7 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type) +target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror) # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index b64a26bbc..6f0c3677b 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/ur_controllers/src/force_mode_controller.cpp @@ -173,7 +173,8 @@ ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& / controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value(); + async_state_ = + command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); // Publish state of force_mode? if (change_requested_) { diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index 50f893e7a..d5b823d53 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -193,15 +193,15 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - async_state_ = async_success_command_interface_->get().get_value(); + async_state_ = async_success_command_interface_->get().get_optional().value(); if (change_requested_) { bool write_success = true; if (freedrive_active_) { // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the // teach pendant. - if (!std::isnan(abort_command_interface_->get().get_value()) && - abort_command_interface_->get().get_value() == 1.0) { + if ((abort_command_interface_->get().get_optional().has_value()) && + abort_command_interface_->get().get_optional().value() == 1.0) { RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request."); freedrive_active_ = false; return controller_interface::return_type::OK; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 4dffc41be..ba16fb4b1 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -197,27 +197,27 @@ void GPIOController::publishIO() { for (size_t i = 0; i < 18; ++i) { io_msg_.digital_out_states[i].pin = i; - io_msg_.digital_out_states[i].state = static_cast(state_interfaces_[i].get_value()); + io_msg_.digital_out_states[i].state = static_cast(state_interfaces_[i].get_optional().value_or(0.0)); io_msg_.digital_in_states[i].pin = i; io_msg_.digital_in_states[i].state = - static_cast(state_interfaces_[i + StateInterfaces::DIGITAL_INPUTS].get_value()); + static_cast(state_interfaces_[i + StateInterfaces::DIGITAL_INPUTS].get_optional().value_or(0.0)); } for (size_t i = 0; i < 2; ++i) { io_msg_.analog_in_states[i].pin = i; io_msg_.analog_in_states[i].state = - static_cast(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_value()); + static_cast(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_optional().value_or(0.0)); io_msg_.analog_in_states[i].domain = - static_cast(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES].get_value()); + static_cast(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES].get_optional().value_or(0.0)); } for (size_t i = 0; i < 2; ++i) { io_msg_.analog_out_states[i].pin = i; io_msg_.analog_out_states[i].state = - static_cast(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_value()); + static_cast(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_optional().value_or(0.0)); io_msg_.analog_out_states[i].domain = - static_cast(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_value()); + static_cast(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_optional().value_or(0.0)); } io_pub_->publish(io_msg_); @@ -225,25 +225,28 @@ void GPIOController::publishIO() void GPIOController::publishToolData() { - tool_data_msg_.tool_mode = static_cast(state_interfaces_[StateInterfaces::TOOL_MODE].get_value()); + tool_data_msg_.tool_mode = + static_cast(state_interfaces_[StateInterfaces::TOOL_MODE].get_optional().value_or(0.0)); tool_data_msg_.analog_input_range2 = - static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_value()); + static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_optional().value_or(0.0)); tool_data_msg_.analog_input_range3 = - static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_value()); - tool_data_msg_.analog_input2 = static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_value()); + static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_optional().value_or(0.0)); + tool_data_msg_.analog_input2 = + static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_optional().value_or(0.0)); tool_data_msg_.analog_input3 = - static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_value()); + static_cast(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_optional().value_or(0.0)); tool_data_msg_.tool_output_voltage = - static_cast(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_value()); - tool_data_msg_.tool_current = static_cast(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_value()); + static_cast(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_optional().value_or(0.0)); + tool_data_msg_.tool_current = + static_cast(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_optional().value_or(0.0)); tool_data_msg_.tool_temperature = - static_cast(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_value()); + static_cast(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_optional().value_or(0.0)); tool_data_pub_->publish(tool_data_msg_); } void GPIOController::publishRobotMode() { - auto robot_mode = static_cast(state_interfaces_[StateInterfaces::ROBOT_MODE].get_value()); + auto robot_mode = static_cast(state_interfaces_[StateInterfaces::ROBOT_MODE].get_optional().value_or(0.0)); if (robot_mode_msg_.mode != robot_mode) { robot_mode_msg_.mode = robot_mode; @@ -253,7 +256,7 @@ void GPIOController::publishRobotMode() void GPIOController::publishSafetyMode() { - auto safety_mode = static_cast(state_interfaces_[StateInterfaces::SAFETY_MODE].get_value()); + auto safety_mode = static_cast(state_interfaces_[StateInterfaces::SAFETY_MODE].get_optional().value_or(0.0)); if (safety_mode_msg_.mode != safety_mode) { safety_mode_msg_.mode = safety_mode; @@ -263,7 +266,8 @@ void GPIOController::publishSafetyMode() void GPIOController::publishProgramRunning() { - auto program_running_value = static_cast(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_value()); + auto program_running_value = + static_cast(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_optional().value_or(0.0)); bool program_running = program_running_value == 1.0 ? true : false; if (program_running_msg_.data != program_running) { program_running_msg_.data = program_running; @@ -274,7 +278,7 @@ void GPIOController::publishProgramRunning() controller_interface::CallbackReturn ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { - while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) { + while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_optional().value_or(0.0) == 0.0) { RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize..."); std::this_thread::sleep_for(std::chrono::milliseconds(50)); } @@ -350,12 +354,14 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: RCLCPP_INFO(get_node()->get_logger(), "Setting digital output '%d' to state: '%1.0f'.", req->pin, req->state); - if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_value()); + resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); return resp->success; } else if (req->fun == req->FUN_SET_ANALOG_OUT && req->pin >= 0 && req->pin <= 2) { // io async success @@ -365,12 +371,15 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state); - if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); return resp->success; } else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) { std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); @@ -378,12 +387,15 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state); - if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); return resp->success; } else { resp->success = false; @@ -423,12 +435,14 @@ bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::Sha RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin, req->data.state, domain_string.c_str()); - if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_value()); + resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); return resp->success; } @@ -444,13 +458,15 @@ bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Reques static_cast(req->speed_slider_fraction)); if (!waitForAsyncCommand([&]() { - return command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value(); + return command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING); })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that target speed fraction was set. (This might happen " "when using the mocked interface)"); } - resp->success = - static_cast(command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING)); } else { RCLCPP_WARN(get_node()->get_logger(), "The desired speed slider fraction must be within range (0; 1.0]. Request " "ignored."); @@ -468,13 +484,16 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP // call the service in the hardware std::ignore = command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_CMD].set_value(1.0); - if (!waitForAsyncCommand( - [&]() { return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that program was sent. (This might happen when using the " "mocked interface)"); } - resp->success = - static_cast(command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING)); if (resp->success) { RCLCPP_INFO(get_node()->get_logger(), "Successfully resent robot program"); @@ -494,13 +513,15 @@ bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr // call the service in the hardware std::ignore = command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_CMD].set_value(1.0); - if (!waitForAsyncCommand( - [&]() { return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that hand_back_control was correctly triggered. (This " "might happen when using the mocked interface)"); } - resp->success = - static_cast(command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); if (resp->success) { RCLCPP_INFO(get_node()->get_logger(), "Deactivated control"); @@ -523,13 +544,15 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y); std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z); - if (!waitForAsyncCommand( - [&]() { return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that payload was set. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); if (resp->success) { RCLCPP_INFO(get_node()->get_logger(), "Payload has been set successfully"); @@ -549,13 +572,16 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r // call the service in the hardware std::ignore = command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0); - if (!waitForAsyncCommand( - [&]() { return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value(); })) { + if (!waitForAsyncCommand([&]() { + return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING); + })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that FTS was zeroed. (This might happen when using the " "mocked interface)"); } - resp->success = static_cast(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value()); + resp->success = static_cast( + command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING)); if (resp->success) { RCLCPP_INFO(get_node()->get_logger(), "Successfully zeroed the force torque sensor"); diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index ec9efc994..11a7ebc35 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -230,14 +230,15 @@ controller_interface::return_type PassthroughTrajectoryController::update(const { const auto active_goal = *rt_active_goal_.readFromRT(); - const auto current_transfer_state = transfer_command_interface_->get().get_value(); + const auto current_transfer_state = transfer_command_interface_->get().get_optional().value_or(TRANSFER_STATE_IDLE); bool write_success = true; if (active_goal && trajectory_active_) { if (current_transfer_state != TRANSFER_STATE_IDLE) { // Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach // pendant. - if (abort_command_interface_->get().get_value() == 1.0 && current_index_ > 0) { + if (abort_command_interface_->get().get_optional().has_value() && + abort_command_interface_->get().get_optional().value() == 1.0 && current_index_ > 0) { RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); std::shared_ptr result = std::make_shared(); @@ -330,7 +331,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const } else if (current_transfer_state == TRANSFER_STATE_IN_MOTION) { // Keep track of how long the trajectory has been executing, if it takes too long, send a warning. if (scaling_state_interface_.has_value()) { - scaling_factor_ = scaling_state_interface_->get().get_value(); + scaling_factor_ = scaling_state_interface_->get().get_optional().value_or(1.0); } #if RCLCPP_VERSION_MAJOR >= 17 @@ -585,8 +586,11 @@ bool PassthroughTrajectoryController::check_goal_tolerance() const std::string joint_name = joint_names_internal->at(i); const auto& joint_tol = goal_tolerance->at(i); const auto& setpoint = active_joint_traj_.points.back().positions[joint_mapping->at(joint_name)]; - const double joint_pos = joint_position_state_interface_[i].get().get_value(); - if (std::abs(joint_pos - setpoint) > joint_tol.position) { + const auto joint_pos = joint_position_state_interface_[i].get().get_optional(); + if (!joint_pos.has_value()) { + return false; + } + if (std::abs(joint_pos.value() - setpoint) > joint_tol.position) { // RCLCPP_ERROR( // get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f", // joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos, joint_tol.position); @@ -594,16 +598,22 @@ bool PassthroughTrajectoryController::check_goal_tolerance() } if (!active_joint_traj_.points.back().velocities.empty()) { - const double joint_vel = joint_velocity_state_interface_[i].get().get_value(); + const auto joint_vel = joint_velocity_state_interface_[i].get().get_optional(); + if (!joint_vel.has_value()) { + return false; + } const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)]; - if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) { + if (std::abs(joint_vel.value() - expected_vel) > joint_tol.velocity) { return false; } } if (!active_joint_traj_.points.back().accelerations.empty()) { - const double joint_vel = joint_acceleration_state_interface_[i].get().get_value(); - const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; - if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) { + const auto joint_acc = joint_acceleration_state_interface_[i].get().get_optional(); + if (!joint_acc.has_value()) { + return false; + } + const auto& expected_acc = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; + if (std::abs(joint_acc.value() - expected_acc) > joint_tol.acceleration) { return false; } } diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index e74f512e4..9ac12d9c8 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -99,7 +99,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const const rclcpp::Duration& period) { if (scaling_state_interface_.has_value()) { - scaling_factor_ = scaling_state_interface_->get().get_value(); + scaling_factor_ = scaling_state_interface_->get().get_optional().value_or(1.0); } if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { diff --git a/ur_controllers/src/speed_scaling_state_broadcaster.cpp b/ur_controllers/src/speed_scaling_state_broadcaster.cpp index a026081fd..5997a365d 100644 --- a/ur_controllers/src/speed_scaling_state_broadcaster.cpp +++ b/ur_controllers/src/speed_scaling_state_broadcaster.cpp @@ -66,7 +66,6 @@ controller_interface::CallbackReturn SpeedScalingStateBroadcaster::on_init() RCLCPP_INFO(get_node()->get_logger(), "Loading UR SpeedScalingStateBroadcaster with tf_prefix: %s", params_.tf_prefix.c_str()); - } catch (std::exception& e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; @@ -136,7 +135,7 @@ controller_interface::return_type SpeedScalingStateBroadcaster::update(const rcl { if (publish_rate_ > 0.0 && period > rclcpp::Duration(1.0 / publish_rate_, 0.0)) { // Speed scaling is the only interface of the controller - speed_scaling_state_msg_.data = state_interfaces_[0].get_value() * 100.0; + speed_scaling_state_msg_.data = state_interfaces_[0].get_optional().value_or(1.0) * 100.0; // publish speed_scaling_state_publisher_->publish(speed_scaling_state_msg_); diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp index 2c118fdcb..b3a69439f 100644 --- a/ur_controllers/src/ur_configuration_controller.cpp +++ b/ur_controllers/src/ur_configuration_controller.cpp @@ -95,10 +95,10 @@ controller_interface::CallbackReturn URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) { robot_software_version_.set([this](const std::shared_ptr ptr) { - ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value(); - ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value(); - ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value(); - ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value(); + ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_optional().value_or(0.0); + ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_optional().value_or(0.0); + ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_optional().value_or(0.0); + ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_optional().value_or(0.0); }); return controller_interface::CallbackReturn::SUCCESS; } From 8bc8813172d6f1827dcec565e15a78bdce622190 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 12 Mar 2025 09:32:21 +0100 Subject: [PATCH 2/2] freedrive_controller: By default use ASYNC_WAITING --- ur_controllers/src/freedrive_mode_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp index d5b823d53..fcda3141c 100644 --- a/ur_controllers/src/freedrive_mode_controller.cpp +++ b/ur_controllers/src/freedrive_mode_controller.cpp @@ -193,7 +193,7 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - async_state_ = async_success_command_interface_->get().get_optional().value(); + async_state_ = async_success_command_interface_->get().get_optional().value_or(ASYNC_WAITING); if (change_requested_) { bool write_success = true;