Extract core logic to ethercat bus master

ethercat_driver/include/ethercat_driver/ethercat_bus_manager.hpp

@@ -0,0 +1,135 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_DRIVER__ETHERCAT_BUS_MANAGER_HPP_
+#define ETHERCAT_DRIVER__ETHERCAT_BUS_MANAGER_HPP_
+
+#include <cstddef>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <pluginlib/class_loader.hpp>
+
+#include "ethercat_interface/ec_master.hpp"
+#include "ethercat_interface/ec_slave.hpp"
+#include "yaml-cpp/yaml.h"
+
+namespace ethercat_driver
+{
+
+struct ConfiguredEcModule
+{
+  std::unordered_map<std::string, std::string> parameters;
+  std::vector<double> * input_values;
+  std::vector<double> * output_values;
+  std::string component_name;
+  std::string module_type;
+  size_t module_number;
+};
+
+/** Bus-wide EtherCAT settings independent of the ros2_control HardwareInfo representation. */
+struct EthercatBusConfig
+{
+  unsigned int master_id{0};
+  double control_frequency{100.0};
+  std::string transfer_config;
+  std::string fsoe_config;
+};
+
+enum class EthercatCycleResult
+{
+  kCompleted,
+  kSkippedInactive,
+  kSkippedBusy,
+  kError
+};
+
+class EthercatBusManager
+{
+public:
+  bool configureModules(
+    const EthercatBusConfig & bus_config,
+    const std::vector<ConfiguredEcModule> & modules);
+
+  bool activateBus();
+
+  void deactivateBus();
+
+  EthercatCycleResult read();
+
+  EthercatCycleResult write();
+
+  /** @brief Get transfer module parameters from YAML file
+   * @param[in] config YAML node containing the transfer configuration root
+   * @return Vector of maps containing transfer module parameters, each map corresponds to a module
+   * involved in a transfer
+   */
+  std::vector<std::unordered_map<std::string, std::string>> getEcTransferModuleParam(
+    const YAML::Node & config);
+
+  /** @brief Get transfer nets from YAML file
+   * @param[in] config YAML node containing the transfer configuration root
+   * @return Vector of transfer nets
+   */
+  std::vector<ethercat_interface::EcTransferNet> getEcTransferNets(const YAML::Node & config);
+
+protected:
+  uint16_t getAliasOrDefaultAlias(
+    const std::unordered_map<std::string,
+    std::string> & slave_parameters);
+
+  /** @brief Load transfer config YAML file
+   * One use case is to load transfers for FailSafe Over EtherCAT Safety
+   * @param[out] node YAML node containing the transfer configuration root
+   * @param[in] path Path to the YAML file, if empty, the file is loaded from the *fsoe_config*
+   * or *transfer_config* of the YAML document
+   */
+  void loadTransferConfigYamlFile(YAML::Node & node, const std::string & path = "");
+
+  bool setupMaster();
+
+  bool configNetwork();
+
+protected:
+  EthercatBusConfig bus_config_;
+  std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
+  std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
+
+  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
+    "ethercat_interface", "ethercat_interface::EcSlave"};
+
+  double control_frequency_;
+
+  std::shared_ptr<ethercat_interface::EcMaster> master_;
+  std::mutex ec_mutex_;
+  bool activated_{false};
+
+  /** Transfer nets */
+  std::vector<ethercat_interface::EcTransferNet> ec_transfer_nets_;
+
+  /** Indexes of modules inside ec_modules_ vector that are transfer masters */
+  std::vector<size_t> ec_transfer_masters_;
+  /** Indexes of modules inside ec_modules_ vector that are transfer slaves only */
+  std::vector<size_t> ec_transfer_slaves_;
+
+  /** Empty interfaces */
+  std::vector<double> empty_interface_;
+};
+
+}  // namespace ethercat_driver
+
+#endif  // ETHERCAT_DRIVER__ETHERCAT_BUS_MANAGER_HPP_

ethercat_driver/include/ethercat_driver/ethercat_driver.hpp

@@ -19,18 +19,15 @@
 #include <memory>
 #include <string>
 #include <vector>
-#include <pluginlib/class_loader.hpp>
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "ethercat_driver/ethercat_bus_manager.hpp"
 #include "ethercat_driver/visibility_control.h"
-#include "ethercat_interface/ec_slave.hpp"
-#include "ethercat_interface/ec_master.hpp"
-#include "yaml-cpp/yaml.h"
 
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
@@ -68,75 +65,14 @@ class EthercatDriver : public hardware_interface::SystemInterface
   hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
 
 protected:
-  std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
-    const std::string & urdf,
-    const std::string & component_name,
-    const std::string & component_type);
-
-  uint16_t getAliasOrDefaultAlias(
-    const std::unordered_map<std::string,
-    std::string> & slave_parameters);
-
-  virtual CallbackReturn setupMaster();
-
-  CallbackReturn configNetwork();
-
-  /** @brief Load transfer config YAML file
-   * One use case is to load transfers for FailSafe Over EtherCAT Safety
-   * @param[out] node YAML node containing the transfer configuration root
-   * @param[in] path Path to the YAML file, if empty, the file is loaded from the *fsoe_config*
-   * or *transfer_config* of the YAML document
-   */
-  void loadTransferConfigYamlFile(YAML::Node & node, const std::string & path = "");
-
-  /** @brief Get transfer module parameters from YAML file
-   * @param[in] config YAML node containing the transfer configuration root
-   * @return Vector of maps containing transfer module parameters, each map corresponds to a module
-   * involved in a transfer
-   */
-  std::vector<std::unordered_map<std::string, std::string>> getEcTransferModuleParam(
-    const YAML::Node & config);
-
-  /** @brief Get transfer nets from YAML file
-   * @param[in] config YAML node containing the transfer configuration root
-   * @return Vector of transfer nets
-   */
-  std::vector<ethercat_interface::EcTransferNet> getEcTransferNets(const YAML::Node & config);
-
-  /** @brief Configure the transfer networks
-   */
-  void configTransferNetwork();
-
-protected:
-  std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
-  std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
-
   std::vector<std::vector<double>> hw_joint_commands_;
   std::vector<std::vector<double>> hw_sensor_commands_;
   std::vector<std::vector<double>> hw_gpio_commands_;
   std::vector<std::vector<double>> hw_joint_states_;
   std::vector<std::vector<double>> hw_sensor_states_;
   std::vector<std::vector<double>> hw_gpio_states_;
 
-  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
-    "ethercat_interface", "ethercat_interface::EcSlave"};
-
-  double control_frequency_;
-
-  std::shared_ptr<ethercat_interface::EcMaster> master_;
-  std::mutex ec_mutex_;
-  bool activated_;
-
-  /** Transfer nets */
-  std::vector<ethercat_interface::EcTransferNet> ec_transfer_nets_;
-
-  /** Indexes of modules inside ec_modules_ vector that are transfer masters */
-  std::vector<size_t> ec_transfer_masters_;
-  /** Indexes of modules inside ec_modules_ vector that are transfer slaves only */
-  std::vector<size_t> ec_transfer_slaves_;
-
-  /** Empty interfaces */
-  std::vector<double> empty_interface_;
+  EthercatBusManager bus_manager_;
 };
 }  // namespace ethercat_driver
 

ethercat_driver/include/ethercat_driver/ethercat_ros2_control_xml_parser.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_DRIVER__ETHERCAT_ROS2_CONTROL_XML_PARSER_HPP_
+#define ETHERCAT_DRIVER__ETHERCAT_ROS2_CONTROL_XML_PARSER_HPP_
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace ethercat_driver
+{
+
+/** @brief Parse <ec_module> blocks for a given ros2_control component from a URDF/Xacro string.
+ * @param[in] urdf Full URDF/Xacro XML string (typically info_.original_xml).
+ * @param[in] component_name Name attribute of the joint, gpio or sensor element to look under.
+ * @param[in] component_type Element tag to look for: "joint", "gpio", or "sensor".
+ * @return One parameter map per <ec_module> child of the matching component.
+ */
+std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
+  const std::string & urdf,
+  const std::string & component_name,
+  const std::string & component_type);
+
+}  // namespace ethercat_driver
+
+#endif  // ETHERCAT_DRIVER__ETHERCAT_ROS2_CONTROL_XML_PARSER_HPP_