Ethercat master abstraction

ethercat_driver/CMakeLists.txt

@@ -51,7 +51,24 @@ install(
 )
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
+  # find_package(ament_cmake_gmock REQUIRED)
+  # find_package(ament_lint_auto REQUIRED)
+  # find_package(pluginlib REQUIRED)
+  # find_package(ethercat_interface REQUIRED)
+  # find_package(ethercat_master_mock REQUIRED)
+  # find_package(ros2_control_test_assets REQUIRED)
+  # ament_lint_auto_find_test_dependencies()
+
+  # ament_add_gmock(test_ethercat_driver
+  #   test/test_ethercat_driver.cpp)
+  # target_include_directories(test_ethercat_driver PRIVATE include)
+  # target_link_libraries(test_ethercat_driver ethercat_driver)
+  # ament_target_dependencies(test_ethercat_driver
+  #   pluginlib
+  #   ros2_control_test_assets
+  #   ethercat_interface
+  #   ethercat_master_mock
+  # )
 endif()
 
 ## EXPORTS

ethercat_driver/include/ethercat_driver/ethercat_driver.hpp

@@ -79,11 +79,12 @@ class EthercatDriver : public hardware_interface::SystemInterface
   std::vector<std::vector<double>> hw_sensor_states_;
   std::vector<std::vector<double>> hw_gpio_states_;
 
-  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
+  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_slave_loader_{
     "ethercat_interface", "ethercat_interface::EcSlave"};
+  pluginlib::ClassLoader<ethercat_interface::EcMaster> ec_master_loader_{
+    "ethercat_interface", "ethercat_interface::EcMaster"};
 
-  int control_frequency_;
-  ethercat_interface::EcMaster master_;
+  std::shared_ptr<ethercat_interface::EcMaster> master_;
 };
 }  // namespace ethercat_driver
 

ethercat_driver/src/ethercat_driver.cpp

@@ -83,8 +83,8 @@ CallbackReturn EthercatDriver::on_init(
           info_.joints[j].command_interfaces[k].name] = std::to_string(k);
       }
       try {
-        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
-        if (!module->setupSlave(
+        auto module = ec_slave_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setup_slave(
             module_params[i], &hw_joint_states_[j], &hw_joint_commands_[j]))
         {
           RCLCPP_FATAL(
@@ -117,8 +117,8 @@ CallbackReturn EthercatDriver::on_init(
           info_.gpios[g].command_interfaces[k].name] = std::to_string(k);
       }
       try {
-        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
-        if (!module->setupSlave(
+        auto module = ec_slave_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setup_slave(
             module_params[i], &hw_gpio_states_[g], &hw_gpio_commands_[g]))
         {
           RCLCPP_FATAL(
@@ -151,8 +151,8 @@ CallbackReturn EthercatDriver::on_init(
           info_.sensors[s].command_interfaces[k].name] = std::to_string(k);
       }
       try {
-        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
-        if (!module->setupSlave(
+        auto module = ec_slave_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setup_slave(
             module_params[i], &hw_sensor_states_[s], &hw_sensor_commands_[s]))
         {
           RCLCPP_FATAL(
@@ -260,86 +260,93 @@ CallbackReturn EthercatDriver::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
   RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Starting ...please wait...");
-  if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) {
-    control_frequency_ = 100;
-  } else {
-    control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]);
+
+  // Get master plugin from hardware description parameter "master_plugin".
+  // Default master plugin is EtherlabMaster
+  std::string master_plugin_name = "ethercat_master/EtherlabMaster";
+  if (info_.hardware_parameters.find("master_plugin") != info_.hardware_parameters.end()) {
+    master_plugin_name = info_.hardware_parameters["master_plugin"];
   }
 
-  if (control_frequency_ < 0) {
+  // Dynamically load master plugin
+  try {
+    master_ = ec_master_loader_.createSharedInstance(master_plugin_name);
+  } catch (pluginlib::PluginlibException & ex) {
     RCLCPP_FATAL(
-      rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!");
+      rclcpp::get_logger("EthercatDriver"),
+      "The master plugin %s failed to load for some reason. Error: %s\n",
+      master_plugin_name.c_str(), ex.what());
     return CallbackReturn::ERROR;
   }
 
-  // start EC and wait until state operative
-
-  master_.setCtrlFrequency(control_frequency_);
+  // Initialize Master
+  std::string master_id = "0";
+  if (info_.hardware_parameters.find("master_id") != info_.hardware_parameters.end()) {
+    master_id = info_.hardware_parameters["master_id"];
+  }
 
-  for (auto i = 0ul; i < ec_modules_.size(); i++) {
-    master_.addSlave(
-      std::stod(ec_module_parameters_[i]["alias"]),
-      std::stod(ec_module_parameters_[i]["position"]),
-      ec_modules_[i].get());
+  if (!master_->init(master_id)) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"),
+      "Failed to initialize Master. Aborting.");
+    return CallbackReturn::ERROR;
   }
 
-  // configure SDO
-  for (auto i = 0ul; i < ec_modules_.size(); i++) {
-    for (auto & sdo : ec_modules_[i]->sdo_config) {
-      uint32_t abort_code;
-      int ret = master_.configSlaveSdo(
-        std::stod(ec_module_parameters_[i]["position"]),
-        sdo,
-        &abort_code
-      );
-      if (ret) {
-        RCLCPP_INFO(
-          rclcpp::get_logger("EthercatDriver"),
-          "Failed to download config SDO for module at position %s with Error: %d",
-          ec_module_parameters_[i]["position"].c_str(),
-          abort_code
-        );
-      }
-    }
+  // Get control frequency for DC sync. Default is 100Hz.
+  int control_frequency = 100;
+  if (info_.hardware_parameters.find("control_frequency") != info_.hardware_parameters.end()) {
+    control_frequency = std::stod(info_.hardware_parameters["control_frequency"]);
   }
 
-  master_.activate();
-  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EcMaster!");
+  if (control_frequency < 0) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!");
+    return CallbackReturn::ERROR;
+  }
 
-  // start after one second
-  struct timespec t;
-  clock_gettime(CLOCK_MONOTONIC, &t);
-  t.tv_sec++;
+  master_->set_ctrl_frequency(control_frequency);
 
-  bool running = true;
-  while (running) {
-    // wait until next shot
-    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
-    // update EtherCAT bus
+  // Add slaves to master
+  for (auto i = 0ul; i < ec_modules_.size(); i++) {
+    if (!master_->add_slave(ec_modules_[i])) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("EthercatDriver"),
+        "Failed to add Slave %li to Master. Aborting.", i);
+      return CallbackReturn::ERROR;
+    }
+  }
 
-    master_.update();
-    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
+  // configure all slaves
+  if (!master_->configure_slaves()) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"),
+      "Failed to configure Slaves. Aborting.");
+    return CallbackReturn::ERROR;
+  }
 
-    // check if operational
-    bool isAllInit = true;
-    for (auto & module : ec_modules_) {
-      isAllInit = isAllInit && module->initialized();
-    }
-    if (isAllInit) {
-      running = false;
-    }
-    // calculate next shot. carry over nanoseconds into microseconds.
-    t.tv_nsec += master_.getInterval();
-    while (t.tv_nsec >= 1000000000) {
-      t.tv_nsec -= 1000000000;
-      t.tv_sec++;
-    }
+  // start EtherCAT communication
+  if (!master_->start()) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"),
+      "Failed to start Master. Aborting.");
+    return CallbackReturn::ERROR;
   }
 
   RCLCPP_INFO(
-    rclcpp::get_logger("EthercatDriver"), "System Successfully started!");
-
-  return CallbackReturn::SUCCESS;
+    rclcpp::get_logger("EthercatDriver"),
+    "Master successfully started!");
+
+  if (master_->spin_slaves_until_operational()) {
+    RCLCPP_INFO(
+      rclcpp::get_logger("EthercatDriver"),
+      "All Slaves are in OPERATIONAL state. System Successfully started!");
+    return CallbackReturn::SUCCESS;
+  } else {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"),
+      "Failed to bring all slaves into OPERATIONAL state");
+    return CallbackReturn::ERROR;
+  }
 }
 
 CallbackReturn EthercatDriver::on_deactivate(
@@ -348,7 +355,7 @@ CallbackReturn EthercatDriver::on_deactivate(
   RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait...");
 
   // stop EC and disconnect
-  master_.stop();
+  master_->stop();
 
   RCLCPP_INFO(
     rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
@@ -360,16 +367,22 @@ hardware_interface::return_type EthercatDriver::read(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
-  master_.readData();
-  return hardware_interface::return_type::OK;
+  if (master_->read_process_data()) {
+    return hardware_interface::return_type::OK;
+  } else {
+    return hardware_interface::return_type::ERROR;
+  }
 }
 
 hardware_interface::return_type EthercatDriver::write(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
-  master_.writeData();
-  return hardware_interface::return_type::OK;
+  if (master_->write_process_data()) {
+    return hardware_interface::return_type::OK;
+  } else {
+    return hardware_interface::return_type::ERROR;
+  }
 }
 
 std::vector<std::unordered_map<std::string, std::string>> EthercatDriver::getEcModuleParam(

ethercat_driver/test/test_ethercat_driver.cpp

@@ -0,0 +1,85 @@
+// Copyright 2023 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.
+
+#include "test_ethercat_driver.hpp"
+#include "ros2_control_test_assets/components_urdfs.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST_F(TestEthercatDriver, load_ethercat_driver)
+{
+  auto urdf = ros2_control_test_assets::urdf_head + urdf_config_ +
+    ros2_control_test_assets::urdf_tail;
+  ASSERT_NO_THROW(TestableResourceManager rm(urdf));
+}
+
+TEST_F(TestEthercatDriver, initialize_ethercat_driver)
+{
+  auto urdf = ros2_control_test_assets::urdf_head + urdf_config_ +
+    ros2_control_test_assets::urdf_tail;
+  TestableResourceManager rm(urdf);
+
+  // check is hardware is configured
+  auto status_map = rm.get_components_status();
+  EXPECT_EQ(
+    status_map["EthercatSystem"].state.label(),
+    hardware_interface::lifecycle_state_names::UNCONFIGURED);
+  configure_components(rm);
+  status_map = rm.get_components_status();
+  EXPECT_EQ(
+    status_map["EthercatSystem"].state.label(),
+    hardware_interface::lifecycle_state_names::INACTIVE);
+  activate_components(rm);
+  status_map = rm.get_components_status();
+  EXPECT_EQ(
+    status_map["EthercatSystem"].state.label(),
+    hardware_interface::lifecycle_state_names::ACTIVE);
+
+  // Check interfaces
+  EXPECT_EQ(1u, rm.system_components_size());
+  ASSERT_EQ(6u, rm.state_interface_keys().size());
+  EXPECT_TRUE(rm.state_interface_exists("joint1/position"));
+  EXPECT_TRUE(rm.state_interface_exists("joint1/velocity"));
+  EXPECT_TRUE(rm.state_interface_exists("gpio1/input1"));
+  EXPECT_TRUE(rm.state_interface_exists("gpio1/input2"));
+  EXPECT_TRUE(rm.state_interface_exists("sensor1/input1"));
+  EXPECT_TRUE(rm.state_interface_exists("sensor1/input2"));
+
+  ASSERT_EQ(3u, rm.command_interface_keys().size());
+  EXPECT_TRUE(rm.command_interface_exists("joint1/position"));
+  EXPECT_TRUE(rm.command_interface_exists("gpio1/output1"));
+  EXPECT_TRUE(rm.command_interface_exists("gpio1/output2"));
+
+  // Check initial values
+  hardware_interface::LoanedStateInterface joint1_si_position =
+    rm.claim_state_interface("joint1/position");
+  hardware_interface::LoanedStateInterface gpio1_si_input2 =
+    rm.claim_state_interface("gpio1/input2");
+  hardware_interface::LoanedStateInterface sensor1_si_input1 =
+    rm.claim_state_interface("sensor1/input1");
+  hardware_interface::LoanedCommandInterface gpio1_ci_output1 =
+    rm.claim_command_interface("gpio1/output1");
+
+  // State interfaces without initial value are set to NaN
+  ASSERT_TRUE(std::isnan(joint1_si_position.get_value()));
+  ASSERT_TRUE(std::isnan(gpio1_si_input2.get_value()));
+  ASSERT_TRUE(std::isnan(sensor1_si_input1.get_value()));
+  ASSERT_TRUE(std::isnan(gpio1_ci_output1.get_value()));
+
+  // set some new values in commands
+  gpio1_ci_output1.set_value(0.123);
+
+  // State values should not be changed
+  ASSERT_TRUE(std::isnan(gpio1_si_input2.get_value()));
+  ASSERT_EQ(0.123, gpio1_ci_output1.get_value());
+}