Skip to content

Update ros2_control API to get_optional #1289

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Mar 12, 2025
Merged

Update ros2_control API to get_optional #1289

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
06ce8ff
Update ros2_control API to get_optional
urfeex Mar 5, 2025
8bc8813
freedrive_controller: By default use ASYNC_WAITING
urfeex Mar 12, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ur_controllers/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
3 changes: 2 additions & 1 deletion ur_controllers/src/force_mode_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,8 @@
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);

Check warning on line 177 in ur_controllers/src/force_mode_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/force_mode_controller.cpp#L176-L177 

Added lines #L176 - L177 were not covered by tests

// Publish state of force_mode?
if (change_requested_) {
Expand Down
6 changes: 3 additions & 3 deletions ur_controllers/src/freedrive_mode_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -193,15 +193,15 @@
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_or(ASYNC_WAITING);

Check warning on line 196 in ur_controllers/src/freedrive_mode_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/freedrive_mode_controller.cpp#L196 

Added line #L196 was not covered by tests

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) {

Check warning on line 204 in ur_controllers/src/freedrive_mode_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/freedrive_mode_controller.cpp#L204 

Added line #L204 was not covered by tests
RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request.");
freedrive_active_ = false;
return controller_interface::return_type::OK;
Expand Down
112 changes: 69 additions & 43 deletions ur_controllers/src/gpio_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,53 +197,56 @@
{
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<bool>(state_interfaces_[i].get_value());
io_msg_.digital_out_states[i].state = static_cast<bool>(state_interfaces_[i].get_optional().value_or(0.0));

Check warning on line 200 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L200 

Added line #L200 was not covered by tests

io_msg_.digital_in_states[i].pin = i;
io_msg_.digital_in_states[i].state =
static_cast<bool>(state_interfaces_[i + StateInterfaces::DIGITAL_INPUTS].get_value());
static_cast<bool>(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<float>(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_value());
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_optional().value_or(0.0));
io_msg_.analog_in_states[i].domain =
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES].get_value());
static_cast<uint8_t>(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<float>(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_value());
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_optional().value_or(0.0));
io_msg_.analog_out_states[i].domain =
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_value());
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_optional().value_or(0.0));
}

io_pub_->publish(io_msg_);
}

void GPIOController::publishToolData()
{
tool_data_msg_.tool_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_MODE].get_value());
tool_data_msg_.tool_mode =

Check warning on line 228 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L228 

Added line #L228 was not covered by tests
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_MODE].get_optional().value_or(0.0));
tool_data_msg_.analog_input_range2 =
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_value());
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_optional().value_or(0.0));
tool_data_msg_.analog_input_range3 =
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_value());
tool_data_msg_.analog_input2 = static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_value());
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_optional().value_or(0.0));
tool_data_msg_.analog_input2 =

Check warning on line 234 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L234 

Added line #L234 was not covered by tests
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_optional().value_or(0.0));
tool_data_msg_.analog_input3 =
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_value());
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_optional().value_or(0.0));
tool_data_msg_.tool_output_voltage =
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_value());
tool_data_msg_.tool_current = static_cast<float>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_value());
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_optional().value_or(0.0));
tool_data_msg_.tool_current =

Check warning on line 240 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L240 

Added line #L240 was not covered by tests
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_optional().value_or(0.0));
tool_data_msg_.tool_temperature =
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_value());
static_cast<float>(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<int8_t>(state_interfaces_[StateInterfaces::ROBOT_MODE].get_value());
auto robot_mode = static_cast<int8_t>(state_interfaces_[StateInterfaces::ROBOT_MODE].get_optional().value_or(0.0));

Check warning on line 249 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L249 

Added line #L249 was not covered by tests

if (robot_mode_msg_.mode != robot_mode) {
robot_mode_msg_.mode = robot_mode;
Expand All @@ -253,7 +256,7 @@

void GPIOController::publishSafetyMode()
{
auto safety_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::SAFETY_MODE].get_value());
auto safety_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::SAFETY_MODE].get_optional().value_or(0.0));

Check warning on line 259 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L259 

Added line #L259 was not covered by tests

if (safety_mode_msg_.mode != safety_mode) {
safety_mode_msg_.mode = safety_mode;
Expand All @@ -263,7 +266,8 @@

void GPIOController::publishProgramRunning()
{
auto program_running_value = static_cast<uint8_t>(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_value());
auto program_running_value =
static_cast<uint8_t>(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;
Expand All @@ -274,7 +278,7 @@
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));
}
Expand Down Expand Up @@ -350,12 +354,14 @@

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);

Check warning on line 358 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L357-L358 

Added lines #L357 - L358 were not covered by tests
})) {
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<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 364 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L364 

Added line #L364 was not covered by tests
return resp->success;
} else if (req->fun == req->FUN_SET_ANALOG_OUT && req->pin >= 0 && req->pin <= 2) {
// io async success
Expand All @@ -365,25 +371,31 @@

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);

Check warning on line 375 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L374-L375 

Added lines #L374 - L375 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 382 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L381-L382 

Added lines #L381 - L382 were not covered by tests
return resp->success;
} else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) {
std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
std::ignore = command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast<double>(req->state));

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);

Check warning on line 391 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L390-L391 

Added lines #L390 - L391 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 398 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L397-L398 

Added lines #L397 - L398 were not covered by tests
return resp->success;
} else {
resp->success = false;
Expand Down Expand Up @@ -423,12 +435,14 @@
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);

Check warning on line 439 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L438-L439 

Added lines #L438 - L439 were not covered by tests
})) {
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<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 445 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L445 

Added line #L445 was not covered by tests
return resp->success;
}

Expand All @@ -444,13 +458,15 @@
static_cast<double>(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);

Check warning on line 462 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L461-L462 

Added lines #L461 - L462 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_optional().value_or(

Check warning on line 468 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L467-L468 

Added lines #L467 - L468 were not covered by tests
ASYNC_WAITING));
} else {
RCLCPP_WARN(get_node()->get_logger(), "The desired speed slider fraction must be within range (0; 1.0]. Request "
"ignored.");
Expand All @@ -468,13 +484,16 @@
// 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);

Check warning on line 489 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L487-L489 

Added lines #L487 - L489 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_optional().value_or(

Check warning on line 495 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L494-L495 

Added lines #L494 - L495 were not covered by tests
ASYNC_WAITING));

if (resp->success) {
RCLCPP_INFO(get_node()->get_logger(), "Successfully resent robot program");
Expand All @@ -494,13 +513,15 @@
// 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);

Check warning on line 518 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L516-L518 

Added lines #L516 - L518 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 524 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L523-L524 

Added lines #L523 - L524 were not covered by tests

if (resp->success) {
RCLCPP_INFO(get_node()->get_logger(), "Deactivated control");
Expand All @@ -523,13 +544,15 @@
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);

Check warning on line 548 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L547-L548 

Added lines #L547 - L548 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 555 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L554-L555 

Added lines #L554 - L555 were not covered by tests

if (resp->success) {
RCLCPP_INFO(get_node()->get_logger(), "Payload has been set successfully");
Expand All @@ -549,13 +572,16 @@
// 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);

Check warning on line 577 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L575-L577 

Added lines #L575 - L577 were not covered by tests
})) {
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<bool>(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value());
resp->success = static_cast<bool>(
command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));

Check warning on line 584 in ur_controllers/src/gpio_controller.cpp

View check run for this annotation

Codecov / codecov/patch

ur_controllers/src/gpio_controller.cpp#L583-L584 

Added lines #L583 - L584 were not covered by tests

if (resp->success) {
RCLCPP_INFO(get_node()->get_logger(), "Successfully zeroed the force torque sensor");
Expand Down
Loading
Loading