Add version check and fix formatting

Author: URJala

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -42,6 +42,8 @@
 #define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
 
 #include <chrono>
+#include <vector>
+#include <memory>
 
 #include <controller_interface/chainable_controller_interface.hpp>
 #include "std_msgs/msg/bool.hpp"
@@ -165,6 +167,7 @@ class ToolContactController : public controller_interface::ChainableControllerIn
   double old_reference_val = 0.0;
 
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_result_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_version_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> tool_contact_status_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> reference_interface_;
 
@@ -183,4 +186,4 @@ class ToolContactController : public controller_interface::ChainableControllerIn
 };
 }  // namespace ur_controllers
 
-#endif
+#endif  // UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_

ur_controllers/src/tool_contact_controller.cpp

@@ -44,7 +44,6 @@
 #include <rclcpp/logging.hpp>
 #include "std_msgs/msg/bool.hpp"
 #include <lifecycle_msgs/msg/state.hpp>
-#include <iostream>
 
 namespace ur_controllers
 {
@@ -89,6 +88,7 @@ controller_interface::InterfaceConfiguration ToolContactController::state_interf
 
   const std::string tf_prefix = tool_contact_params_.tf_prefix;
   config.names.push_back(tf_prefix + "tool_contact/tool_contact_result");
+  config.names.push_back(tf_prefix + "get_robot_software_version/get_version_major");
   return config;
 }
 
@@ -140,6 +140,27 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
       return controller_interface::CallbackReturn::ERROR;
     }
   }
+  {
+    const std::string interface_name = tool_contact_params_.tf_prefix + "get_robot_software_version/get_version_major";
+    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_version_interface_ = *it;
+      if (!tool_contact_result_interface_->get().get_value()) {
+        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;
+      }
+    } else {
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+  }
+  if (tool_contact_version_interface_->get().get_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;
+  }
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -420,4 +441,4 @@ controller_interface::return_type ToolContactController::update_and_write_comman
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(ur_controllers::ToolContactController, controller_interface::ChainableControllerInterface)
+PLUGINLIB_EXPORT_CLASS(ur_controllers::ToolContactController, controller_interface::ChainableControllerInterface)