Update to use std::optional

Author: URJala

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -103,8 +103,6 @@ class ToolContactController : public controller_interface::ControllerInterface
   rclcpp_action::CancelResponse goal_cancelled_callback(
       const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
 
-  double tool_contact_active_state_interface;
-
   std::atomic<bool> tool_contact_enable_ = false;
   std::atomic<bool> tool_contact_active_ = false;
   std::atomic<bool> tool_contact_abort_ = false;
@@ -116,7 +114,6 @@ class ToolContactController : public controller_interface::ControllerInterface
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> major_version_state_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_state_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> tool_contact_set_state_interface_;
-  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> reference_interface_;
 
   rclcpp_action::Server<ur_msgs::action::ToolContact>::SharedPtr tool_contact_action_server_;
 

ur_controllers/src/tool_contact_controller.cpp

@@ -38,6 +38,7 @@
  */
 //----------------------------------------------------------------------
 
+#include <optional>
 #include <ur_controllers/tool_contact_controller.hpp>
 #include "controller_interface/helpers.hpp"
 #include <rclcpp/rclcpp.hpp>
@@ -73,7 +74,6 @@ controller_interface::InterfaceConfiguration ToolContactController::command_inte
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   const std::string tf_prefix = tool_contact_params_.tf_prefix;
 
-  // Start or stop tool contact functionality
   config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_set_state");
   return config;
 }
@@ -93,14 +93,15 @@ controller_interface::InterfaceConfiguration ToolContactController::state_interf
 controller_interface::CallbackReturn
 ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
 {
-  double val;
+  std::optional<double> val;
   {
     const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_state";
     auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(),
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != state_interfaces_.end()) {
       tool_contact_state_interface_ = *it;
-      if (!tool_contact_state_interface_->get().get_value(val)) {
+      val = tool_contact_state_interface_->get().get_optional();
+      if (!val) {
         RCLCPP_ERROR(get_node()->get_logger(),
                      "Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
         return controller_interface::CallbackReturn::ERROR;
@@ -133,7 +134,8 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != state_interfaces_.end()) {
       tool_contact_result_interface_ = *it;
-      if (!tool_contact_result_interface_->get().get_value(val)) {
+      val = tool_contact_result_interface_->get().get_optional();
+      if (!val) {
         RCLCPP_ERROR(get_node()->get_logger(),
                      "Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
         return controller_interface::CallbackReturn::ERROR;
@@ -149,13 +151,13 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != state_interfaces_.end()) {
       major_version_state_interface_ = *it;
-      double major_version = 0.0;
-      if (!tool_contact_result_interface_->get().get_value(major_version)) {
+      std::optional<double> major_version = tool_contact_result_interface_->get().get_optional();
+      if (!major_version) {
         RCLCPP_ERROR(get_node()->get_logger(),
                      "Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
         return controller_interface::CallbackReturn::ERROR;
       }
-      if (major_version < 5) {
+      if (major_version.value() < 5) {
         RCLCPP_ERROR(get_node()->get_logger(), "This feature is not supported on CB3 robots, controller will not be "
                                                "started.");
         return controller_interface::CallbackReturn::ERROR;
@@ -284,8 +286,14 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
   }
 
   const auto active_goal = *rt_active_goal_.readFromRT();
+  std::optional<double> state_optional = tool_contact_state_interface_->get().get_optional();
+
+  if (!state_optional) {
+    RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to read state interface, aborting.");
+    return controller_interface::return_type::ERROR;
+  }
+  const int state = static_cast<int>(state_optional.value());
 
-  const int state = static_cast<int>(tool_contact_state_interface_->get().get_value());
   switch (state) {
     case static_cast<int>(TOOL_CONTACT_EXECUTING):
     {
@@ -294,8 +302,12 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
         RCLCPP_INFO(get_node()->get_logger(), "Tool contact enabled successfully.");
         logged_once_ = true;
       }
-      double result = tool_contact_result_interface_->get().get_value();
-      if (result == 0.0) {
+      std::optional<double> result = tool_contact_result_interface_->get().get_optional();
+      if (!result) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to read result interface, aborting.");
+        return controller_interface::return_type::ERROR;
+      }
+      if (result.value() == 0.0) {
         tool_contact_active_ = false;
         RCLCPP_INFO(get_node()->get_logger(), "Tool contact finished successfully.");
 
@@ -305,7 +317,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
           active_goal->setSucceeded(result);
           should_reset_goal = true;
         }
-      } else if (result == 1.0) {
+      } else if (result.value() == 1.0) {
         tool_contact_active_ = false;
         RCLCPP_ERROR(get_node()->get_logger(), "Tool contact aborted by hardware.");
 
@@ -357,11 +369,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
     default:
       break;
   }
-  if (tool_contact_result_interface_->get().get_value() == 0.0) {
-    tool_contact_active_state_interface = 2.0;
-  } else {
-    tool_contact_active_state_interface = static_cast<double>(tool_contact_active_);
-  }
+
   if (!write_success) {
     RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to update or read command/state interface.");
     return controller_interface::return_type::ERROR;