Removed unused clients and control mode check added

Now the CbHwFwPos::_checkJoints method checks also whether or not the
joints included in the ros2_control configuration are in the "position"
control mode. If not and error will be thrown

ros2_components_ws/src/cb_hw_fw_pos/include/cb_hw_fw_pos/cb_hw_fw_pos.hpp

@@ -57,9 +57,7 @@ class CbHwFwPos : public hardware_interface::SystemInterface
     std::string  m_posTopicName;                 // Position commands topic
     std::string  m_getModesClientName;           // Service client for joints current control modes
     std::string  m_getPositionClientName;        // Service client to get current position values
-    std::string  m_setModesClientName;           // Service client to set joints control modes
     std::string  m_getJointsNamesClientName;     // Service client to get the available joints names
-    std::string  m_getAvailableModesClientName;  // Services client to get the available control modes
     mutable std::mutex       m_cmdMutex;
     std::vector<std::string> m_jointNames; // name of the joints
 
@@ -82,8 +80,6 @@ class CbHwFwPos : public hardware_interface::SystemInterface
     rclcpp::Client<yarp_control_msgs::srv::GetJointsNames>::SharedPtr           m_getJointsNamesClient;
     rclcpp::Client<yarp_control_msgs::srv::GetControlModes>::SharedPtr          m_getControlModesClient;
     rclcpp::Client<yarp_control_msgs::srv::GetPosition>::SharedPtr              m_getPositionClient;
-    rclcpp::Client<yarp_control_msgs::srv::SetControlModes>::SharedPtr          m_setControlModesClient;
-    rclcpp::Client<yarp_control_msgs::srv::GetAvailableControlModes>::SharedPtr m_getAvailableModesClient;
 
     // Internal functions
     /* Check whether or not the joints specified in the ros2 robot configuration files are actually

ros2_components_ws/src/cb_hw_fw_pos/src/cb_hw_fw_pos.cpp

@@ -36,8 +36,9 @@ bool CbHwFwPos::_checkJoints(const std::vector<hardware_interface::ComponentInfo
         }
         RCLCPP_INFO(m_node->get_logger(), "service not available, waiting again...");
     }
-    auto namesResponse = m_getJointsNamesClient->async_send_request(namesRequest);
-    if(rclcpp::spin_until_future_complete(m_node, namesResponse) == rclcpp::FutureReturnCode::SUCCESS) {
+    auto namesFuture = m_getJointsNamesClient->async_send_request(namesRequest);
+    if(rclcpp::spin_until_future_complete(m_node, namesFuture) == rclcpp::FutureReturnCode::SUCCESS) {
+        auto namesResponse = namesFuture.get();
         RCLCPP_INFO(m_node->get_logger(), "Got joints names");
         all_joints = namesResponse.get()->names;
     }
@@ -55,6 +56,33 @@ bool CbHwFwPos::_checkJoints(const std::vector<hardware_interface::ComponentInfo
         }
         m_jointNames.push_back(joint.name);
     }
+
+    auto modeRequest = std::make_shared<yarp_control_msgs::srv::GetControlModes::Request>();
+    while (!m_getControlModesClient->wait_for_service(1s)) {
+        if (!rclcpp::ok()) {
+            RCLCPP_ERROR(m_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
+            return false;
+        }
+        RCLCPP_INFO(m_node->get_logger(), "service not available, waiting again...");
+    }
+    modeRequest->names = m_jointNames;
+    auto modeFuture = m_getControlModesClient->async_send_request(modeRequest);
+    if(rclcpp::spin_until_future_complete(m_node, modeFuture) == rclcpp::FutureReturnCode::SUCCESS) {
+        auto modeResponse = modeFuture.get();
+        for(size_t i=0; i<m_jointNames.size(); i++)
+        {
+            if(modeResponse->modes[i] != "POSITION")
+            {
+                RCLCPP_ERROR(m_node->get_logger(), "Joint %s in not in control mode POSITION. Check your configuration and, if possible, change the joint control mode",m_jointNames[i].c_str());
+                return false;
+            }
+        }
+    }
+    else {
+        RCLCPP_ERROR(m_node->get_logger(),"Failed to get joints control modes");
+        return false;
+    }
+
     return true;
 }
 
@@ -161,9 +189,7 @@ CallbackReturn CbHwFwPos::on_init(const hardware_interface::HardwareInfo & info)
     // Initialize topics and services names ------------------------------------------------------------------- //
     m_posTopicName = m_msgs_name+"/position";
     m_getModesClientName = m_msgs_name+"/get_modes";
-    m_setModesClientName = m_msgs_name+"/set_modes";
     m_getPositionClientName = m_msgs_name+"/get_position";
-    m_getAvailableModesClientName = m_msgs_name+"/get_available_modes";
     m_getJointsNamesClientName = m_msgs_name+"/get_joints_names";
 
     // Initialize publishers ---------------------------------------------------------------------------------- //
@@ -189,16 +215,6 @@ CallbackReturn CbHwFwPos::on_init(const hardware_interface::HardwareInfo & info)
         RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetPosition service client");
         return CallbackReturn::ERROR;
     }
-    m_setControlModesClient = m_node->create_client<yarp_control_msgs::srv::SetControlModes>(m_setModesClientName);
-    if(!m_setControlModesClient){
-        RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the SetControlModes service client");
-        return CallbackReturn::ERROR;
-    }
-    m_getAvailableModesClient = m_node->create_client<yarp_control_msgs::srv::GetAvailableControlModes>(m_getAvailableModesClientName);
-    if(!m_getAvailableModesClient){
-        RCLCPP_ERROR(m_node->get_logger(),"Could not initialize the GetAvailableControlModes service client");
-        return CallbackReturn::ERROR;
-    }
 
     return _initExportableInterfaces(info_.joints);
 }