Skip to content

Commit 57d8fab

Browse files
urfeexurfeex
authored
Update ros2_control API to get_optional (#1289)
1 parent 1af8c8d commit 57d8fab

8 files changed

+101
-65
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ ament_target_dependencies(${PROJECT_NAME}
111111
${THIS_PACKAGE_INCLUDE_DEPENDS}
112112
)
113113

114-
target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type)
114+
target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror)
115115

116116
# prevent pluginlib from using boost
117117
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

‎ur_controllers/src/force_mode_controller.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -173,7 +173,8 @@ ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& /
173173
controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/,
174174
const rclcpp::Duration& /*period*/)
175175
{
176-
async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value();
176+
async_state_ =
177+
command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
177178

178179
// Publish state of force_mode?
179180
if (change_requested_) {

‎ur_controllers/src/freedrive_mode_controller.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -193,15 +193,15 @@ ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::S
193193
controller_interface::return_type ur_controllers::FreedriveModeController::update(const rclcpp::Time& /*time*/,
194194
const rclcpp::Duration& /*period*/)
195195
{
196-
async_state_ = async_success_command_interface_->get().get_value();
196+
async_state_ = async_success_command_interface_->get().get_optional().value_or(ASYNC_WAITING);
197197

198198
if (change_requested_) {
199199
bool write_success = true;
200200
if (freedrive_active_) {
201201
// Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the
202202
// teach pendant.
203-
if (!std::isnan(abort_command_interface_->get().get_value()) &&
204-
abort_command_interface_->get().get_value() == 1.0) {
203+
if ((abort_command_interface_->get().get_optional().has_value()) &&
204+
abort_command_interface_->get().get_optional().value() == 1.0) {
205205
RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode aborted by hardware, aborting request.");
206206
freedrive_active_ = false;
207207
return controller_interface::return_type::OK;

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 69 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -197,53 +197,56 @@ void GPIOController::publishIO()
197197
{
198198
for (size_t i = 0; i < 18; ++i) {
199199
io_msg_.digital_out_states[i].pin = i;
200-
io_msg_.digital_out_states[i].state = static_cast<bool>(state_interfaces_[i].get_value());
200+
io_msg_.digital_out_states[i].state = static_cast<bool>(state_interfaces_[i].get_optional().value_or(0.0));
201201

202202
io_msg_.digital_in_states[i].pin = i;
203203
io_msg_.digital_in_states[i].state =
204-
static_cast<bool>(state_interfaces_[i + StateInterfaces::DIGITAL_INPUTS].get_value());
204+
static_cast<bool>(state_interfaces_[i + StateInterfaces::DIGITAL_INPUTS].get_optional().value_or(0.0));
205205
}
206206

207207
for (size_t i = 0; i < 2; ++i) {
208208
io_msg_.analog_in_states[i].pin = i;
209209
io_msg_.analog_in_states[i].state =
210-
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_value());
210+
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_INPUTS].get_optional().value_or(0.0));
211211
io_msg_.analog_in_states[i].domain =
212-
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES].get_value());
212+
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES].get_optional().value_or(0.0));
213213
}
214214

215215
for (size_t i = 0; i < 2; ++i) {
216216
io_msg_.analog_out_states[i].pin = i;
217217
io_msg_.analog_out_states[i].state =
218-
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_value());
218+
static_cast<float>(state_interfaces_[i + StateInterfaces::ANALOG_OUTPUTS].get_optional().value_or(0.0));
219219
io_msg_.analog_out_states[i].domain =
220-
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_value());
220+
static_cast<uint8_t>(state_interfaces_[i + StateInterfaces::ANALOG_IO_TYPES + 2].get_optional().value_or(0.0));
221221
}
222222

223223
io_pub_->publish(io_msg_);
224224
}
225225

226226
void GPIOController::publishToolData()
227227
{
228-
tool_data_msg_.tool_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_MODE].get_value());
228+
tool_data_msg_.tool_mode =
229+
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_MODE].get_optional().value_or(0.0));
229230
tool_data_msg_.analog_input_range2 =
230-
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_value());
231+
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES].get_optional().value_or(0.0));
231232
tool_data_msg_.analog_input_range3 =
232-
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_value());
233-
tool_data_msg_.analog_input2 = static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_value());
233+
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_ANALOG_IO_TYPES + 1].get_optional().value_or(0.0));
234+
tool_data_msg_.analog_input2 =
235+
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS].get_optional().value_or(0.0));
234236
tool_data_msg_.analog_input3 =
235-
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_value());
237+
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_ANALOG_INPUTS + 1].get_optional().value_or(0.0));
236238
tool_data_msg_.tool_output_voltage =
237-
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_value());
238-
tool_data_msg_.tool_current = static_cast<float>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_value());
239+
static_cast<uint8_t>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_VOLTAGE].get_optional().value_or(0.0));
240+
tool_data_msg_.tool_current =
241+
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_OUTPUT_CURRENT].get_optional().value_or(0.0));
239242
tool_data_msg_.tool_temperature =
240-
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_value());
243+
static_cast<float>(state_interfaces_[StateInterfaces::TOOL_TEMPERATURE].get_optional().value_or(0.0));
241244
tool_data_pub_->publish(tool_data_msg_);
242245
}
243246

244247
void GPIOController::publishRobotMode()
245248
{
246-
auto robot_mode = static_cast<int8_t>(state_interfaces_[StateInterfaces::ROBOT_MODE].get_value());
249+
auto robot_mode = static_cast<int8_t>(state_interfaces_[StateInterfaces::ROBOT_MODE].get_optional().value_or(0.0));
247250

248251
if (robot_mode_msg_.mode != robot_mode) {
249252
robot_mode_msg_.mode = robot_mode;
@@ -253,7 +256,7 @@ void GPIOController::publishRobotMode()
253256

254257
void GPIOController::publishSafetyMode()
255258
{
256-
auto safety_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::SAFETY_MODE].get_value());
259+
auto safety_mode = static_cast<uint8_t>(state_interfaces_[StateInterfaces::SAFETY_MODE].get_optional().value_or(0.0));
257260

258261
if (safety_mode_msg_.mode != safety_mode) {
259262
safety_mode_msg_.mode = safety_mode;
@@ -263,7 +266,8 @@ void GPIOController::publishSafetyMode()
263266

264267
void GPIOController::publishProgramRunning()
265268
{
266-
auto program_running_value = static_cast<uint8_t>(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_value());
269+
auto program_running_value =
270+
static_cast<uint8_t>(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_optional().value_or(0.0));
267271
bool program_running = program_running_value == 1.0 ? true : false;
268272
if (program_running_msg_.data != program_running) {
269273
program_running_msg_.data = program_running;
@@ -274,7 +278,7 @@ void GPIOController::publishProgramRunning()
274278
controller_interface::CallbackReturn
275279
ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
276280
{
277-
while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) {
281+
while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_optional().value_or(0.0) == 0.0) {
278282
RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize...");
279283
std::this_thread::sleep_for(std::chrono::milliseconds(50));
280284
}
@@ -350,12 +354,14 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
350354

351355
RCLCPP_INFO(get_node()->get_logger(), "Setting digital output '%d' to state: '%1.0f'.", req->pin, req->state);
352356

353-
if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
357+
if (!waitForAsyncCommand([&]() {
358+
return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
359+
})) {
354360
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
355361
"mocked interface)");
356362
}
357363

358-
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_value());
364+
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
359365
return resp->success;
360366
} else if (req->fun == req->FUN_SET_ANALOG_OUT && req->pin >= 0 && req->pin <= 2) {
361367
// io async success
@@ -365,25 +371,31 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
365371

366372
RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state);
367373

368-
if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
374+
if (!waitForAsyncCommand([&]() {
375+
return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
376+
})) {
369377
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
370378
"mocked interface)");
371379
}
372380

373-
resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
381+
resp->success = static_cast<bool>(
382+
command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
374383
return resp->success;
375384
} else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) {
376385
std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
377386
std::ignore = command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast<double>(req->state));
378387

379388
RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state);
380389

381-
if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
390+
if (!waitForAsyncCommand([&]() {
391+
return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
392+
})) {
382393
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
383394
"mocked interface)");
384395
}
385396

386-
resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
397+
resp->success = static_cast<bool>(
398+
command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
387399
return resp->success;
388400
} else {
389401
resp->success = false;
@@ -423,12 +435,14 @@ bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::Sha
423435
RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin,
424436
req->data.state, domain_string.c_str());
425437

426-
if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
438+
if (!waitForAsyncCommand([&]() {
439+
return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
440+
})) {
427441
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
428442
"mocked interface)");
429443
}
430444

431-
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_value());
445+
resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
432446
return resp->success;
433447
}
434448

@@ -444,13 +458,15 @@ bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Reques
444458
static_cast<double>(req->speed_slider_fraction));
445459

446460
if (!waitForAsyncCommand([&]() {
447-
return command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value();
461+
return command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_optional().value_or(
462+
ASYNC_WAITING);
448463
})) {
449464
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that target speed fraction was set. (This might happen "
450465
"when using the mocked interface)");
451466
}
452-
resp->success =
453-
static_cast<bool>(command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value());
467+
resp->success = static_cast<bool>(
468+
command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_optional().value_or(
469+
ASYNC_WAITING));
454470
} else {
455471
RCLCPP_WARN(get_node()->get_logger(), "The desired speed slider fraction must be within range (0; 1.0]. Request "
456472
"ignored.");
@@ -468,13 +484,16 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP
468484
// call the service in the hardware
469485
std::ignore = command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_CMD].set_value(1.0);
470486

471-
if (!waitForAsyncCommand(
472-
[&]() { return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value(); })) {
487+
if (!waitForAsyncCommand([&]() {
488+
return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_optional().value_or(
489+
ASYNC_WAITING);
490+
})) {
473491
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that program was sent. (This might happen when using the "
474492
"mocked interface)");
475493
}
476-
resp->success =
477-
static_cast<bool>(command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value());
494+
resp->success = static_cast<bool>(
495+
command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_optional().value_or(
496+
ASYNC_WAITING));
478497

479498
if (resp->success) {
480499
RCLCPP_INFO(get_node()->get_logger(), "Successfully resent robot program");
@@ -494,13 +513,15 @@ bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr
494513
// call the service in the hardware
495514
std::ignore = command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_CMD].set_value(1.0);
496515

497-
if (!waitForAsyncCommand(
498-
[&]() { return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value(); })) {
516+
if (!waitForAsyncCommand([&]() {
517+
return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_optional().value_or(
518+
ASYNC_WAITING);
519+
})) {
499520
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that hand_back_control was correctly triggered. (This "
500521
"might happen when using the mocked interface)");
501522
}
502-
resp->success =
503-
static_cast<bool>(command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value());
523+
resp->success = static_cast<bool>(
524+
command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
504525

505526
if (resp->success) {
506527
RCLCPP_INFO(get_node()->get_logger(), "Deactivated control");
@@ -523,13 +544,15 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
523544
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
524545
std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
525546

526-
if (!waitForAsyncCommand(
527-
[&]() { return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value(); })) {
547+
if (!waitForAsyncCommand([&]() {
548+
return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING);
549+
})) {
528550
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that payload was set. (This might happen when using the "
529551
"mocked interface)");
530552
}
531553

532-
resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value());
554+
resp->success = static_cast<bool>(
555+
command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
533556

534557
if (resp->success) {
535558
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
549572
// call the service in the hardware
550573
std::ignore = command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0);
551574

552-
if (!waitForAsyncCommand(
553-
[&]() { return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value(); })) {
575+
if (!waitForAsyncCommand([&]() {
576+
return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_optional().value_or(
577+
ASYNC_WAITING);
578+
})) {
554579
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that FTS was zeroed. (This might happen when using the "
555580
"mocked interface)");
556581
}
557582

558-
resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value());
583+
resp->success = static_cast<bool>(
584+
command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_optional().value_or(ASYNC_WAITING));
559585

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

0 commit comments

Comments
 (0)