Refactor prepare_switch

Refactored prepare_switch mechanism. This way it should be easier to
understand and to extend.

Author: urfeex

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -40,10 +40,11 @@
 #define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_
 
 // System
+#include <limits>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
-#include <limits>
 
 // ros2_control hardware_interface
 #include "hardware_interface/hardware_info.hpp"
@@ -115,6 +116,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(URPositionHardwareInterface);
+  URPositionHardwareInterface();
   virtual ~URPositionHardwareInterface();
 
   hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) final;
@@ -319,6 +321,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   const std::string FORCE_MODE_GPIO = "force_mode";
   const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
   const std::string TOOL_CONTACT_GPIO = "tool_contact";
+
+  std::unordered_map<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
 };
 }  // namespace ur_robot_driver