diff --git a/ethercat_driver/CMakeLists.txt b/ethercat_driver/CMakeLists.txt index a97e0e4a..6a727dbe 100644 --- a/ethercat_driver/CMakeLists.txt +++ b/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 diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp index 4315482e..4d877571 100644 --- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp +++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp @@ -79,11 +79,12 @@ class EthercatDriver : public hardware_interface::SystemInterface std::vector> hw_sensor_states_; std::vector> hw_gpio_states_; - pluginlib::ClassLoader ec_loader_{ + pluginlib::ClassLoader ec_slave_loader_{ "ethercat_interface", "ethercat_interface::EcSlave"}; + pluginlib::ClassLoader ec_master_loader_{ + "ethercat_interface", "ethercat_interface::EcMaster"}; - int control_frequency_; - ethercat_interface::EcMaster master_; + std::shared_ptr master_; }; } // namespace ethercat_driver diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index 45258c90..fd5e617b 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/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> EthercatDriver::getEcModuleParam( diff --git a/ethercat_driver/test/test_ethercat_driver.cpp b/ethercat_driver/test/test_ethercat_driver.cpp new file mode 100644 index 00000000..442e1c74 --- /dev/null +++ b/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()); +} diff --git a/ethercat_driver/test/test_ethercat_driver.hpp b/ethercat_driver/test/test_ethercat_driver.hpp new file mode 100644 index 00000000..46e9f748 --- /dev/null +++ b/ethercat_driver/test/test_ethercat_driver.hpp @@ -0,0 +1,141 @@ +// 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. + +#ifndef TEST_ETHERCAT_DRIVER_HPP_ +#define TEST_ETHERCAT_DRIVER_HPP_ + +#include + +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/state.hpp" + +class TestEthercatDriver : public ::testing::Test +{ +public: + void test_ethercat_driver(std::string & urdf); + +protected: + void SetUp() override + { + // REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED + urdf_config_ = + R"( + + + ethercat_driver/EthercatDriver + ethercat_master/MockMaster + 0 + 100 + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 1 + + + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 2 + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 3 + + + + )"; + } + + std::string urdf_config_; +}; + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend TestEthercatDriver; + + FRIEND_TEST(TestEthercatDriver, initialize_ethercat_driver); + + TestableResourceManager() + : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + +void set_components_state( + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) +{ + for (const auto & component : components) { + rclcpp_lifecycle::State state(state_id, state_name); + rm.set_component_state(component, state); + } +} + +auto configure_components = []( + TestableResourceManager & rm, + const std::vector & components = {"EthercatSystem"}) + { + set_components_state( + rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + }; + +auto activate_components = []( + TestableResourceManager & rm, + const std::vector & components = {"EthercatSystem"}) + { + set_components_state( + rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + }; + +auto deactivate_components = []( + TestableResourceManager & rm, + const std::vector & components = {"EthercatSystem"}) + { + set_components_state( + rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + }; + +#endif // TEST_ETHERCAT_DRIVER_HPP_ diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp index 07ec2a04..716efc9e 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp @@ -40,9 +40,10 @@ class EcCiA402Drive : public GenericEcSlave * The transition through the state machine is handled automatically. */ bool initialized() const; - virtual void processData(size_t index, uint8_t * domain_address); + virtual int process_data( + size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address); - virtual bool setupSlave( + virtual bool setup_slave( std::unordered_map slave_paramters, std::vector * state_interface, std::vector * command_interface); diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp index f06426db..73fbb046 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp @@ -27,10 +27,13 @@ EcCiA402Drive::~EcCiA402Drive() {} bool EcCiA402Drive::initialized() const {return initialized_;} -void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) +int EcCiA402Drive::process_data( + size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address) { // Special case: ControlWord - if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) { + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_RPDO_CONTROLWORD) + { if (is_operational_) { if (fault_reset_command_interface_index_ >= 0) { if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0) { @@ -46,50 +49,62 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) } if (auto_state_transitions_) { - pdo_channels_info_[index].default_value = transition( + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].default_value = + transition( state_, - pdo_channels_info_[index].ec_read(domain_address)); + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].ec_read( + domain_address)); } } } // setup current position as default position - if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) { + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_RPDO_POSITION) + { if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE) { - pdo_channels_info_[index].default_value = - pdo_channels_info_[index].factor * last_position_ + - pdo_channels_info_[index].offset; + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].default_value = + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].factor * + last_position_ + + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].offset; } - pdo_channels_info_[index].override_command = + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].override_command = (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false; } // setup mode of operation - if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION) { + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_RPDO_MODE_OF_OPERATION) + { if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) { - pdo_channels_info_[index].default_value = mode_of_operation_; + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].default_value = + mode_of_operation_; } } - pdo_channels_info_[index].ec_update(domain_address); + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].ec_update(domain_address); // get mode_of_operation_display_ - if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) { - mode_of_operation_display_ = pdo_channels_info_[index].last_value; + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) + { + mode_of_operation_display_ = + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].last_value; } - if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) { - last_position_ = pdo_channels_info_[index].last_value; + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_TPDO_POSITION) + { + last_position_ = + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].last_value; } // Special case: StatusWord - if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) { - status_word_ = pdo_channels_info_[index].last_value; - } - - - // CHECK FOR STATE CHANGE - if (index == all_channels_.size() - 1) { // if last entry in domain + if (pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].index == + CiA402D_TPDO_STATUSWORD) + { + status_word_ = pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].last_value; + // Check for state changes if (status_word_ != last_status_word_) { state_ = deviceState(status_word_); if (state_ != last_state_) { @@ -104,9 +119,10 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) last_state_ = state_; counter_++; } + return 0; } -bool EcCiA402Drive::setupSlave( +bool EcCiA402Drive::setup_slave( std::unordered_map slave_paramters, std::vector * state_interface, std::vector * command_interface) @@ -125,7 +141,6 @@ bool EcCiA402Drive::setupSlave( } setup_interface_mapping(); - setup_syncs(); if (paramters_.find("mode_of_operation") != paramters_.end()) { mode_of_operation_ = std::stod(paramters_["mode_of_operation"]); diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp index 8d2d959d..d3b87558 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp @@ -70,7 +70,7 @@ TEST_F(EcCiA402DriveTest, SlaveSetupNoDriveConfig) std::unordered_map slave_paramters; // setup failed, 'drive_config' parameter not set ASSERT_EQ( - plugin_->setupSlave( + plugin_->setup_slave( slave_paramters, &state_interface, &command_interface @@ -88,7 +88,7 @@ TEST_F(EcCiA402DriveTest, SlaveSetupMissingFileDriveConfig) slave_paramters["drive_config"] = "drive_config.yaml"; // setup failed, 'drive_config.yaml' file not set ASSERT_EQ( - plugin_->setupSlave( + plugin_->setup_slave( slave_paramters, &state_interface, &command_interface @@ -104,109 +104,57 @@ TEST_F(EcCiA402DriveTest, SlaveSetupDriveFromConfig) plugin_->setup_from_config(YAML::Load(test_drive_config)), true ); - ASSERT_EQ(plugin_->vendor_id_, 0x00000011); - ASSERT_EQ(plugin_->product_id_, 0x07030924); - ASSERT_EQ(plugin_->assign_activate_, 0x0321); + ASSERT_EQ(plugin_->vendor_id_, 0x00000011u); + ASSERT_EQ(plugin_->product_id_, 0x07030924u); + ASSERT_EQ(plugin_->assign_activate_, 0x0321u); ASSERT_EQ(plugin_->auto_fault_reset_, false); - ASSERT_EQ(plugin_->rpdos_.size(), 1); - ASSERT_EQ(plugin_->rpdos_[0].index, 0x1607); - - ASSERT_EQ(plugin_->tpdos_.size(), 2); - ASSERT_EQ(plugin_->tpdos_[0].index, 0x1a07); - ASSERT_EQ(plugin_->tpdos_[1].index, 0x1a45); - - ASSERT_EQ(plugin_->pdo_channels_info_[1].interface_name, "velocity"); - ASSERT_EQ(plugin_->pdo_channels_info_[3].default_value, 1000); - ASSERT_TRUE(std::isnan(plugin_->pdo_channels_info_[0].default_value)); - ASSERT_EQ(plugin_->pdo_channels_info_[4].interface_name, "null"); - ASSERT_EQ(plugin_->pdo_channels_info_[12].interface_name, "analog_input2"); - ASSERT_EQ(plugin_->pdo_channels_info_[4].data_type, "uint16"); -} - -TEST_F(EcCiA402DriveTest, SlaveSetupPdoChannels) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_drive_config)); - std::vector channels( - plugin_->channels(), - plugin_->channels() + plugin_->all_channels_.size() - ); - - ASSERT_EQ(channels.size(), 13); - ASSERT_EQ(channels[0].index, 0x607a); - ASSERT_EQ(channels[11].index, 0x2205); - ASSERT_EQ(channels[11].subindex, 0x01); -} - -TEST_F(EcCiA402DriveTest, SlaveSetupSyncs) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_drive_config)); - plugin_->setup_syncs(); - std::vector syncs( - plugin_->syncs(), - plugin_->syncs() + plugin_->syncSize() - ); - - ASSERT_EQ(syncs.size(), 5); - ASSERT_EQ(syncs[1].index, 1); - ASSERT_EQ(syncs[1].dir, EC_DIR_INPUT); - ASSERT_EQ(syncs[1].n_pdos, 0); - ASSERT_EQ(syncs[1].watchdog_mode, EC_WD_DISABLE); - ASSERT_EQ(syncs[2].dir, EC_DIR_OUTPUT); - ASSERT_EQ(syncs[2].n_pdos, 1); - ASSERT_EQ(syncs[3].index, 3); - ASSERT_EQ(syncs[3].dir, EC_DIR_INPUT); - ASSERT_EQ(syncs[3].n_pdos, 2); - ASSERT_EQ(syncs[3].watchdog_mode, EC_WD_DISABLE); -} - -TEST_F(EcCiA402DriveTest, SlaveSetupDomains) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_drive_config)); - std::map> domains; - plugin_->domains(domains); - - ASSERT_EQ(domains[0].size(), 13); - ASSERT_EQ(domains[0][0], 0); - ASSERT_EQ(domains[0][12], 12); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[1].interface_name, "velocity"); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[3].default_value, 1000); + ASSERT_TRUE(std::isnan(plugin_->get_pdo_config()[0].pdo_channel_config[0].default_value)); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[4].interface_name, "null"); + ASSERT_EQ(plugin_->get_pdo_config()[2].pdo_channel_config[1].interface_name, "analog_input2"); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[4].data_type, "uint16"); } TEST_F(EcCiA402DriveTest, EcReadTPDOToStateInterface) { SetUp(); - std::unordered_map slave_paramters; std::vector state_interface = {0, 0}; - plugin_->state_interface_ptr_ = &state_interface; + std::unordered_map slave_paramters; slave_paramters["state_interface/effort"] = "1"; + + plugin_->state_interface_ptr_ = &state_interface; plugin_->paramters_ = slave_paramters; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1); + + ASSERT_EQ(plugin_->get_pdo_config().size(), 3u); + ASSERT_EQ(plugin_->get_pdo_config()[1].pdo_channel_config[2].interface_index, 1); uint8_t domain_address[2]; - EC_WRITE_S16(domain_address, 42); - plugin_->processData(8, domain_address); + write_s16(domain_address, 42); + plugin_->process_data(1, 2, domain_address); ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 42); } TEST_F(EcCiA402DriveTest, EcWriteRPDOFromCommandInterface) { SetUp(); - std::unordered_map slave_paramters; std::vector command_interface = {0, 42}; - plugin_->command_interface_ptr_ = &command_interface; + std::unordered_map slave_paramters; slave_paramters["command_interface/effort"] = "1"; + + plugin_->command_interface_ptr_ = &command_interface; plugin_->paramters_ = slave_paramters; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1); + + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].interface_index, 1); plugin_->mode_of_operation_display_ = 10; uint8_t domain_address[2]; - plugin_->processData(2, domain_address); - ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, 42); - ASSERT_EQ(EC_READ_S16(domain_address), 42); + plugin_->process_data(0, 2, domain_address); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].last_value, 42); + ASSERT_EQ(read_s16(domain_address), 42); } TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue) @@ -214,11 +162,12 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue) SetUp(); plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); + plugin_->mode_of_operation_display_ = 10; uint8_t domain_address[2]; - plugin_->processData(2, domain_address); - ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, -5); - ASSERT_EQ(EC_READ_S16(domain_address), -5); + plugin_->process_data(0, 2, domain_address); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].last_value, -5); + ASSERT_EQ(read_s16(domain_address), -5); } // TEST_F(EcCiA402DriveTest, FaultReset) @@ -237,15 +186,15 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue) // ASSERT_FALSE(plugin_->fault_reset_); // ASSERT_EQ(plugin_->command_interface_ptr_->at( // plugin_->fault_reset_command_interface_index_), 1); -// plugin_->processData(4, &domain_address); +// plugin_->process_data(4, &domain_address); // ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000); // plugin_->pdo_channels_info_[4].last_value = 0; -// plugin_->processData(4, &domain_address); +// plugin_->process_data(4, &domain_address); // ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000); // command_interface[1] = 0; -// plugin_->processData(4, &domain_address); +// plugin_->process_data(4, &domain_address); // ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000); -// command_interface[1] = 2; plugin_->processData(4, &domain_address); +// command_interface[1] = 2; plugin_->process_data(4, &domain_address); // ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000); // } @@ -256,18 +205,20 @@ TEST_F(EcCiA402DriveTest, SwitchModeOfOperation) std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; slave_paramters["command_interface/mode_of_operation"] = "1"; + plugin_->paramters_ = slave_paramters; plugin_->command_interface_ptr_ = &command_interface; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); + plugin_->is_operational_ = true; uint8_t domain_address[2]; - plugin_->processData(5, domain_address); - ASSERT_EQ(EC_READ_S8(domain_address), 8); + plugin_->process_data(0, 5, domain_address); + ASSERT_EQ(read_s8(domain_address), 8); command_interface[1] = 9; - plugin_->processData(5, domain_address); - plugin_->processData(10, domain_address); - ASSERT_EQ(EC_READ_S8(domain_address), 9); + plugin_->process_data(0, 5, domain_address); + plugin_->process_data(1, 4, domain_address); + ASSERT_EQ(read_s8(domain_address), 9); ASSERT_EQ(plugin_->mode_of_operation_display_, 9); } @@ -278,41 +229,42 @@ TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; slave_paramters["command_interface/mode_of_operation"] = "1"; + plugin_->paramters_ = slave_paramters; plugin_->command_interface_ptr_ = &command_interface; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); + plugin_->is_operational_ = true; plugin_->mode_of_operation_display_ = 8; uint8_t domain_address[4]; uint8_t domain_address_moo[2]; - - plugin_->processData(5, domain_address_moo); - plugin_->processData(10, domain_address_moo); + plugin_->process_data(0, 5, domain_address_moo); + plugin_->process_data(1, 4, domain_address_moo); ASSERT_EQ(plugin_->mode_of_operation_display_, 8); - EC_WRITE_S32(domain_address, 123456); - plugin_->processData(6, domain_address); + write_s32(domain_address, 123456); + plugin_->process_data(1, 0, domain_address); ASSERT_EQ(plugin_->last_position_, 123456); - EC_WRITE_S32(domain_address, 0); - plugin_->processData(0, domain_address); - ASSERT_EQ(EC_READ_S32(domain_address), 123456); + write_s32(domain_address, 0); + plugin_->process_data(0, 0, domain_address); + ASSERT_EQ(read_s32(domain_address), 123456); command_interface[1] = 9; - plugin_->processData(5, domain_address_moo); - plugin_->processData(10, domain_address_moo); + plugin_->process_data(0, 5, domain_address_moo); + plugin_->process_data(1, 4, domain_address_moo); ASSERT_EQ(plugin_->mode_of_operation_display_, 9); - EC_WRITE_S32(domain_address, 0); - plugin_->processData(0, domain_address); - ASSERT_EQ(EC_READ_S32(domain_address), 123456); + write_s32(domain_address, 0); + plugin_->process_data(0, 0, domain_address); + ASSERT_EQ(read_s32(domain_address), 123456); - EC_WRITE_S32(domain_address, 654321); - plugin_->processData(6, domain_address); + write_s32(domain_address, 654321); + plugin_->process_data(1, 0, domain_address); ASSERT_EQ(plugin_->last_position_, 654321); - EC_WRITE_S32(domain_address, 0); - plugin_->processData(0, domain_address); - ASSERT_EQ(EC_READ_S32(domain_address), 654321); + write_s32(domain_address, 0); + plugin_->process_data(0, 0, domain_address); + ASSERT_EQ(read_s32(domain_address), 654321); } diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.hpp index ad123ff9..64bf9180 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.hpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.hpp @@ -22,6 +22,7 @@ #include "gmock/gmock.h" +#include "ethercat_interface/ec_buffer_tools.h" #include "ethercat_generic_plugins/generic_ec_cia402_drive.hpp" // subclassing and friending so we can access member variables diff --git a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp index 9d733ee9..fb7026f5 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp @@ -23,8 +23,6 @@ #include "yaml-cpp/yaml.h" #include "ethercat_interface/ec_slave.hpp" -#include "ethercat_interface/ec_pdo_channel_manager.hpp" -#include "ethercat_interface/ec_sync_manager.hpp" namespace ethercat_generic_plugins { @@ -33,41 +31,23 @@ class GenericEcSlave : public ethercat_interface::EcSlave { public: GenericEcSlave(); - virtual ~GenericEcSlave(); - virtual int assign_activate_dc_sync(); + ~GenericEcSlave(); - virtual void processData(size_t index, uint8_t * domain_address); - - virtual const ec_sync_info_t * syncs(); - virtual size_t syncSize(); - virtual const ec_pdo_entry_info_t * channels(); - virtual void domains(DomainMap & domains) const; - - virtual bool setupSlave( + bool setup_slave( std::unordered_map slave_paramters, std::vector * state_interface, std::vector * command_interface); + YAML::Node get_slave_config(); + protected: - uint32_t counter_ = 0; - std::vector rpdos_; - std::vector tpdos_; - std::vector all_channels_; - std::vector pdo_channels_info_; - std::vector sm_configs_; - std::vector syncs_; - std::vector domain_map_; YAML::Node slave_config_; - uint32_t assign_activate_ = 0; + // ethercat_interface::pdo_channels_t pdo_channels_info_; /** set up of the drive configuration from yaml node*/ bool setup_from_config(YAML::Node slave_config); /** set up of the drive configuration from yaml file*/ bool setup_from_config_file(std::string config_file); - - void setup_syncs(); - - void setup_interface_mapping(); }; } // namespace ethercat_generic_plugins diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 7514cf01..a629ebea 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -29,6 +29,8 @@ size_t type2bytes(std::string type) return 4; } else if (type == "int64" || type == "uint64") { return 8; + } else { + return 0; } } @@ -36,54 +38,10 @@ namespace ethercat_generic_plugins { GenericEcSlave::GenericEcSlave() -: EcSlave(0, 0) {} +: EcSlave() {} GenericEcSlave::~GenericEcSlave() {} -int GenericEcSlave::assign_activate_dc_sync() {return assign_activate_;} - -void GenericEcSlave::processData(size_t index, uint8_t * domain_address) -{ - pdo_channels_info_[domain_map_[index]].ec_update(domain_address); -} - -const ec_sync_info_t * GenericEcSlave::syncs() -{ - return syncs_.data(); -} -size_t GenericEcSlave::syncSize() -{ - return syncs_.size(); -} -const ec_pdo_entry_info_t * GenericEcSlave::channels() -{ - return all_channels_.data(); -} -void GenericEcSlave::domains(DomainMap & domains) const -{ - domains = {{0, domain_map_}}; -} -void GenericEcSlave::setup_syncs() -{ - if (sm_configs_.size() == 0) { - syncs_.push_back({0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}); - syncs_.push_back({1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}); - syncs_.push_back({2, EC_DIR_OUTPUT, rpdos_.size(), rpdos_.data(), EC_WD_ENABLE}); - syncs_.push_back({3, EC_DIR_INPUT, tpdos_.size(), tpdos_.data(), EC_WD_DISABLE}); - } else { - for (auto & sm : sm_configs_) { - if (sm.pdo_name == "null") { - syncs_.push_back({sm.index, sm.type, 0, NULL, sm.watchdog}); - } else if (sm.pdo_name == "rpdo") { - syncs_.push_back({sm.index, sm.type, rpdos_.size(), rpdos_.data(), sm.watchdog}); - } else if (sm.pdo_name == "tpdo") { - syncs_.push_back({sm.index, sm.type, tpdos_.size(), tpdos_.data(), sm.watchdog}); - } - } - } - syncs_.push_back({0xff}); -} - -bool GenericEcSlave::setupSlave( +bool GenericEcSlave::setup_slave( std::unordered_map slave_paramters, std::vector * state_interface, std::vector * command_interface) @@ -102,7 +60,6 @@ bool GenericEcSlave::setupSlave( } setup_interface_mapping(); - setup_syncs(); return true; } @@ -110,112 +67,87 @@ bool GenericEcSlave::setupSlave( bool GenericEcSlave::setup_from_config(YAML::Node slave_config) { if (slave_config.size() != 0) { + // configure slave vendor id if (slave_config["vendor_id"]) { vendor_id_ = slave_config["vendor_id"].as(); } else { std::cerr << "GenericEcSlave: failed to load drive vendor ID." << std::endl; return false; } + // configure slave product id if (slave_config["product_id"]) { product_id_ = slave_config["product_id"].as(); } else { std::cerr << "GenericEcSlave: failed to load drive product ID." << std::endl; return false; } + // configure sc sync if (slave_config["assign_activate"]) { assign_activate_ = slave_config["assign_activate"].as(); } - + // configure sync manager if (slave_config["sm"]) { for (const auto & sm : slave_config["sm"]) { ethercat_interface::SMConfig config; if (config.load_from_config(sm)) { - sm_configs_.push_back(config); + sm_config_.push_back(config); } } } - + // configure sdo if (slave_config["sdo"]) { for (const auto & sdo : slave_config["sdo"]) { ethercat_interface::SdoConfigEntry config; if (config.load_from_config(sdo)) { - sdo_config.push_back(config); + sdo_config_.push_back(config); } } } - - auto channels_nbr = 0; - - if (slave_config["rpdo"]) { - for (auto i = 0ul; i < slave_config["rpdo"].size(); i++) { - channels_nbr += slave_config["rpdo"][i]["channels"].size(); - } - } - if (slave_config["tpdo"]) { - for (auto i = 0ul; i < slave_config["tpdo"].size(); i++) { - channels_nbr += slave_config["tpdo"][i]["channels"].size(); - } - } - - all_channels_.reserve(channels_nbr); - channels_nbr = 0; - + // configure rpdo if (slave_config["rpdo"]) { for (auto i = 0ul; i < slave_config["rpdo"].size(); i++) { auto rpdo_channels_size = slave_config["rpdo"][i]["channels"].size(); + ethercat_interface::pdo_channels_t rpdo_channels_info; for (auto c = 0ul; c < rpdo_channels_size; c++) { ethercat_interface::EcPdoChannelManager channel_info; channel_info.pdo_type = ethercat_interface::RPDO; channel_info.load_from_config(slave_config["rpdo"][i]["channels"][c]); - pdo_channels_info_.push_back(channel_info); - all_channels_.push_back(channel_info.get_pdo_entry_info()); + rpdo_channels_info.push_back(channel_info); + // pdo_channels_info_.push_back(channel_info); } - rpdos_.push_back( - { - slave_config["rpdo"][i]["index"].as(), - rpdo_channels_size, - all_channels_.data() + channels_nbr - } - ); - channels_nbr += rpdo_channels_size; + ethercat_interface::pdo_mapping_t rpdo_mapping; + rpdo_mapping.pdo_type = ethercat_interface::RPDO; + rpdo_mapping.index = slave_config["rpdo"][i]["index"].as(); + rpdo_mapping.pdo_channel_config = rpdo_channels_info; + pdo_config_.push_back(rpdo_mapping); } } - + // configure tpdo if (slave_config["tpdo"]) { for (auto i = 0ul; i < slave_config["tpdo"].size(); i++) { auto tpdo_channels_size = slave_config["tpdo"][i]["channels"].size(); - + ethercat_interface::pdo_channels_t tpdo_channels_info; for (auto c = 0ul; c < tpdo_channels_size; c++) { ethercat_interface::EcPdoChannelManager channel_info; channel_info.pdo_type = ethercat_interface::TPDO; channel_info.load_from_config(slave_config["tpdo"][i]["channels"][c]); - pdo_channels_info_.push_back(channel_info); - all_channels_.push_back(channel_info.get_pdo_entry_info()); + tpdo_channels_info.push_back(channel_info); + // pdo_channels_info_.push_back(channel_info); } - tpdos_.push_back( - { - slave_config["tpdo"][i]["index"].as(), - tpdo_channels_size, - all_channels_.data() + channels_nbr - } - ); - channels_nbr += tpdo_channels_size; - } - } - - // Remove gaps from domain mapping - for (auto i = 0ul; i < all_channels_.size(); i++) { - if (all_channels_[i].index != 0x0000) { - domain_map_.push_back(i); + ethercat_interface::pdo_mapping_t tpdo_mapping; + tpdo_mapping.pdo_type = ethercat_interface::TPDO; + tpdo_mapping.index = slave_config["tpdo"][i]["index"].as(); + tpdo_mapping.pdo_channel_config = tpdo_channels_info; + pdo_config_.push_back(tpdo_mapping); } } - - return true; } else { std::cerr << "GenericEcSlave: failed to load slave configuration: empty configuration" << std::endl; return false; } + + return true; } bool GenericEcSlave::setup_from_config_file(std::string config_file) @@ -236,26 +168,30 @@ bool GenericEcSlave::setup_from_config_file(std::string config_file) return true; } -void GenericEcSlave::setup_interface_mapping() +// void GenericEcSlave::setup_interface_mapping() +// { +// for (auto & channel : pdo_channels_info_) { +// if (channel.pdo_type == ethercat_interface::TPDO) { +// if (paramters_.find("state_interface/" + channel.interface_name) != paramters_.end()) { +// channel.interface_index = +// std::stoi(paramters_["state_interface/" + channel.interface_name]); +// } +// } +// if (channel.pdo_type == ethercat_interface::RPDO) { +// if (paramters_.find("command_interface/" + channel.interface_name) != paramters_.end()) { +// channel.interface_index = std::stoi( +// paramters_["command_interface/" + channel.interface_name]); +// } +// } + +// channel.setup_interface_ptrs(state_interface_ptr_, command_interface_ptr_); +// } +// } + +YAML::Node GenericEcSlave::get_slave_config() { - for (auto & channel : pdo_channels_info_) { - if (channel.pdo_type == ethercat_interface::TPDO) { - if (paramters_.find("state_interface/" + channel.interface_name) != paramters_.end()) { - channel.interface_index = - std::stoi(paramters_["state_interface/" + channel.interface_name]); - } - } - if (channel.pdo_type == ethercat_interface::RPDO) { - if (paramters_.find("command_interface/" + channel.interface_name) != paramters_.end()) { - channel.interface_index = std::stoi( - paramters_["command_interface/" + channel.interface_name]); - } - } - - channel.setup_interface_ptrs(state_interface_ptr_, command_interface_ptr_); - } + return slave_config_; } - } // namespace ethercat_generic_plugins #include diff --git a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp index d16c3527..c3958fc6 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp @@ -74,7 +74,7 @@ TEST_F(GenericEcSlaveTest, SlaveSetupNoSlaveConfig) std::unordered_map slave_paramters; // setup failed, 'slave_config' parameter not set ASSERT_EQ( - plugin_->setupSlave( + plugin_->setup_slave( slave_paramters, &state_interface, &command_interface @@ -92,7 +92,7 @@ TEST_F(GenericEcSlaveTest, SlaveSetupMissingFileSlaveConfig) slave_paramters["slave_config"] = "slave_config.yaml"; // setup failed, 'slave_config.yaml' file not set ASSERT_EQ( - plugin_->setupSlave( + plugin_->setup_slave( slave_paramters, &state_interface, &command_interface @@ -108,109 +108,57 @@ TEST_F(GenericEcSlaveTest, SlaveSetupSlaveFromConfig) plugin_->setup_from_config(YAML::Load(test_slave_config)), true ); - ASSERT_EQ(plugin_->vendor_id_, 0x00000011); - ASSERT_EQ(plugin_->product_id_, 0x07030924); - ASSERT_EQ(plugin_->assign_activate_, 0x0321); - - ASSERT_EQ(plugin_->rpdos_.size(), 1); - ASSERT_EQ(plugin_->rpdos_[0].index, 0x1607); - - ASSERT_EQ(plugin_->tpdos_.size(), 2); - ASSERT_EQ(plugin_->tpdos_[0].index, 0x1a07); - ASSERT_EQ(plugin_->tpdos_[1].index, 0x1a45); - - ASSERT_EQ(plugin_->pdo_channels_info_[1].interface_name, "velocity"); - ASSERT_EQ(plugin_->pdo_channels_info_[2].factor, 2); - ASSERT_EQ(plugin_->pdo_channels_info_[2].offset, 10); - ASSERT_EQ(plugin_->pdo_channels_info_[3].default_value, 1000); - ASSERT_TRUE(std::isnan(plugin_->pdo_channels_info_[0].default_value)); - ASSERT_EQ(plugin_->pdo_channels_info_[4].interface_name, "null"); - ASSERT_EQ(plugin_->pdo_channels_info_[12].interface_name, "analog_input2"); - ASSERT_EQ(plugin_->pdo_channels_info_[4].data_type, "uint16"); -} - -TEST_F(GenericEcSlaveTest, SlaveSetupPdoChannels) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_slave_config)); - std::vector channels( - plugin_->channels(), - plugin_->channels() + plugin_->all_channels_.size() - ); - - ASSERT_EQ(channels.size(), 13); - ASSERT_EQ(channels[0].index, 0x607a); - ASSERT_EQ(channels[11].index, 0x2205); - ASSERT_EQ(channels[11].subindex, 0x01); -} - -TEST_F(GenericEcSlaveTest, SlaveSetupSyncs) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_slave_config)); - plugin_->setup_syncs(); - std::vector syncs( - plugin_->syncs(), - plugin_->syncs() + plugin_->syncSize() - ); - - ASSERT_EQ(syncs.size(), 5); - ASSERT_EQ(syncs[1].index, 1); - ASSERT_EQ(syncs[1].dir, EC_DIR_INPUT); - ASSERT_EQ(syncs[1].n_pdos, 0); - ASSERT_EQ(syncs[1].watchdog_mode, EC_WD_DISABLE); - ASSERT_EQ(syncs[2].dir, EC_DIR_OUTPUT); - ASSERT_EQ(syncs[2].n_pdos, 1); - ASSERT_EQ(syncs[3].index, 3); - ASSERT_EQ(syncs[3].dir, EC_DIR_INPUT); - ASSERT_EQ(syncs[3].n_pdos, 2); - ASSERT_EQ(syncs[3].watchdog_mode, EC_WD_DISABLE); -} - -TEST_F(GenericEcSlaveTest, SlaveSetupDomains) -{ - SetUp(); - plugin_->setup_from_config(YAML::Load(test_slave_config)); - std::map> domains; - plugin_->domains(domains); - - ASSERT_EQ(domains[0].size(), 13); - ASSERT_EQ(domains[0][0], 0); - ASSERT_EQ(domains[0][12], 12); + ASSERT_EQ(plugin_->vendor_id_, 0x00000011u); + ASSERT_EQ(plugin_->product_id_, 0x07030924u); + ASSERT_EQ(plugin_->assign_activate_, 0x0321u); + + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[1].interface_name, "velocity"); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].factor, 2); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].offset, 10); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[3].default_value, 1000); + ASSERT_TRUE(std::isnan(plugin_->get_pdo_config()[0].pdo_channel_config[0].default_value)); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[4].interface_name, "null"); + ASSERT_EQ(plugin_->get_pdo_config()[2].pdo_channel_config[1].interface_name, "analog_input2"); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[4].data_type, "uint16"); } TEST_F(GenericEcSlaveTest, EcReadTPDOToStateInterface) { SetUp(); - std::unordered_map slave_paramters; std::vector state_interface = {0, 0}; - plugin_->state_interface_ptr_ = &state_interface; + std::unordered_map slave_paramters; slave_paramters["state_interface/effort"] = "1"; + + plugin_->state_interface_ptr_ = &state_interface; plugin_->paramters_ = slave_paramters; plugin_->setup_from_config(YAML::Load(test_slave_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1); + + ASSERT_EQ(plugin_->get_pdo_config().size(), 3u); + ASSERT_EQ(plugin_->get_pdo_config()[1].pdo_channel_config[2].interface_index, 1); uint8_t domain_address[2]; - EC_WRITE_S16(domain_address, 42); - plugin_->processData(8, domain_address); + write_s16(domain_address, 42); + plugin_->process_data(1, 2, domain_address); ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 5 * 42 + 15); } TEST_F(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface) { SetUp(); - std::unordered_map slave_paramters; std::vector command_interface = {0, 42}; - plugin_->command_interface_ptr_ = &command_interface; + std::unordered_map slave_paramters; slave_paramters["command_interface/effort"] = "1"; + + plugin_->command_interface_ptr_ = &command_interface; plugin_->paramters_ = slave_paramters; plugin_->setup_from_config(YAML::Load(test_slave_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1); + + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].interface_index, 1); uint8_t domain_address[2]; - plugin_->processData(2, domain_address); - ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, 2 * 42 + 10); - ASSERT_EQ(EC_READ_S16(domain_address), 2 * 42 + 10); + plugin_->process_data(0, 2, domain_address); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].last_value, 2 * 42 + 10); + ASSERT_EQ(read_s16(domain_address), 2 * 42 + 10); } TEST_F(GenericEcSlaveTest, EcWriteRPDODefaultValue) @@ -218,35 +166,36 @@ TEST_F(GenericEcSlaveTest, EcWriteRPDODefaultValue) SetUp(); plugin_->setup_from_config(YAML::Load(test_slave_config)); plugin_->setup_interface_mapping(); + uint8_t domain_address[2]; - plugin_->processData(2, domain_address); - ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, -5); - ASSERT_EQ(EC_READ_S16(domain_address), -5); + plugin_->process_data(0, 2, domain_address); + ASSERT_EQ(plugin_->get_pdo_config()[0].pdo_channel_config[2].last_value, -5); + ASSERT_EQ(read_s16(domain_address), -5); } TEST_F(GenericEcSlaveTest, SlaveSetupSDOConfig) { SetUp(); plugin_->setup_from_config(YAML::Load(test_slave_config)); - ASSERT_EQ(plugin_->sdo_config[0].index, 0x60C2); - ASSERT_EQ(plugin_->sdo_config[0].sub_index, 1); - ASSERT_EQ(plugin_->sdo_config[1].sub_index, 2); - ASSERT_EQ(plugin_->sdo_config[0].data_size(), 1); - ASSERT_EQ(plugin_->sdo_config[0].data, 10); - ASSERT_EQ(plugin_->sdo_config[2].index, 0x6098); - ASSERT_EQ(plugin_->sdo_config[3].data_type, "int32"); - ASSERT_EQ(plugin_->sdo_config[3].data_size(), 4); + ASSERT_EQ(plugin_->get_sdo_config()[0].index, 0x60C2); + ASSERT_EQ(plugin_->get_sdo_config()[0].sub_index, 1); + ASSERT_EQ(plugin_->get_sdo_config()[1].sub_index, 2); + ASSERT_EQ(plugin_->get_sdo_config()[0].data_size(), 1ul); + ASSERT_EQ(plugin_->get_sdo_config()[0].data, 10); + ASSERT_EQ(plugin_->get_sdo_config()[2].index, 0x6098); + ASSERT_EQ(plugin_->get_sdo_config()[3].data_type, "int32"); + ASSERT_EQ(plugin_->get_sdo_config()[3].data_size(), 4ul); } TEST_F(GenericEcSlaveTest, SlaveSetupSyncManagerConfig) { SetUp(); plugin_->setup_from_config(YAML::Load(test_slave_config)); - ASSERT_EQ(plugin_->sm_configs_.size(), 4); - ASSERT_EQ(plugin_->sm_configs_[0].index, 0); - ASSERT_EQ(plugin_->sm_configs_[0].type, EC_DIR_OUTPUT); - ASSERT_EQ(plugin_->sm_configs_[0].watchdog, EC_WD_DISABLE); - ASSERT_EQ(plugin_->sm_configs_[0].pdo_name, "null"); - ASSERT_EQ(plugin_->sm_configs_[2].pdo_name, "rpdo"); - ASSERT_EQ(plugin_->sm_configs_[2].watchdog, EC_WD_ENABLE); + ASSERT_EQ(plugin_->get_sm_config().size(), 4ul); + ASSERT_EQ(plugin_->get_sm_config()[0].index, 0); + ASSERT_EQ(plugin_->get_sm_config()[0].type, 0); + ASSERT_EQ(plugin_->get_sm_config()[0].watchdog, -1); + ASSERT_EQ(plugin_->get_sm_config()[0].pdo_name, "null"); + ASSERT_EQ(plugin_->get_sm_config()[2].pdo_name, "rpdo"); + ASSERT_EQ(plugin_->get_sm_config()[2].watchdog, 1); } diff --git a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.hpp b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.hpp index a78e4821..271ca9b7 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.hpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.hpp @@ -21,6 +21,7 @@ #include #include "gmock/gmock.h" +#include "ethercat_interface/ec_buffer_tools.h" #include "ethercat_generic_plugins/generic_ec_slave.hpp" @@ -28,9 +29,9 @@ class FriendGenericEcSlave : public ethercat_generic_plugins::GenericEcSlave { FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSlaveFromConfig); - FRIEND_TEST(GenericEcSlaveTest, SlaveSetupPdoChannels); - FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSyncs); - FRIEND_TEST(GenericEcSlaveTest, SlaveSetupDomains); + // FRIEND_TEST(GenericEcSlaveTest, SlaveSetupPdoChannels); + // FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSyncs); + // FRIEND_TEST(GenericEcSlaveTest, SlaveSetupDomains); FRIEND_TEST(GenericEcSlaveTest, EcReadTPDOToStateInterface); FRIEND_TEST(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface); FRIEND_TEST(GenericEcSlaveTest, EcWriteRPDODefaultValue); diff --git a/ethercat_interface/CMakeLists.txt b/ethercat_interface/CMakeLists.txt index 3843fe6e..c7abb671 100644 --- a/ethercat_interface/CMakeLists.txt +++ b/ethercat_interface/CMakeLists.txt @@ -10,41 +10,11 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) -# EtherLab -set(ETHERLAB_DIR /usr/local/etherlab) -set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - -find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib) - ament_export_include_directories( include - ${ETHERLAB_DIR}/include -) - -add_library( - ${PROJECT_NAME} - SHARED - src/ec_master.cpp) - -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include - ${ETHERLAB_DIR}/include -) - -target_link_libraries(${PROJECT_NAME} ${ETHERCAT_LIB}) - -ament_target_dependencies( - ${PROJECT_NAME} - rclcpp ) # INSTALL -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib -) install( DIRECTORY include/ DESTINATION include @@ -61,21 +31,10 @@ if(BUILD_TESTING) test_ec_pdo_channel_manager test/test_ec_pdo_channel_manager.cpp ) - target_include_directories(test_ec_pdo_channel_manager PRIVATE include ${ETHERLAB_DIR}/include) + target_include_directories(test_ec_pdo_channel_manager PRIVATE include) ament_target_dependencies(test_ec_pdo_channel_manager yaml_cpp_vendor ) endif() -## EXPORTS -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} - ${ETHERCAT_LIBRARY} -) -ament_export_dependencies( - rclcpp -) ament_package() diff --git a/ethercat_interface/include/ethercat_interface/ec_buffer_tools.h b/ethercat_interface/include/ethercat_interface/ec_buffer_tools.h new file mode 100644 index 00000000..7c2ba23b --- /dev/null +++ b/ethercat_interface/include/ethercat_interface/ec_buffer_tools.h @@ -0,0 +1,241 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#ifndef ETHERCAT_INTERFACE__EC_BUFFER_TOOLS_H_ +#define ETHERCAT_INTERFACE__EC_BUFFER_TOOLS_H_ + +#include +/****************************************************************************** + * Bitwise read/write macros + *****************************************************************************/ + +/** Read a certain bit of an Buffer data byte. + * + * \param DATA Buffer data pointer + * \param POS bit position + */ +#define read_bit(DATA, POS) ((*((uint8_t *) (DATA)) >> (POS)) & 0x01) + +/** Write a certain bit of an Buffer data byte. + * + * \param DATA Buffer data pointer + * \param POS bit position + * \param VAL new bit value + */ +#define write_bit(DATA, POS, VAL) \ + do { \ + if (VAL) { \ + *((uint8_t *) (DATA)) |= (1 << (POS)); \ + } else { \ + *((uint8_t *) (DATA)) &= ~(1 << (POS)); \ + } \ + } while (0) + +/****************************************************************************** + * Byte-swapping functions for user space + *****************************************************************************/ + +#ifndef __KERNEL__ + +#if __BYTE_ORDER == __LITTLE_ENDIAN + +#define le16_to_cpu(x) x +#define le32_to_cpu(x) x +#define le64_to_cpu(x) x + +#define cpu_to_le16(x) x +#define cpu_to_le32(x) x +#define cpu_to_le64(x) x + +#elif __BYTE_ORDER == __BIG_ENDIAN + +#define swap16(x) \ + ((uint16_t)( \ + (((uint16_t)(x) & 0x00ffU) << 8) | \ + (((uint16_t)(x) & 0xff00U) >> 8) )) +#define swap32(x) \ + ((uint32_t)( \ + (((uint32_t)(x) & 0x000000ffUL) << 24) | \ + (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ + (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ + (((uint32_t)(x) & 0xff000000UL) >> 24) )) +#define swap64(x) \ + ((uint64_t)( \ + (((uint64_t)(x) & 0x00000000000000ffULL) << 56) | \ + (((uint64_t)(x) & 0x000000000000ff00ULL) << 40) | \ + (((uint64_t)(x) & 0x0000000000ff0000ULL) << 24) | \ + (((uint64_t)(x) & 0x00000000ff000000ULL) << 8) | \ + (((uint64_t)(x) & 0x000000ff00000000ULL) >> 8) | \ + (((uint64_t)(x) & 0x0000ff0000000000ULL) >> 24) | \ + (((uint64_t)(x) & 0x00ff000000000000ULL) >> 40) | \ + (((uint64_t)(x) & 0xff00000000000000ULL) >> 56) )) + +#define le16_to_cpu(x) swap16(x) +#define le32_to_cpu(x) swap32(x) +#define le64_to_cpu(x) swap64(x) + +#define cpu_to_le16(x) swap16(x) +#define cpu_to_le32(x) swap32(x) +#define cpu_to_le64(x) swap64(x) + +#endif + +#define le16_to_cpup(x) le16_to_cpu(*((uint16_t *)(x))) +#define le32_to_cpup(x) le32_to_cpu(*((uint32_t *)(x))) +#define le64_to_cpup(x) le64_to_cpu(*((uint64_t *)(x))) + +#endif /* ifndef __KERNEL__ */ + +/****************************************************************************** + * Read macros + *****************************************************************************/ + +/** Read an 8-bit unsigned value from Buffer data. + * + * \return Buffer data value + */ +#define read_u8(DATA) \ + ((uint8_t) *((uint8_t *) (DATA))) + +/** Read an 8-bit signed value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_s8(DATA) \ + ((int8_t) *((uint8_t *) (DATA))) + +/** Read a 16-bit unsigned value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_u16(DATA) \ + ((uint16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 16-bit signed value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_s16(DATA) \ + ((int16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 32-bit unsigned value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_u32(DATA) \ + ((uint32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 32-bit signed value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_s32(DATA) \ + ((int32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 64-bit unsigned value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_u64(DATA) \ + ((uint64_t) le64_to_cpup((void *) (DATA))) + +/** Read a 64-bit signed value from Buffer data. + * + * \param DATA Buffer data pointer + * \return Buffer data value + */ +#define read_s64(DATA) \ + ((int64_t) le64_to_cpup((void *) (DATA))) + +/****************************************************************************** + * Write macros + *****************************************************************************/ + +/** Write an 8-bit unsigned value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_u8(DATA, VAL) \ + do { \ + *((uint8_t *)(DATA)) = ((uint8_t) (VAL)); \ + } while (0) + +/** Write an 8-bit signed value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_s8(DATA, VAL) write_u8(DATA, VAL) + +/** Write a 16-bit unsigned value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_u16(DATA, VAL) \ + do { \ + *((uint16_t *) (DATA)) = cpu_to_le16((uint16_t) (VAL)); \ + } while (0) + +/** Write a 16-bit signed value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_s16(DATA, VAL) write_u16(DATA, VAL) + +/** Write a 32-bit unsigned value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_u32(DATA, VAL) \ + do { \ + *((uint32_t *) (DATA)) = cpu_to_le32((uint32_t) (VAL)); \ + } while (0) + +/** Write a 32-bit signed value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_s32(DATA, VAL) write_u32(DATA, VAL) + +/** Write a 64-bit unsigned value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_u64(DATA, VAL) \ + do { \ + *((uint64_t *) (DATA)) = cpu_to_le64((uint64_t) (VAL)); \ + } while (0) + +/** Write a 64-bit signed value to Buffer data. + * + * \param DATA Buffer data pointer + * \param VAL new value + */ +#define write_s64(DATA, VAL) write_u64(DATA, VAL) + +#endif // ETHERCAT_INTERFACE__EC_BUFFER_TOOLS_H_ diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index da037331..4fe41f8b 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// 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. @@ -11,167 +11,49 @@ // 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) #ifndef ETHERCAT_INTERFACE__EC_MASTER_HPP_ #define ETHERCAT_INTERFACE__EC_MASTER_HPP_ -#include - -#include #include -#include -#include -#include +#include #include "ethercat_interface/ec_slave.hpp" - namespace ethercat_interface { - class EcMaster { public: - explicit EcMaster(const int master = 0); - virtual ~EcMaster(); + EcMaster() {} + virtual ~EcMaster() {} - /** \brief add a slave device to the master - * alias and position can be found by running the following command - * /opt/etherlab/bin$ sudo ./ethercat slaves - * look for the "A B:C STATUS DEVICE" (e.g. B=alias, C=position) - */ - void addSlave(uint16_t alias, uint16_t position, EcSlave * slave); + /** \brief add a slave device to the master */ + virtual bool add_slave(std::shared_ptr slave) = 0; - /** \brief configure slave using SDO - */ - int configSlaveSdo(uint16_t slave_position, SdoConfigEntry sdo_config, uint32_t * abort_code); + /** \brief configure slave using SDO */ + virtual bool configure_slaves() = 0; - /** call after adding all slaves, and before update */ - void activate(); + virtual bool init(std::string iface) = 0; - /** perform one EtherCAT cycle, passing the domain to the slaves */ - virtual void update(uint32_t domain = 0); + virtual bool start() = 0; - /** run a control loop of update() and user_callback(), blocking. - * call activate and setThreadHighPriority/RealTime first. */ - typedef void (* SIMPLECAT_CONTRL_CALLBACK)(void); - virtual void run(SIMPLECAT_CONTRL_CALLBACK user_callback); + virtual bool stop() = 0; - /** stop the control loop. use within callback, or from a separate thread. */ - virtual void stop() {running_ = false;} + virtual bool spin_slaves_until_operational() = 0; - /** time of last ethercat update, since calling run. stops if stop called. - * returns actual time. use elapsedCycles()/frequency for discrete time at last update. */ - virtual double elapsedTime(); + virtual bool read_process_data() = 0; - /** number of EtherCAT updates since calling run. */ - virtual uint64_t elapsedCycles(); + virtual bool write_process_data() = 0; - /** add ctr-c exit callback. - * default exits the run loop and prints timing */ - typedef void (* SIMPLECAT_EXIT_CALLBACK)(int); - static void setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback = NULL); - - /** set the thread to a priority of -19 - * priority range is -20 (highest) to 19 (lowest) */ - static void setThreadHighPriority(); - - /** set the thread to real time (FIFO) - * thread cannot be preempted. - * set priority as 49 (kernel and interrupts are 50) */ - static void setThreadRealTime(); - - void setCtrlFrequency(double frequency) + void set_ctrl_frequency(double frequency) { interval_ = 1000000000.0 / frequency; } - uint32_t getInterval() {return interval_;} - - void readData(uint32_t domain = 0); - void writeData(uint32_t domain = 0); - -private: - /** true if running */ - volatile bool running_ = false; - - /** start and current time */ - std::chrono::time_point start_t_, curr_t_; - - // EtherCAT Control - - /** register a domain of the slave */ - struct DomainInfo; - void registerPDOInDomain( - uint16_t alias, uint16_t position, - std::vector & channel_indices, - DomainInfo * domain_info, - EcSlave * slave); - - /** check for change in the domain state */ - void checkDomainState(uint32_t domain); - - /** check for change in the master state */ - void checkMasterState(); - - /** check for change in the slave states */ - void checkSlaveStates(); - - /** print warning message to terminal */ - static void printWarning(const std::string & message); - - /** EtherCAT master data */ - ec_master_t * master_ = NULL; - ec_master_state_t master_state_ = {}; - - /** data for a single domain */ - struct DomainInfo - { - explicit DomainInfo(ec_master_t * master); - ~DomainInfo(); - - ec_domain_t * domain = NULL; - ec_domain_state_t domain_state = {}; - uint8_t * domain_pd = NULL; - - /** domain pdo registration array. - * do not modify after active(), or may invalidate */ - std::vector domain_regs; - - /** slave's pdo entries in the domain */ - struct Entry - { - EcSlave * slave = NULL; - int num_pdos = 0; - uint32_t * offset = NULL; - uint32_t * bit_position = NULL; - }; - - std::vector entries; - }; - - /** map from domain index to domain info */ - std::map domain_info_; - - /** data needed to check slave state */ - struct SlaveInfo - { - EcSlave * slave = NULL; - ec_slave_config_t * config = NULL; - ec_slave_config_state_t config_state = {0}; - }; - - std::vector slave_info_; - - /** counter of control loops */ - uint64_t update_counter_ = 0; - - /** frequency to check for master or slave state change. - * state checked every frequency_ control loops */ - uint32_t check_state_frequency_ = 10; - +protected: uint32_t interval_; }; - } // namespace ethercat_interface - #endif // ETHERCAT_INTERFACE__EC_MASTER_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index f6981d93..2264331e 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Maciej Bednarczyk (macbednarczyk@gmail.com) +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) #ifndef ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_ #define ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_ -#include #include #include #include #include "yaml-cpp/yaml.h" +#include "ethercat_interface/ec_buffer_tools.h" namespace ethercat_interface { @@ -36,6 +36,26 @@ class EcPdoChannelManager { public: EcPdoChannelManager() {} + EcPdoChannelManager( + uint16_t index, + uint8_t sub_index, + PdoType pdo_type, + std::string data_type, + std::string interface_name = "", + uint8_t data_mask = 255, + double default_value = std::numeric_limits::quiet_NaN(), + double factor = 1, + double offset = 0) + : index(index), + sub_index(sub_index), + pdo_type(pdo_type), + data_type(data_type), + interface_name(interface_name), + data_mask(data_mask), + default_value(default_value), + factor(factor), + offset(offset) + {} ~EcPdoChannelManager() {} void setup_interface_ptrs( std::vector * state_interface, @@ -45,30 +65,28 @@ class EcPdoChannelManager state_interface_ptr_ = state_interface; } - ec_pdo_entry_info_t get_pdo_entry_info() {return {index, sub_index, type2bits(data_type)};} - double ec_read(uint8_t * domain_address) { if (data_type == "uint8") { - last_value = static_cast(EC_READ_U8(domain_address)); + last_value = static_cast(read_u8(domain_address)); } else if (data_type == "int8") { - last_value = static_cast(EC_READ_S8(domain_address)); + last_value = static_cast(read_s8(domain_address)); } else if (data_type == "uint16") { - last_value = static_cast(EC_READ_U16(domain_address)); + last_value = static_cast(read_u16(domain_address)); } else if (data_type == "int16") { - last_value = static_cast(EC_READ_S16(domain_address)); + last_value = static_cast(read_s16(domain_address)); } else if (data_type == "uint32") { - last_value = static_cast(EC_READ_U32(domain_address)); + last_value = static_cast(read_u32(domain_address)); } else if (data_type == "int32") { - last_value = static_cast(EC_READ_S32(domain_address)); + last_value = static_cast(read_s32(domain_address)); } else if (data_type == "uint64") { - last_value = static_cast(EC_READ_U64(domain_address)); + last_value = static_cast(read_u64(domain_address)); } else if (data_type == "int64") { - last_value = static_cast(EC_READ_S64(domain_address)); + last_value = static_cast(read_s64(domain_address)); } else if (data_type == "bool") { - last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0; + last_value = (read_u8(domain_address) & data_mask) ? 1 : 0; } else { - last_value = static_cast(EC_READ_U8(domain_address) & data_mask); + last_value = static_cast(read_u8(domain_address) & data_mask); } last_value = factor * last_value + offset; return last_value; @@ -77,23 +95,23 @@ class EcPdoChannelManager void ec_write(uint8_t * domain_address, double value) { if (data_type == "uint8") { - EC_WRITE_U8(domain_address, static_cast(value)); + write_u8(domain_address, static_cast(value)); } else if (data_type == "int8") { - EC_WRITE_S8(domain_address, static_cast(value)); + write_s8(domain_address, static_cast(value)); } else if (data_type == "uint16") { - EC_WRITE_U16(domain_address, static_cast(value)); + write_u16(domain_address, static_cast(value)); } else if (data_type == "int16") { - EC_WRITE_S16(domain_address, static_cast(value)); + write_s16(domain_address, static_cast(value)); } else if (data_type == "uint32") { - EC_WRITE_U32(domain_address, static_cast(value)); + write_u32(domain_address, static_cast(value)); } else if (data_type == "int32") { - EC_WRITE_S32(domain_address, static_cast(value)); + write_s32(domain_address, static_cast(value)); } else if (data_type == "uint64") { - EC_WRITE_U64(domain_address, static_cast(value)); + write_u64(domain_address, static_cast(value)); } else if (data_type == "int64") { - EC_WRITE_S64(domain_address, static_cast(value)); + write_s64(domain_address, static_cast(value)); } else { - buffer_ = EC_READ_U8(domain_address); + buffer_ = read_u8(domain_address); if (popcount(data_mask) == 1) { buffer_ &= ~(data_mask); if (value) {buffer_ |= data_mask;} @@ -101,7 +119,7 @@ class EcPdoChannelManager buffer_ = 0; buffer_ |= (static_cast(value) & data_mask); } - EC_WRITE_U8(domain_address, buffer_); + write_u8(domain_address, buffer_); } last_value = value; } @@ -203,9 +221,11 @@ class EcPdoChannelManager return -1; } - PdoType pdo_type; + uint8_t get_bits_size() {return type2bits(data_type);} + uint16_t index; uint8_t sub_index; + PdoType pdo_type; std::string data_type; std::string interface_name; uint8_t data_mask = 255; diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 9f141595..6a829645 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Maciej Bednarczyk (macbednarczyk@gmail.com) +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) #ifndef ETHERCAT_INTERFACE__EC_SDO_MANAGER_HPP_ #define ETHERCAT_INTERFACE__EC_SDO_MANAGER_HPP_ -#include #include #include #include +#include #include "yaml-cpp/yaml.h" +#include "ethercat_interface/ec_buffer_tools.h" namespace ethercat_interface { @@ -31,26 +32,33 @@ class SdoConfigEntry { public: SdoConfigEntry() {} + SdoConfigEntry(uint16_t index, uint8_t sub_index, std::string data_type, int data) + : index(index), + sub_index(sub_index), + data_type(data_type), + data(data) + { + } ~SdoConfigEntry() {} void buffer_write(uint8_t * buffer) { if (data_type == "uint8") { - EC_WRITE_U8(buffer, static_cast(data)); + write_u8(buffer, static_cast(data)); } else if (data_type == "int8") { - EC_WRITE_S8(buffer, static_cast(data)); + write_s8(buffer, static_cast(data)); } else if (data_type == "uint16") { - EC_WRITE_U16(buffer, static_cast(data)); + write_u16(buffer, static_cast(data)); } else if (data_type == "int16") { - EC_WRITE_S16(buffer, static_cast(data)); + write_s16(buffer, static_cast(data)); } else if (data_type == "uint32") { - EC_WRITE_U32(buffer, static_cast(data)); + write_u32(buffer, static_cast(data)); } else if (data_type == "int32") { - EC_WRITE_S32(buffer, static_cast(data)); + write_s32(buffer, static_cast(data)); } else if (data_type == "uint64") { - EC_WRITE_U64(buffer, static_cast(data)); + write_u64(buffer, static_cast(data)); } else if (data_type == "int64") { - EC_WRITE_S64(buffer, static_cast(data)); + write_s64(buffer, static_cast(data)); } } @@ -109,6 +117,8 @@ class SdoConfigEntry return 4; } else if (type == "int64" || type == "uint64") { return 8; + } else { + return 0; } } }; diff --git a/ethercat_interface/include/ethercat_interface/ec_slave.hpp b/ethercat_interface/include/ethercat_interface/ec_slave.hpp index da88a826..6ac06ee8 100644 --- a/ethercat_interface/include/ethercat_interface/ec_slave.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_slave.hpp @@ -15,7 +15,6 @@ #ifndef ETHERCAT_INTERFACE__EC_SLAVE_HPP_ #define ETHERCAT_INTERFACE__EC_SLAVE_HPP_ -#include #include #include #include @@ -24,34 +23,41 @@ #include #include "ethercat_interface/ec_sdo_manager.hpp" +#include "ethercat_interface/ec_pdo_channel_manager.hpp" +#include "ethercat_interface/ec_sync_manager.hpp" +#include "ethercat_interface/ec_buffer_tools.h" namespace ethercat_interface { +typedef std::vector pdo_channels_t; +typedef std::vector sm_config_t; +typedef std::vector sdo_config_t; +typedef struct +{ + uint16_t index; + ethercat_interface::PdoType pdo_type; + pdo_channels_t pdo_channel_config; +} pdo_mapping_t; +typedef std::vector pdo_config_t; + class EcSlave { public: - EcSlave(uint32_t vendor_id, uint32_t product_id) - : vendor_id_(vendor_id), - product_id_(product_id) {} - virtual ~EcSlave() {} + EcSlave() {} + ~EcSlave() {} /** read or write data to the domain */ - virtual void processData(size_t /*index*/, uint8_t * /*domain_address*/) {} - /** a pointer to syncs. return &syncs[0] */ - virtual const ec_sync_info_t * syncs() {return NULL;} - virtual bool initialized() {return true;} + virtual int process_data( + size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address) + { + pdo_config_[pdo_mapping_index].pdo_channel_config[pdo_channel_index].ec_update(domain_address); + return 0; + } virtual void set_state_is_operational(bool value) {is_operational_ = value;} /** Assign activate DC synchronization. return activate word*/ - virtual int assign_activate_dc_sync() {return 0x00;} - /** number of elements in the syncs array. */ - virtual size_t syncSize() {return 0;} - /** a pointer to all PDO entries */ - virtual const ec_pdo_entry_info_t * channels() {return NULL;} - /** a map from domain index to pdo indices in that domain. - * map > */ - typedef std::map> DomainMap; - virtual void domains(DomainMap & /*domains*/) const {} - virtual bool setupSlave( + virtual int dc_sync() {return assign_activate_;} + bool initialized() {return is_initialized_;} + virtual bool setup_slave( std::unordered_map slave_paramters, std::vector * state_interface, std::vector * command_interface) @@ -59,18 +65,80 @@ class EcSlave state_interface_ptr_ = state_interface; command_interface_ptr_ = command_interface; paramters_ = slave_paramters; + is_initialized_ = true; + setup_interface_mapping(); return true; } - uint32_t vendor_id_; - uint32_t product_id_; - std::vector sdo_config; + uint32_t get_vendor_id() {return vendor_id_;} + uint32_t get_product_id() {return product_id_;} + + pdo_config_t get_pdo_config() + { + return pdo_config_; + } + + sm_config_t get_sm_config() + { + return sm_config_; + } + + sdo_config_t get_sdo_config() + { + return sdo_config_; + } + + std::unordered_map get_slave_parameters() + { + return paramters_; + } + + std::vector * get_state_interface_ptr() + { + return state_interface_ptr_; + } + + std::vector * get_command_interface_ptr() + { + return command_interface_ptr_; + } protected: std::vector * state_interface_ptr_; std::vector * command_interface_ptr_; std::unordered_map paramters_; + bool is_initialized_ = true; bool is_operational_ = false; + uint32_t vendor_id_ = 0; + uint32_t product_id_ = 0; + uint32_t assign_activate_ = 0; + + pdo_config_t pdo_config_; + sm_config_t sm_config_; + sdo_config_t sdo_config_; + + void setup_interface_mapping() + { + for (auto & mapping : pdo_config_) { + for (auto & channel : mapping.pdo_channel_config) { + if (channel.pdo_type == ethercat_interface::TPDO) { + if (paramters_.find("state_interface/" + channel.interface_name) != paramters_.end()) { + channel.interface_index = + std::stoi(paramters_["state_interface/" + channel.interface_name]); + } + } + if (channel.pdo_type == ethercat_interface::RPDO) { + if (paramters_.find("command_interface/" + channel.interface_name) != paramters_.end()) { + channel.interface_index = std::stoi( + paramters_["command_interface/" + channel.interface_name]); + } + } + + channel.setup_interface_ptrs(state_interface_ptr_, command_interface_ptr_); + } + } + } }; + } // namespace ethercat_interface #endif // ETHERCAT_INTERFACE__EC_SLAVE_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_sync_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sync_manager.hpp index e32e7f50..847c44ee 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sync_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sync_manager.hpp @@ -17,7 +17,6 @@ #ifndef ETHERCAT_INTERFACE__EC_SYNC_MANAGER_HPP_ #define ETHERCAT_INTERFACE__EC_SYNC_MANAGER_HPP_ -#include #include #include #include @@ -31,6 +30,13 @@ class SMConfig { public: SMConfig() {} + SMConfig(uint8_t index, int type, std::string pdo_name = "null", int watchdog = 0) + : index(index), + type(type), + pdo_name(pdo_name), + watchdog(watchdog) + { + } ~SMConfig() {} bool load_from_config(YAML::Node sm_config) @@ -45,9 +51,9 @@ class SMConfig // type if (sm_config["type"]) { if (sm_config["type"].as() == "input") { - type = EC_DIR_INPUT; + type = 1; } else if (sm_config["type"].as() == "output") { - type = EC_DIR_OUTPUT; + type = 0; } else { std::cerr << "sm " << index << ": type should be input/output" << std::endl; return false; @@ -67,9 +73,9 @@ class SMConfig // watchdog if (sm_config["watchdog"]) { if (sm_config["watchdog"].as() == "enable") { - watchdog = EC_WD_ENABLE; + watchdog = 1; } else if (sm_config["watchdog"].as() == "disable") { - watchdog = EC_WD_DISABLE; + watchdog = -1; } } @@ -77,9 +83,9 @@ class SMConfig } uint8_t index; - ec_direction_t type; + int type; // 0=output, 1=input std::string pdo_name = "null"; - ec_watchdog_mode_t watchdog = EC_WD_DEFAULT; + int watchdog = 0; // 0=default, 1=enable, -1=disable }; } // namespace ethercat_interface diff --git a/ethercat_interface/test/test_ec_pdo_channel_manager.cpp b/ethercat_interface/test/test_ec_pdo_channel_manager.cpp index a43dd51c..e5f8a10f 100644 --- a/ethercat_interface/test/test_ec_pdo_channel_manager.cpp +++ b/ethercat_interface/test/test_ec_pdo_channel_manager.cpp @@ -50,7 +50,7 @@ TEST(TestEcPdoChannelManager, EcReadS16) pdo_manager.load_from_config(config); uint8_t buffer[16]; - EC_WRITE_S16(buffer, 42); + write_s16(buffer, 42); ASSERT_EQ(pdo_manager.ec_read(buffer), 2 * 42 + 10); } @@ -70,19 +70,19 @@ TEST(TestEcPdoChannelManager, EcReadWriteBit2) ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 2); uint8_t buffer[1]; - EC_WRITE_U8(buffer, 0); + write_u8(buffer, 0); ASSERT_EQ(pdo_manager.ec_read(buffer), 0); - EC_WRITE_U8(buffer, 3); + write_u8(buffer, 3); ASSERT_EQ(pdo_manager.ec_read(buffer), 3); - EC_WRITE_U8(buffer, 5); + write_u8(buffer, 5); ASSERT_EQ(pdo_manager.ec_read(buffer), 1); pdo_manager.ec_write(buffer, 0); - ASSERT_EQ(EC_READ_U8(buffer), 0); + ASSERT_EQ(read_u8(buffer), 0); pdo_manager.ec_write(buffer, 2); - ASSERT_EQ(EC_READ_U8(buffer), 2); + ASSERT_EQ(read_u8(buffer), 2); pdo_manager.ec_write(buffer, 5); - ASSERT_EQ(EC_READ_U8(buffer), 1); + ASSERT_EQ(read_u8(buffer), 1); } TEST(TestEcPdoChannelManager, EcReadWriteBoolMask1) @@ -101,15 +101,15 @@ TEST(TestEcPdoChannelManager, EcReadWriteBoolMask1) ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 1); uint8_t buffer[1]; - EC_WRITE_U8(buffer, 3); + write_u8(buffer, 3); ASSERT_EQ(pdo_manager.ec_read(buffer), 1); - EC_WRITE_U8(buffer, 0); + write_u8(buffer, 0); ASSERT_EQ(pdo_manager.ec_read(buffer), 0); pdo_manager.ec_write(buffer, 0); - ASSERT_EQ(EC_READ_U8(buffer), 0); + ASSERT_EQ(read_u8(buffer), 0); pdo_manager.ec_write(buffer, 5); - ASSERT_EQ(EC_READ_U8(buffer), 1); + ASSERT_EQ(read_u8(buffer), 1); } TEST(TestEcPdoChannelManager, EcReadWriteBoolMask5) @@ -128,17 +128,17 @@ TEST(TestEcPdoChannelManager, EcReadWriteBoolMask5) ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 1); uint8_t buffer[1]; - EC_WRITE_U8(buffer, 7); + write_u8(buffer, 7); ASSERT_EQ(pdo_manager.ec_read(buffer), 1); - EC_WRITE_U8(buffer, 0); + write_u8(buffer, 0); ASSERT_EQ(pdo_manager.ec_read(buffer), 0); pdo_manager.ec_write(buffer, 0); - ASSERT_EQ(EC_READ_U8(buffer), 0); + ASSERT_EQ(read_u8(buffer), 0); pdo_manager.ec_write(buffer, 3); - ASSERT_EQ(EC_READ_U8(buffer), 1); + ASSERT_EQ(read_u8(buffer), 1); pdo_manager.ec_write(buffer, 7); - ASSERT_EQ(EC_READ_U8(buffer), 5); + ASSERT_EQ(read_u8(buffer), 5); pdo_manager.ec_write(buffer, 5); - ASSERT_EQ(EC_READ_U8(buffer), 5); + ASSERT_EQ(read_u8(buffer), 5); } diff --git a/ethercat_master/ethercat_master_etherlab/CMakeLists.txt b/ethercat_master/ethercat_master_etherlab/CMakeLists.txt new file mode 100644 index 00000000..93454574 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.8) +project(ethercat_master_etherlab) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ethercat_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +# EtherLab +set(ETHERLAB_DIR /usr/local/etherlab) +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib) + +file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) +add_library(${PROJECT_NAME} SHARED ${PLUGINS_SRC}) +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${ETHERLAB_DIR}/include +) +target_link_libraries(${PROJECT_NAME} ${ETHERCAT_LIB}) + +ament_target_dependencies( + ${PROJECT_NAME} + ethercat_interface + pluginlib + rclcpp +) + +pluginlib_export_plugin_description_file(ethercat_interface master_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(pluginlib REQUIRED) + find_package(ethercat_interface REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Test Load EtherCAT modules + ament_add_gmock( + test_load_${PROJECT_NAME} + test/test_load_ec_master_etherlab.cpp + ) + target_include_directories(test_load_${PROJECT_NAME} PRIVATE include ${ETHERLAB_DIR}/include) + ament_target_dependencies(test_load_${PROJECT_NAME} + pluginlib + ethercat_interface + rclcpp + ) + # Test Etherlab EtherCAT Slave + ament_add_gmock( + test_etherlab_slave + test/test_etherlab_slave.cpp + ) + target_include_directories(test_etherlab_slave PRIVATE include ${ETHERLAB_DIR}/include) + target_link_libraries(test_etherlab_slave + ${PROJECT_NAME} + ) + ament_target_dependencies(test_etherlab_slave + pluginlib + ethercat_interface + ) +endif() + +ament_export_include_directories( + include + ${ETHERLAB_DIR}/include +) +ament_export_libraries( + ${PROJECT_NAME} + ${ETHERCAT_LIBRARY} +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_master_etherlab.hpp b/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_master_etherlab.hpp new file mode 100644 index 00000000..cb2d358e --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_master_etherlab.hpp @@ -0,0 +1,137 @@ +// 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_MASTER__EC_MASTER_ETHERLAB_HPP_ +#define ETHERCAT_MASTER__EC_MASTER_ETHERLAB_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "ethercat_interface/ec_master.hpp" +#include "ethercat_interface/ec_slave.hpp" +#include "ethercat_master/ec_slave_etherlab.hpp" + +namespace ethercat_master +{ + +class EtherlabMaster : public ethercat_interface::EcMaster +{ +public: + EtherlabMaster(); + ~EtherlabMaster(); + + bool init(std::string master_interface = "0"); + + bool add_slave(std::shared_ptr slave); + + bool configure_slaves(); + + bool start(); + + void update(uint32_t domain = 0); + + bool spin_slaves_until_operational(); + + /** stop the control loop. + */ + bool stop(); + + uint32_t get_interval() {return interval_;} + + bool read_process_data(); + bool write_process_data(); + +private: + // EtherCAT Control + + /** register a domain of the slave */ + struct DomainInfo; + void registerPDOInDomain( + uint16_t alias, uint16_t position, + std::vector & channel_indices, + DomainInfo * domain_info, + std::shared_ptr slave); + + /** check for change in the domain state */ + void checkDomainState(uint32_t domain); + + /** check for change in the master state */ + void checkMasterState(); + + /** check for change in the slave states */ + void checkSlaveStates(); + + /** print warning message to terminal */ + static void printWarning(const std::string & message); + + /** EtherCAT master data */ + ec_master_t * master_ = NULL; + ec_master_state_t master_state_ = {}; + + /** data for a single domain */ + struct DomainInfo + { + explicit DomainInfo(ec_master_t * master); + ~DomainInfo(); + + ec_domain_t * domain = NULL; + ec_domain_state_t domain_state = {}; + uint8_t * domain_pd = NULL; + + /** domain pdo registration array. + * do not modify after active(), or may invalidate */ + std::vector domain_regs; + + /** slave's pdo entries in the domain */ + struct Entry + { + std::shared_ptr slave = nullptr; + int num_pdos = 0; + uint32_t * offset = NULL; + uint32_t * bit_position = NULL; + }; + + std::vector entries; + }; + + /** map from domain index to domain info */ + std::map domain_info_; + + /** data needed to check slave state */ + struct SlaveInfo + { + std::shared_ptr slave = nullptr; + ec_slave_config_t * config = NULL; + ec_slave_config_state_t config_state = {0}; + }; + + std::vector slave_info_; + + /** counter of control loops */ + uint64_t update_counter_ = 0; + + /** frequency to check for master or slave state change. + * state checked every frequency_ control loops */ + uint32_t check_state_frequency_ = 10; +}; + +} // namespace ethercat_master + +#endif // ETHERCAT_MASTER__EC_MASTER_ETHERLAB_HPP_ diff --git a/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_slave_etherlab.hpp b/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_slave_etherlab.hpp new file mode 100644 index 00000000..8902bb54 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/include/ethercat_master/ec_slave_etherlab.hpp @@ -0,0 +1,83 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#ifndef ETHERCAT_MASTER__EC_SLAVE_ETHERLAB_HPP_ +#define ETHERCAT_MASTER__EC_SLAVE_ETHERLAB_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ethercat_interface/ec_sdo_manager.hpp" +#include "ethercat_interface/ec_slave.hpp" + +namespace ethercat_master +{ + +class EtherlabSlave +{ +public: + explicit EtherlabSlave(std::shared_ptr slave); + ~EtherlabSlave(); + /** read or write data to the domain */ + int process_data(size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address); + /** a pointer to syncs. return &syncs[0] */ + const ec_sync_info_t * syncs(); + bool initialized(); + void set_state_is_operational(bool value); + /** Assign activate DC synchronization. return activate word*/ + int dc_sync(); + /** number of elements in the syncs array. */ + size_t sync_size(); + /** a pointer to all PDO entries */ + const ec_pdo_entry_info_t * channels(); + /** a map from domain index to pdo indices in that domain. + * map > */ + typedef std::map> DomainMap; + void domains(DomainMap & /*domains*/) const; + + uint32_t get_vendor_id(); + uint32_t get_product_id(); + int get_bus_position(); + int get_bus_alias(); + + ethercat_interface::pdo_config_t get_pdo_config(); + ethercat_interface::sm_config_t get_sm_config(); + ethercat_interface::sdo_config_t get_sdo_config(); + +protected: + std::shared_ptr slave_; + int bus_position_; + int bus_alias_; + + std::vector rpdos_; + std::vector tpdos_; + std::vector all_channels_; + std::vector syncs_; + std::vector domain_map_; + + bool setup_slave(); + void setup_syncs(); + ec_direction_t set_sm_type(int type); + ec_watchdog_mode_t set_sm_watchdog(int watchdog); +}; +} // namespace ethercat_master +#endif // ETHERCAT_MASTER__EC_SLAVE_ETHERLAB_HPP_ diff --git a/ethercat_master/ethercat_master_etherlab/master_plugin.xml b/ethercat_master/ethercat_master_etherlab/master_plugin.xml new file mode 100644 index 00000000..1714600c --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/master_plugin.xml @@ -0,0 +1,7 @@ + + + Etherlab Master interface. + + diff --git a/ethercat_master/ethercat_master_etherlab/package.xml b/ethercat_master/ethercat_master_etherlab/package.xml new file mode 100644 index 00000000..e8007d49 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/package.xml @@ -0,0 +1,21 @@ + + + + ethercat_master_etherlab + 1.2.0 + Plugin implementations of Etherlab Master interface + Maciej Bednarczyk + Apache-2.0 + + ament_cmake_ros + + ethercat_interface + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_master/ethercat_master_etherlab/src/ec_master_etherlab.cpp similarity index 62% rename from ethercat_interface/src/ec_master.cpp rename to ethercat_master/ethercat_master_etherlab/src/ec_master_etherlab.cpp index 2b91363f..3fb81518 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_master/ethercat_master_etherlab/src/ec_master_etherlab.cpp @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ethercat_interface/ec_master.hpp" -#include "ethercat_interface/ec_slave.hpp" - #include #include #include @@ -26,13 +23,17 @@ #include #include +#include "ethercat_master/ec_master_etherlab.hpp" +#include "ethercat_interface/ec_master.hpp" +#include "rclcpp/rclcpp.hpp" + #define EC_NEWTIMEVAL2NANO(TV) \ (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec) -namespace ethercat_interface +namespace ethercat_master { -EcMaster::DomainInfo::DomainInfo(ec_master_t * master) +EtherlabMaster::DomainInfo::DomainInfo(ec_master_t * master) { domain = ecrt_master_create_domain(master); if (domain == NULL) { @@ -45,7 +46,7 @@ EcMaster::DomainInfo::DomainInfo(ec_master_t * master) } -EcMaster::DomainInfo::~DomainInfo() +EtherlabMaster::DomainInfo::~DomainInfo() { for (Entry & entry : entries) { delete[] entry.offset; @@ -54,77 +55,81 @@ EcMaster::DomainInfo::~DomainInfo() } -EcMaster::EcMaster(const int master) +EtherlabMaster::EtherlabMaster() { - master_ = ecrt_request_master(master); - if (master_ == NULL) { - printWarning("Failed to obtain master."); - return; - } interval_ = 0; } -EcMaster::~EcMaster() +EtherlabMaster::~EtherlabMaster() { - for (SlaveInfo & slave : slave_info_) { - // - } for (auto & domain : domain_info_) { delete domain.second; } } -void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) +bool EtherlabMaster::init(std::string master_interface) +{ + master_ = ecrt_request_master(std::stoi(master_interface)); + if (master_ == NULL) { + printWarning("Failed to obtain master."); + return false; + } + return true; +} + +bool EtherlabMaster::add_slave(std::shared_ptr slave) { // configure slave in master - SlaveInfo slave_info; - slave_info.slave = slave; - slave_info.config = ecrt_master_slave_config( - master_, alias, position, - slave->vendor_id_, - slave->product_id_); - if (slave_info.config == NULL) { + slave_info_.emplace_back(); + + slave_info_.back().slave = std::make_shared(slave); + slave_info_.back().config = ecrt_master_slave_config( + master_, + slave_info_.back().slave->get_bus_alias(), + slave_info_.back().slave->get_bus_position(), + slave_info_.back().slave->get_vendor_id(), + slave_info_.back().slave->get_product_id()); + if (slave_info_.back().config == NULL) { printWarning("Add slave. Failed to get slave configuration."); - return; + return false; } // check and setup dc - if (slave->assign_activate_dc_sync()) { + if (slave_info_.back().slave->dc_sync()) { struct timespec t; clock_gettime(CLOCK_MONOTONIC, &t); ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); ecrt_slave_config_dc( - slave_info.config, - slave->assign_activate_dc_sync(), + slave_info_.back().config, + slave_info_.back().slave->dc_sync(), interval_, interval_ - (t.tv_nsec % (interval_)), 0, 0); } - slave_info_.push_back(slave_info); - // check if slave has pdos - size_t num_syncs = slave->syncSize(); - const ec_sync_info_t * syncs = slave->syncs(); + size_t num_syncs = slave_info_.back().slave->sync_size(); + const ec_sync_info_t * syncs = slave_info_.back().slave->syncs(); if (num_syncs > 0) { // configure pdos in slave - int pdos_status = ecrt_slave_config_pdos(slave_info.config, num_syncs, syncs); + int pdos_status = ecrt_slave_config_pdos(slave_info_.back().config, num_syncs, syncs); if (pdos_status) { printWarning("Add slave. Failed to configure PDOs"); - return; + return false; } } else { printWarning( "Add slave. Sync size is zero for " + - std::to_string(alias) + ":" + std::to_string(position)); + std::to_string(slave_info_.back().slave->get_bus_alias()) + ":" + + std::to_string(slave_info_.back().slave->get_bus_position())); } // check if slave registered any pdos for the domain - EcSlave::DomainMap domain_map; - slave->domains(domain_map); + EtherlabSlave::DomainMap domain_map; + slave_info_.back().slave->domains(domain_map); for (auto & iter : domain_map) { // get the domain info, create if necessary uint32_t domain_index = iter.first; @@ -135,35 +140,51 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) } registerPDOInDomain( - alias, position, + slave_info_.back().slave->get_bus_alias(), slave_info_.back().slave->get_bus_position(), iter.second, domain_info, - slave); + slave_info_.back().slave); } + + return true; } -int EcMaster::configSlaveSdo( - uint16_t slave_position, SdoConfigEntry sdo_config, - uint32_t * abort_code) +bool EtherlabMaster::configure_slaves() { - uint8_t buffer[8]; - sdo_config.buffer_write(buffer); - int ret = ecrt_master_sdo_download( - master_, - slave_position, - sdo_config.index, - sdo_config.sub_index, - buffer, - sdo_config.data_size(), - abort_code - ); - return ret; + for (auto i = 0ul; i < slave_info_.size(); i++) { + for (auto & sdo : slave_info_[i].slave->get_sdo_config()) { + uint8_t buffer[8]; + sdo.buffer_write(buffer); + uint32_t abort_code; + int ret = ecrt_master_sdo_download( + master_, + slave_info_[i].slave->get_bus_position(), + sdo.index, + sdo.sub_index, + buffer, + sdo.data_size(), + &abort_code + ); + + if (ret) { + RCLCPP_FATAL( + rclcpp::get_logger("EtherlabMaster"), + "Failed to download config SDO for module at position %i with Error: %d", + slave_info_[i].slave->get_bus_position(), + abort_code + ); + return false; + } + } + } + + return true; } -void EcMaster::registerPDOInDomain( +void EtherlabMaster::registerPDOInDomain( uint16_t alias, uint16_t position, std::vector & channel_indices, DomainInfo * domain_info, - EcSlave * slave) + std::shared_ptr slave) { // expand the size of the domain uint32_t num_pdo_regs = channel_indices.size(); @@ -178,7 +199,7 @@ void EcMaster::registerPDOInDomain( domain_entry.bit_position = new uint32_t[num_pdo_regs]; domain_info->entries.push_back(domain_entry); - EcSlave::DomainMap domain_map; + EtherlabSlave::DomainMap domain_map; slave->domains(domain_map); // add to array of pdos registrations @@ -188,8 +209,8 @@ void EcMaster::registerPDOInDomain( ec_pdo_entry_reg_t & pdo_reg = domain_info->domain_regs[start_index + i]; pdo_reg.alias = alias; pdo_reg.position = position; - pdo_reg.vendor_id = slave->vendor_id_; - pdo_reg.product_code = slave->product_id_; + pdo_reg.vendor_id = slave->get_vendor_id(); + pdo_reg.product_code = slave->get_product_id(); pdo_reg.index = pdo_regs[channel_indices[i]].index; pdo_reg.subindex = pdo_regs[channel_indices[i]].subindex; pdo_reg.offset = &(domain_entry.offset[i]); @@ -210,7 +231,7 @@ void EcMaster::registerPDOInDomain( domain_info->domain_regs.back() = empty; } -void EcMaster::activate() +bool EtherlabMaster::start() { // register domain for (auto & iter : domain_info_) { @@ -220,7 +241,7 @@ void EcMaster::activate() &(domain_info->domain_regs[0])); if (domain_status) { printWarning("Activate. Failed to register domain PDO entries."); - return; + return false; } } // set application time @@ -232,7 +253,7 @@ void EcMaster::activate() bool activate_status = ecrt_master_activate(master_); if (activate_status) { printWarning("Activate. Failed to activate master."); - return; + return false; } // retrieve domain data @@ -241,12 +262,53 @@ void EcMaster::activate() domain_info->domain_pd = ecrt_domain_data(domain_info->domain); if (domain_info->domain_pd == NULL) { printWarning("Activate. Failed to retrieve domain process data."); - return; + return false; + } + } + return true; +} + +bool EtherlabMaster::stop() +{ + return true; +} + +bool EtherlabMaster::spin_slaves_until_operational() +{ + // start after one second + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + t.tv_sec++; + + bool running = true; + while (running) { + // wait until next shot + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); + // update EtherCAT bus + + update(); + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); + + // check if operational + bool isAllInit = true; + for (auto & slave_info : slave_info_) { + isAllInit = isAllInit && slave_info.slave->initialized(); + } + if (isAllInit) { + running = false; + } + // calculate next shot. carry over nanoseconds into microseconds. + t.tv_nsec += get_interval(); + while (t.tv_nsec >= 1000000000) { + t.tv_nsec -= 1000000000; + t.tv_sec++; } } + + return true; } -void EcMaster::update(uint32_t domain) +void EtherlabMaster::update(uint32_t domain) { // receive process data ecrt_master_receive(master_); @@ -266,8 +328,18 @@ void EcMaster::update(uint32_t domain) // read and write process data for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + int channel_counter = 0; + for (auto mi = 0ul; mi < (entry.slave)->get_pdo_config().size(); mi++) { + for (auto ci = 0ul; ci < (entry.slave)->get_pdo_config()[mi].pdo_channel_config.size(); + ci++) + { + if ((entry.slave)->get_pdo_config()[mi].pdo_channel_config[ci].index != 0x0000) { + (entry.slave)->process_data( + mi, ci, + domain_info->domain_pd + entry.offset[channel_counter]); + channel_counter++; + } + } } } @@ -285,17 +357,17 @@ void EcMaster::update(uint32_t domain) ++update_counter_; } -void EcMaster::readData(uint32_t domain) +bool EtherlabMaster::read_process_data() { // receive process data ecrt_master_receive(master_); - DomainInfo * domain_info = domain_info_[domain]; + DomainInfo * domain_info = domain_info_[0]; ecrt_domain_process(domain_info->domain); // check process data state (optional) - checkDomainState(domain); + checkDomainState(0); // check for master and slave state change if (update_counter_ % check_state_frequency_ == 0) { @@ -305,21 +377,42 @@ void EcMaster::readData(uint32_t domain) // read and write process data for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + int channel_counter = 0; + for (auto mi = 0ul; mi < (entry.slave)->get_pdo_config().size(); mi++) { + for (auto ci = 0ul; ci < (entry.slave)->get_pdo_config()[mi].pdo_channel_config.size(); + ci++) + { + if ((entry.slave)->get_pdo_config()[mi].pdo_channel_config[ci].index != 0x0000) { + (entry.slave)->process_data( + mi, ci, + domain_info->domain_pd + entry.offset[channel_counter]); + channel_counter++; + } + } } } ++update_counter_; + return true; } -void EcMaster::writeData(uint32_t domain) +bool EtherlabMaster::write_process_data() { - DomainInfo * domain_info = domain_info_[domain]; + DomainInfo * domain_info = domain_info_[0]; // read and write process data for (DomainInfo::Entry & entry : domain_info->entries) { - for (int i = 0; i < entry.num_pdos; ++i) { - (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + int channel_counter = 0; + for (auto mi = 0ul; mi < (entry.slave)->get_pdo_config().size(); mi++) { + for (auto ci = 0ul; ci < (entry.slave)->get_pdo_config()[mi].pdo_channel_config.size(); + ci++) + { + if ((entry.slave)->get_pdo_config()[mi].pdo_channel_config[ci].index != 0x0000) { + (entry.slave)->process_data( + mi, ci, + domain_info->domain_pd + entry.offset[channel_counter]); + channel_counter++; + } + } } } @@ -333,98 +426,11 @@ void EcMaster::writeData(uint32_t domain) // send process data ecrt_domain_queue(domain_info->domain); ecrt_master_send(master_); -} -void EcMaster::setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback) -{ - // ctrl c handler - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = user_callback; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + return true; } -void EcMaster::run(SIMPLECAT_CONTRL_CALLBACK user_callback) -{ - // start after one second - struct timespec t; - clock_gettime(CLOCK_MONOTONIC, &t); - t.tv_sec++; - - running_ = true; - start_t_ = std::chrono::system_clock::now(); - while (running_) { - // wait until next shot - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); - - // update EtherCAT bus - this->update(); - - // get actual time - curr_t_ = std::chrono::system_clock::now(); - - // user callback - user_callback(); - - // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += interval_; - while (t.tv_nsec >= 1000000000) { - t.tv_nsec -= 1000000000; - t.tv_sec++; - } - } -} - -double EcMaster::elapsedTime() -{ - std::chrono::duration elapsed_seconds = curr_t_ - start_t_; - return elapsed_seconds.count() - 1.0; // started after 1 second -} - -uint64_t EcMaster::elapsedCycles() -{ - return update_counter_; -} - -void EcMaster::setThreadHighPriority() -{ - pid_t pid = getpid(); - int priority_status = setpriority(PRIO_PROCESS, pid, -19); - if (priority_status) { - printWarning("setThreadHighPriority. Failed to set priority."); - return; - } -} - -void EcMaster::setThreadRealTime() -{ - /* Declare ourself as a real time task, priority 49. - PRREMPT_RT uses priority 50 - for kernel tasklets and interrupt handler by default */ - struct sched_param param; - param.sched_priority = 49; - // pthread_t this_thread = pthread_self(); - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - perror("sched_setscheduler failed"); - exit(-1); - } - - /* Lock memory */ - if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { - perror("mlockall failed"); - exit(-2); - } - - /* Pre-fault our stack - 8*1024 is the maximum stack size - which is guaranteed safe to access without faulting */ - int MAX_SAFE_STACK = 8 * 1024; - unsigned char dummy[MAX_SAFE_STACK]; - memset(dummy, 0, MAX_SAFE_STACK); -} - -void EcMaster::checkDomainState(uint32_t domain) +void EtherlabMaster::checkDomainState(uint32_t domain) { DomainInfo * domain_info = domain_info_[domain]; @@ -441,7 +447,7 @@ void EcMaster::checkDomainState(uint32_t domain) } -void EcMaster::checkMasterState() +void EtherlabMaster::checkMasterState() { ec_master_state_t ms; ecrt_master_state(master_, &ms); @@ -459,7 +465,7 @@ void EcMaster::checkMasterState() } -void EcMaster::checkSlaveStates() +void EtherlabMaster::checkSlaveStates() { for (SlaveInfo & slave : slave_info_) { ec_slave_config_state_t s; @@ -481,9 +487,13 @@ void EcMaster::checkSlaveStates() } -void EcMaster::printWarning(const std::string & message) +void EtherlabMaster::printWarning(const std::string & message) { std::cout << "WARNING. Master. " << message << std::endl; } -} // namespace ethercat_interface +} // namespace ethercat_master + +#include + +PLUGINLIB_EXPORT_CLASS(ethercat_master::EtherlabMaster, ethercat_interface::EcMaster) diff --git a/ethercat_master/ethercat_master_etherlab/src/ec_slave_etherlab.cpp b/ethercat_master/ethercat_master_etherlab/src/ec_slave_etherlab.cpp new file mode 100644 index 00000000..98aa15bf --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/src/ec_slave_etherlab.cpp @@ -0,0 +1,215 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#include "ethercat_master/ec_slave_etherlab.hpp" + +namespace ethercat_master +{ +EtherlabSlave::EtherlabSlave(std::shared_ptr slave) +{ + slave_ = slave; + setup_slave(); +} + +EtherlabSlave::~EtherlabSlave() +{ +} + +int EtherlabSlave::process_data( + size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address) +{ + slave_->process_data(pdo_mapping_index, pdo_channel_index, domain_address); + return 0; +} + +const ec_sync_info_t * EtherlabSlave::syncs() +{ + return syncs_.data(); +} + +bool EtherlabSlave::initialized() +{ + return slave_->initialized(); +} + +void EtherlabSlave::set_state_is_operational(bool value) +{ + slave_->set_state_is_operational(value); +} + +int EtherlabSlave::dc_sync() +{ + return slave_->dc_sync(); +} + +size_t EtherlabSlave::sync_size() +{ + return syncs_.size(); +} + +const ec_pdo_entry_info_t * EtherlabSlave::channels() +{ + return all_channels_.data(); +} + +void EtherlabSlave::domains(DomainMap & domains) const +{ + domains = {{0, domain_map_}}; +} + +ec_direction_t EtherlabSlave::set_sm_type(int type) +{ + return (type == 1) ? EC_DIR_INPUT : EC_DIR_OUTPUT; +} + +ec_watchdog_mode_t EtherlabSlave::set_sm_watchdog(int watchdog) +{ + switch (watchdog) { + case -1: + return EC_WD_DISABLE; + case 0: + return EC_WD_DEFAULT; + case 1: + return EC_WD_ENABLE; + } +} + +void EtherlabSlave::setup_syncs() +{ + if (slave_->get_sm_config().size() == 0) { + syncs_.push_back({0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}); + syncs_.push_back({1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}); + syncs_.push_back({2, EC_DIR_OUTPUT, rpdos_.size(), rpdos_.data(), EC_WD_ENABLE}); + syncs_.push_back({3, EC_DIR_INPUT, tpdos_.size(), tpdos_.data(), EC_WD_DISABLE}); + } else { + for (auto & sm : slave_->get_sm_config()) { + if (sm.pdo_name == "null") { + syncs_.push_back({sm.index, set_sm_type(sm.type), 0, NULL, set_sm_watchdog(sm.watchdog)}); + } else if (sm.pdo_name == "rpdo") { + syncs_.push_back( + {sm.index, set_sm_type(sm.type), rpdos_.size(), + rpdos_.data(), set_sm_watchdog(sm.watchdog)}); + } else if (sm.pdo_name == "tpdo") { + syncs_.push_back( + {sm.index, set_sm_type(sm.type), tpdos_.size(), + tpdos_.data(), set_sm_watchdog(sm.watchdog)}); + } + } + } + syncs_.push_back({0xff}); +} + +bool EtherlabSlave::setup_slave() +{ + auto channels_nbr = 0; + + for (auto & mapping : slave_->get_pdo_config()) { + channels_nbr += mapping.pdo_channel_config.size(); + } + + all_channels_.reserve(channels_nbr); + + channels_nbr = 0; + + for (auto & mapping : slave_->get_pdo_config()) { + for (auto & channel_info : mapping.pdo_channel_config) { + all_channels_.push_back( + { + channel_info.index, + channel_info.sub_index, + channel_info.get_bits_size() + }); + } + if (mapping.pdo_type == ethercat_interface::RPDO) { + rpdos_.push_back( + { + mapping.index, + mapping.pdo_channel_config.size(), + all_channels_.data() + channels_nbr + } + ); + channels_nbr += mapping.pdo_channel_config.size(); + } else if (mapping.pdo_type == ethercat_interface::TPDO) { + tpdos_.push_back( + { + mapping.index, + mapping.pdo_channel_config.size(), + all_channels_.data() + channels_nbr + } + ); + channels_nbr += mapping.pdo_channel_config.size(); + } + } + + // Remove gaps from domain mapping + for (auto i = 0ul; i < all_channels_.size(); i++) { + if (all_channels_[i].index != 0x0000) { + domain_map_.push_back(i); + } + } + + if (slave_->get_slave_parameters().find("position") != slave_->get_slave_parameters().end()) { + bus_position_ = std::stoi(slave_->get_slave_parameters()["position"]); + } else { + bus_position_ = 0; + } + + if (slave_->get_slave_parameters().find("alias") != slave_->get_slave_parameters().end()) { + bus_alias_ = std::stoi(slave_->get_slave_parameters()["alias"]); + } else { + bus_alias_ = 0; + } + + setup_syncs(); + + return true; +} + +uint32_t EtherlabSlave::get_vendor_id() +{ + return slave_->get_vendor_id(); +} + +uint32_t EtherlabSlave::get_product_id() +{ + return slave_->get_product_id(); +} + +int EtherlabSlave::get_bus_position() +{ + return bus_position_; +} + +int EtherlabSlave::get_bus_alias() +{ + return bus_alias_; +} + +ethercat_interface::pdo_config_t EtherlabSlave::get_pdo_config() +{ + return slave_->get_pdo_config(); +} + +ethercat_interface::sm_config_t EtherlabSlave::get_sm_config() +{ + return slave_->get_sm_config(); +} + +ethercat_interface::sdo_config_t EtherlabSlave::get_sdo_config() +{ + return slave_->get_sdo_config(); +} +} // namespace ethercat_master diff --git a/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.cpp b/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.cpp new file mode 100644 index 00000000..3b34cbe4 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.cpp @@ -0,0 +1,229 @@ +// 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 +#include +#include "ethercat_interface/ec_slave.hpp" +#include "test_etherlab_slave.hpp" + +TEST_F(EtherlabSlaveTest, SlaveSetup) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + + ASSERT_EQ( + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ), + true + ); + + etherlab_slave_ = std::make_unique(test_slave_ptr); + + ASSERT_EQ(etherlab_slave_->get_vendor_id(), 0x00000011u); + ASSERT_EQ(etherlab_slave_->get_product_id(), 0x07030924u); + ASSERT_EQ(etherlab_slave_->dc_sync(), 0x0321); + + ASSERT_EQ(etherlab_slave_->get_sm_config().size(), 4u); + ASSERT_EQ(etherlab_slave_->get_sdo_config().size(), 4u); + ASSERT_EQ(etherlab_slave_->get_pdo_config().size(), 3u); + + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[1].interface_name, "velocity"); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[2].factor, 2); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[2].offset, 10); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[3].default_value, 1000); + ASSERT_TRUE(std::isnan(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[0].default_value)); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[4].interface_name, "null"); + ASSERT_EQ( + etherlab_slave_->get_pdo_config()[2].pdo_channel_config[1].interface_name, "analog_input2"); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[4].data_type, "uint16"); + + ASSERT_EQ(etherlab_slave_->rpdos_.size(), 1u); + ASSERT_EQ(etherlab_slave_->rpdos_[0].index, 0x1607u); + + ASSERT_EQ(etherlab_slave_->tpdos_.size(), 2u); + ASSERT_EQ(etherlab_slave_->tpdos_[0].index, 0x1a07u); + ASSERT_EQ(etherlab_slave_->tpdos_[1].index, 0x1a45u); +} + +TEST_F(EtherlabSlaveTest, SlaveSetupPdoChannels) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + std::vector channels( + etherlab_slave_->channels(), + etherlab_slave_->channels() + etherlab_slave_->all_channels_.size() + ); + + ASSERT_EQ(channels.size(), 13u); + ASSERT_EQ(channels[0].index, 0x607au); + ASSERT_EQ(channels[11].index, 0x2205u); + ASSERT_EQ(channels[11].subindex, 0x01u); +} + +TEST_F(EtherlabSlaveTest, SlaveSetupSyncs) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + std::vector syncs( + etherlab_slave_->syncs(), + etherlab_slave_->syncs() + etherlab_slave_->sync_size() + ); + + ASSERT_EQ(syncs.size(), 5u); + ASSERT_EQ(syncs[1].index, 1u); + ASSERT_EQ(syncs[1].dir, EC_DIR_INPUT); + ASSERT_EQ(syncs[1].n_pdos, 0u); + ASSERT_EQ(syncs[1].watchdog_mode, EC_WD_DISABLE); + ASSERT_EQ(syncs[2].dir, EC_DIR_OUTPUT); + ASSERT_EQ(syncs[2].n_pdos, 1u); + ASSERT_EQ(syncs[3].index, 3u); + ASSERT_EQ(syncs[3].dir, EC_DIR_INPUT); + ASSERT_EQ(syncs[3].n_pdos, 2u); + ASSERT_EQ(syncs[3].watchdog_mode, EC_WD_DISABLE); +} + +TEST_F(EtherlabSlaveTest, SlaveSetupDomains) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + std::map> domains; + etherlab_slave_->domains(domains); + + ASSERT_EQ(domains[0].size(), 13u); + ASSERT_EQ(domains[0][0], 0u); + ASSERT_EQ(domains[0][12], 12u); +} + +TEST_F(EtherlabSlaveTest, EcReadTPDOToStateInterface) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0, 0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + slave_paramters["state_interface/effort"] = "1"; + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + ASSERT_EQ(etherlab_slave_->get_pdo_config()[1].pdo_channel_config[2].interface_index, 1); + uint8_t domain_address[2]; + write_s16(domain_address, 42); + etherlab_slave_->process_data(1, 2, domain_address); + ASSERT_EQ(etherlab_slave_->slave_->get_state_interface_ptr()->at(1), 5 * 42 + 15); +} + +TEST_F(EtherlabSlaveTest, EcWriteRPDOFromCommandInterface) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0, 0}; + std::vector command_interface = {0, 42}; + std::unordered_map slave_paramters; + slave_paramters["command_interface/effort"] = "1"; + + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[2].interface_index, 1); + uint8_t domain_address[2]; + etherlab_slave_->process_data(0, 2, domain_address); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[2].last_value, 2 * 42 + 10); + ASSERT_EQ(read_s16(domain_address), 2 * 42 + 10); +} + +TEST_F(EtherlabSlaveTest, EcWriteRPDODefaultValue) +{ + auto test_slave_ptr = std::make_shared(); + std::vector state_interface = {0}; + std::vector command_interface = {0}; + std::unordered_map slave_paramters; + test_slave_ptr->setup_slave( + slave_paramters, + &state_interface, + &command_interface + ); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + uint8_t domain_address[2]; + etherlab_slave_->process_data(0, 2, domain_address); + ASSERT_EQ(etherlab_slave_->get_pdo_config()[0].pdo_channel_config[2].last_value, -5); + ASSERT_EQ(read_s16(domain_address), -5); +} + +TEST_F(EtherlabSlaveTest, SlaveSetupSDOConfig) +{ + auto test_slave_ptr = std::make_shared(); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + ASSERT_EQ(etherlab_slave_->get_sdo_config()[0].index, 0x60C2); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[0].sub_index, 1); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[1].sub_index, 2); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[0].data_size(), 1ul); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[0].data, 10); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[2].index, 0x6098); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[3].data_type, "int32"); + ASSERT_EQ(etherlab_slave_->get_sdo_config()[3].data_size(), 4ul); +} + +TEST_F(EtherlabSlaveTest, SlaveSetupSyncManagerConfig) +{ + auto test_slave_ptr = std::make_shared(); + etherlab_slave_ = std::make_unique(test_slave_ptr); + + ASSERT_EQ(etherlab_slave_->get_sm_config().size(), 4ul); + ASSERT_EQ(etherlab_slave_->get_sm_config()[0].index, 0); + ASSERT_EQ(etherlab_slave_->get_sm_config()[0].type, 0); + ASSERT_EQ(etherlab_slave_->get_sm_config()[0].watchdog, -1); + ASSERT_EQ(etherlab_slave_->get_sm_config()[0].pdo_name, "null"); + ASSERT_EQ(etherlab_slave_->get_sm_config()[2].pdo_name, "rpdo"); + ASSERT_EQ(etherlab_slave_->get_sm_config()[2].watchdog, 1); +} diff --git a/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.hpp b/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.hpp new file mode 100644 index 00000000..8223d848 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/test/test_etherlab_slave.hpp @@ -0,0 +1,145 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#ifndef TEST_ETHERLAB_SLAVE_HPP_ +#define TEST_ETHERLAB_SLAVE_HPP_ + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "ethercat_interface/ec_buffer_tools.h" +#include "ethercat_interface/ec_slave.hpp" +#include "ethercat_master/ec_slave_etherlab.hpp" + + +// subclassing and friending so we can access member variables +class FriendEtherlabSlave : public ethercat_master::EtherlabSlave +{ +public: + explicit FriendEtherlabSlave(std::shared_ptr slave) + : EtherlabSlave(slave) {} + +private: + FRIEND_TEST(EtherlabSlaveTest, SlaveSetup); + FRIEND_TEST(EtherlabSlaveTest, SlaveSetupPdoChannels); + FRIEND_TEST(EtherlabSlaveTest, SlaveSetupSyncs); + FRIEND_TEST(EtherlabSlaveTest, SlaveSetupDomains); + FRIEND_TEST(EtherlabSlaveTest, EcReadTPDOToStateInterface); + FRIEND_TEST(EtherlabSlaveTest, EcWriteRPDOFromCommandInterface); + FRIEND_TEST(EtherlabSlaveTest, EcWriteRPDODefaultValue); + FRIEND_TEST(EtherlabSlaveTest, SlaveSetupSDOConfig); + FRIEND_TEST(EtherlabSlaveTest, SlaveSetupSyncManagerConfig); +}; + +class EtherlabSlaveTest : public ::testing::Test +{ +protected: + std::unique_ptr etherlab_slave_; +}; + +class TestSlave : public ethercat_interface::EcSlave +{ +public: + TestSlave() + { + vendor_id_ = 0x00000011; + product_id_ = 0x07030924; + assign_activate_ = 0x0321; + + sm_config_.push_back(ethercat_interface::SMConfig(0, 0, "null", -1)); + sm_config_.push_back(ethercat_interface::SMConfig(1, 1, "null", -1)); + sm_config_.push_back(ethercat_interface::SMConfig(2, 0, "rpdo", 1)); + sm_config_.push_back(ethercat_interface::SMConfig(3, 1, "tpdo", -1)); + + sdo_config_.push_back(ethercat_interface::SdoConfigEntry(0x60C2, 1, "int8", 10)); + sdo_config_.push_back(ethercat_interface::SdoConfigEntry(0x60C2, 2, "int8", -3)); + sdo_config_.push_back(ethercat_interface::SdoConfigEntry(0x6098, 0, "int8", 35)); + sdo_config_.push_back(ethercat_interface::SdoConfigEntry(0x6099, 0, "int32", 0)); + + ethercat_interface::pdo_mapping_t pdo_mapping; + pdo_mapping.index = 0x1607; + pdo_mapping.pdo_type = ethercat_interface::RPDO; + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x607a, 0, ethercat_interface::RPDO, "int32", + "position")); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x60ff, 0, ethercat_interface::RPDO, "int32", + "velocity", 255, 0)); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6071, 0, ethercat_interface::RPDO, "int16", + "effort", 255, -5, 2, 10)); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6072, 0, ethercat_interface::RPDO, "int16", "null", + 255, 1000)); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6040, 0, ethercat_interface::RPDO, "uint16", "null", + 255, 0)); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6060, 0, ethercat_interface::RPDO, "int8", "null", + 255, 8)); + pdo_config_.push_back(pdo_mapping); + + pdo_mapping.pdo_channel_config.clear(); + pdo_mapping.index = 0x1a07; + pdo_mapping.pdo_type = ethercat_interface::TPDO; + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6064, 0, ethercat_interface::TPDO, "int32", + "position")); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x606c, 0, ethercat_interface::TPDO, "int32", + "velocity")); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6077, 0, ethercat_interface::TPDO, "int16", "effort", + 255, std::numeric_limits::quiet_NaN(), 5, 15) + ); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x6041, 0, ethercat_interface::TPDO, "uint16", + "null")); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager(0x6061, 0, ethercat_interface::TPDO, "int8", "null")); + pdo_config_.push_back(pdo_mapping); + + pdo_mapping.pdo_channel_config.clear(); + pdo_mapping.index = 0x1a45; + pdo_mapping.pdo_type = ethercat_interface::TPDO; + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x2205, 0x01, ethercat_interface::TPDO, "int16", + "analog_input1")); + pdo_mapping.pdo_channel_config.push_back( + ethercat_interface::EcPdoChannelManager( + 0x2205, 0x02, ethercat_interface::TPDO, "int16", + "analog_input2")); + pdo_config_.push_back(pdo_mapping); + } + ~TestSlave() {} +}; + +#endif // TEST_ETHERLAB_SLAVE_HPP_ diff --git a/ethercat_master/ethercat_master_etherlab/test/test_load_ec_master_etherlab.cpp b/ethercat_master/ethercat_master_etherlab/test/test_load_ec_master_etherlab.cpp new file mode 100644 index 00000000..0dd40e03 --- /dev/null +++ b/ethercat_master/ethercat_master_etherlab/test/test_load_ec_master_etherlab.cpp @@ -0,0 +1,28 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#include +#include + +#include +#include "ethercat_interface/ec_master.hpp" + +TEST(TestLoadEcMasterEtherlab, load_ec_master) +{ + pluginlib::ClassLoader ec_loader_{ + "ethercat_interface", "ethercat_interface::EcMaster"}; + ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_master/EtherlabMaster")); +} diff --git a/ethercat_master/ethercat_master_mock/CMakeLists.txt b/ethercat_master/ethercat_master_mock/CMakeLists.txt new file mode 100644 index 00000000..2261f9c6 --- /dev/null +++ b/ethercat_master/ethercat_master_mock/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.8) +project(ethercat_master_mock) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(ethercat_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) + +file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) +add_library(${PROJECT_NAME} SHARED ${PLUGINS_SRC}) +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies( + ${PROJECT_NAME} + ethercat_interface + pluginlib + rclcpp +) + +pluginlib_export_plugin_description_file(ethercat_interface master_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(pluginlib REQUIRED) + find_package(ethercat_interface REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Test Load EtherCAT modules + ament_add_gmock( + test_load_${PROJECT_NAME} + test/test_load_ec_master_mock.cpp + ) + target_include_directories(test_load_${PROJECT_NAME} PRIVATE include) + ament_target_dependencies(test_load_${PROJECT_NAME} + pluginlib + ethercat_interface + rclcpp + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_master_mock.hpp b/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_master_mock.hpp new file mode 100644 index 00000000..b5c39ac9 --- /dev/null +++ b/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_master_mock.hpp @@ -0,0 +1,69 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#ifndef ETHERCAT_MASTER__EC_MASTER_MOCK_HPP_ +#define ETHERCAT_MASTER__EC_MASTER_MOCK_HPP_ + +#include +#include +#include +#include + +#include "ethercat_interface/ec_master.hpp" +#include "ethercat_interface/ec_slave.hpp" +#include "ethercat_master/ec_slave_mock.hpp" + +namespace ethercat_master +{ + +class MockMaster : public ethercat_interface::EcMaster +{ +public: + MockMaster(); + ~MockMaster(); + + bool init(std::string master_interface = "0"); + + bool add_slave(std::shared_ptr slave); + + bool configure_slaves(); + + bool start(); + + void update(uint32_t domain = 0); + + bool spin_slaves_until_operational(); + + bool stop(); + + void set_ctrl_frequency(double frequency) + { + interval_ = 1000000000.0 / frequency; + } + + uint32_t get_interval() {return interval_;} + + bool read_process_data(); + bool write_process_data(); + +private: + uint32_t interval_; + std::vector> slave_list_; +}; + +} // namespace ethercat_master + +#endif // ETHERCAT_MASTER__EC_MASTER_MOCK_HPP_ diff --git a/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_slave_mock.hpp b/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_slave_mock.hpp new file mode 100644 index 00000000..4d131f9c --- /dev/null +++ b/ethercat_master/ethercat_master_mock/include/ethercat_master/ec_slave_mock.hpp @@ -0,0 +1,61 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#ifndef ETHERCAT_MASTER__EC_SLAVE_MOCK_HPP_ +#define ETHERCAT_MASTER__EC_SLAVE_MOCK_HPP_ + +#include +#include +#include +#include +#include + +#include "ethercat_interface/ec_sdo_manager.hpp" +#include "ethercat_interface/ec_slave.hpp" + +namespace ethercat_master +{ + +class MockSlave +{ +public: + explicit MockSlave(std::shared_ptr slave); + ~MockSlave(); + /** read or write data to the domain */ + int process_data(size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address); + bool initialized(); + void set_state_is_operational(bool value); + /** Assign activate DC synchronization. return activate word*/ + int dc_sync(); + + uint32_t get_vendor_id(); + uint32_t get_product_id(); + int get_bus_position(); + + ethercat_interface::pdo_config_t get_pdo_config(); + ethercat_interface::sm_config_t get_sm_config(); + ethercat_interface::sdo_config_t get_sdo_config(); + + std::vector sdo_config; + +protected: + std::shared_ptr slave_; + int bus_position_; + + bool setup_slave(); +}; +} // namespace ethercat_master +#endif // ETHERCAT_MASTER__EC_SLAVE_MOCK_HPP_ diff --git a/ethercat_master/ethercat_master_mock/master_plugin.xml b/ethercat_master/ethercat_master_mock/master_plugin.xml new file mode 100644 index 00000000..d91c4c5e --- /dev/null +++ b/ethercat_master/ethercat_master_mock/master_plugin.xml @@ -0,0 +1,7 @@ + + + Mock Master interface for testing. + + diff --git a/ethercat_master/ethercat_master_mock/package.xml b/ethercat_master/ethercat_master_mock/package.xml new file mode 100644 index 00000000..107a09a4 --- /dev/null +++ b/ethercat_master/ethercat_master_mock/package.xml @@ -0,0 +1,21 @@ + + + + ethercat_master_mock + 1.2.0 + Plugin implementations of Mock Master interface for testing purposes + Maciej Bednarczyk + Apache-2.0 + + ament_cmake_ros + + ethercat_interface + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ethercat_master/ethercat_master_mock/src/ec_master_mock.cpp b/ethercat_master/ethercat_master_mock/src/ec_master_mock.cpp new file mode 100644 index 00000000..703648fb --- /dev/null +++ b/ethercat_master/ethercat_master_mock/src/ec_master_mock.cpp @@ -0,0 +1,77 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#include "ethercat_master/ec_master_mock.hpp" + +namespace ethercat_master +{ + +MockMaster::MockMaster() +{ + interval_ = 0; +} + +MockMaster::~MockMaster() +{ +} + +bool MockMaster::init(std::string master_interface) +{ + return true; +} + +bool MockMaster::add_slave(std::shared_ptr slave) +{ + // configure slave in master + slave_list_.emplace_back(); + slave_list_.back() = std::make_shared(slave); + return true; +} + +bool MockMaster::configure_slaves() +{ + return true; +} + +bool MockMaster::start() +{ + return true; +} + +bool MockMaster::stop() +{ + return true; +} + +bool MockMaster::spin_slaves_until_operational() +{ + return true; +} + +bool MockMaster::read_process_data() +{ + return true; +} + +bool MockMaster::write_process_data() +{ + return true; +} +} // namespace ethercat_master + +#include + +PLUGINLIB_EXPORT_CLASS(ethercat_master::MockMaster, ethercat_interface::EcMaster) diff --git a/ethercat_master/ethercat_master_mock/src/ec_slave_mock.cpp b/ethercat_master/ethercat_master_mock/src/ec_slave_mock.cpp new file mode 100644 index 00000000..04d19207 --- /dev/null +++ b/ethercat_master/ethercat_master_mock/src/ec_slave_mock.cpp @@ -0,0 +1,62 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#include "ethercat_master/ec_slave_mock.hpp" + +namespace ethercat_master +{ +MockSlave::MockSlave(std::shared_ptr slave) +{ + slave_ = slave; + setup_slave(); +} + +MockSlave::~MockSlave() +{ +} + +int MockSlave::process_data( + size_t pdo_mapping_index, size_t pdo_channel_index, uint8_t * domain_address) +{ + slave_->process_data(pdo_mapping_index, pdo_channel_index, domain_address); + return 0; +} + +bool MockSlave::initialized() +{ + return slave_->initialized(); +} + +void MockSlave::set_state_is_operational(bool value) +{ + slave_->set_state_is_operational(value); +} + +int MockSlave::dc_sync() +{ + return slave_->dc_sync(); +} + +bool MockSlave::setup_slave() +{ + if (slave_->get_slave_parameters().find("position") != slave_->get_slave_parameters().end()) { + bus_position_ = std::stoi(slave_->get_slave_parameters()["position"]); + } else { + bus_position_ = 0; + } + return true; +} +} // namespace ethercat_master diff --git a/ethercat_master/ethercat_master_mock/test/test_load_ec_master_mock.cpp b/ethercat_master/ethercat_master_mock/test/test_load_ec_master_mock.cpp new file mode 100644 index 00000000..e24e447b --- /dev/null +++ b/ethercat_master/ethercat_master_mock/test/test_load_ec_master_mock.cpp @@ -0,0 +1,28 @@ +// 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. +// +// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) + +#include +#include + +#include +#include "ethercat_interface/ec_master.hpp" + +TEST(TestLoadEcMasterMock, load_ec_master) +{ + pluginlib::ClassLoader ec_loader_{ + "ethercat_interface", "ethercat_interface::EcMaster"}; + ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_master/MockMaster")); +}