diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/CMakeLists.txt b/ethercat_controllers/ethercat_generic_cia402_controller/CMakeLists.txt new file mode 100644 index 00000000..8c23f592 --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.8) +project(ethercat_generic_cia402_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + pluginlib + rclcpp + realtime_tools + ethercat_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(ethercat_generic_cia402_controller SHARED + src/generic_cia402_controller.cpp +) +target_compile_features(ethercat_generic_cia402_controller PUBLIC c_std_99 cxx_std_17) +target_include_directories(ethercat_generic_cia402_controller PUBLIC + $ + $ +) + +ament_target_dependencies( ethercat_generic_cia402_controller PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ethercat_generic_cia402_controller + PRIVATE + "ETHERCAT_GENERIC_CIA402_CONTROLLER_BUILDING_DLL" +) +pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ethercat_generic_cia402_controller + 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(controller_manager REQUIRED) + find_package(controller_interface REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Test Load controller + ament_add_gmock( + test_load_cia402_controller + test/test_load_cia402_controller.cpp + ) + + target_include_directories(test_load_cia402_controller PRIVATE include) + + ament_target_dependencies(test_load_cia402_controller + pluginlib + controller_manager + controller_interface + hardware_interface + ros2_control_test_assets + ) + + # ament_add_gmock( + # test_cia402_controller + # test/test_cia402_controller.cpp + # ) + + # target_include_directories( + # test_cia402_controller + # PRIVATE + # include + # ) + + # target_link_libraries( + # test_cia402_controller + # ethercat_generic_cia402_controller + # ) + + # ament_target_dependencies( + # test_cia402_controller + # controller_interface + # hardware_interface + # rclcpp + # rclcpp_lifecycle + # realtime_tools + # ethercat_msgs + # ) +endif() + +ament_export_include_directories(include) +ament_export_libraries(ethercat_generic_cia402_controller) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/controller_plugin.xml b/ethercat_controllers/ethercat_generic_cia402_controller/controller_plugin.xml new file mode 100644 index 00000000..077262ad --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/controller_plugin.xml @@ -0,0 +1,7 @@ + + + The CiA402 controller controls EtherCAT CiA402 Drives using the ethercat_generic_plugins/EcCiA402Drive plugin. + + diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/generic_cia402_controller.hpp b/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/generic_cia402_controller.hpp new file mode 100644 index 00000000..408b4586 --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/generic_cia402_controller.hpp @@ -0,0 +1,104 @@ +// 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 ETHERCAT_CONTROLLERS__GENERIC_CIA402_CONTROLLER_HPP_ +#define ETHERCAT_CONTROLLERS__GENERIC_CIA402_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "ethercat_controllers/visibility_control.h" +#include "rclcpp/subscription.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "ethercat_msgs/msg/cia402_drive_states.hpp" +#include "ethercat_msgs/srv/switch_drive_mode_of_operation.hpp" +#include "ethercat_msgs/srv/reset_drive_fault.hpp" + +namespace ethercat_controllers +{ +using DriveStateMsgType = ethercat_msgs::msg::Cia402DriveStates; +using SwitchMOOSrv = ethercat_msgs::srv::SwitchDriveModeOfOperation; +using ResetFaultSrv = ethercat_msgs::srv::ResetDriveFault; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class CiA402Controller : public controller_interface::ControllerInterface +{ +public: + CIA402_CONTROLLER_PUBLIC + CiA402Controller(); + + CIA402_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + CIA402_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + CIA402_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CIA402_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CIA402_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CIA402_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + CIA402_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + +protected: + std::vector dof_names_; + std::vector mode_ops_; + std::vector control_words_; + std::vector reset_faults_; + + + using DriveStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr drive_state_publisher_; + std::unique_ptr rt_drive_state_publisher_; + + realtime_tools::RealtimeBuffer> rt_moo_srv_ptr_; + rclcpp::Service::SharedPtr moo_srv_ptr_; + + realtime_tools::RealtimeBuffer> rt_reset_fault_srv_ptr_; + rclcpp::Service::SharedPtr reset_fault_srv_ptr_; + + std::string logger_name_; + + std::string device_state_str(uint16_t status_word); + std::string mode_of_operation_str(double mode_of_operation); + + void switch_moo_callback( + const std::shared_ptr request, + std::shared_ptr response + ); + + void reset_fault_callback( + const std::shared_ptr request, + std::shared_ptr response + ); +}; + +} // namespace ethercat_controllers + +#endif // ETHERCAT_CONTROLLERS__GENERIC_CIA402_CONTROLLER_HPP_ diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/visibility_control.h b/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/visibility_control.h new file mode 100644 index 00000000..b41c6040 --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/include/ethercat_controllers/visibility_control.h @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ETHERCAT_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define ETHERCAT_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define CIA402_CONTROLLER_EXPORT __attribute__((dllexport)) +#define CIA402_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define CIA402_CONTROLLER_EXPORT __declspec(dllexport) +#define CIA402_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef CIA402_CONTROLLER_BUILDING_DLL +#define CIA402_CONTROLLER_PUBLIC CIA402_CONTROLLER_EXPORT +#else +#define CIA402_CONTROLLER_PUBLIC CIA402_CONTROLLER_IMPORT +#endif +#define CIA402_CONTROLLER_PUBLIC_TYPE CIA402_CONTROLLER_PUBLIC +#define CIA402_CONTROLLER_LOCAL +#else +#define CIA402_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define CIA402_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define CIA402_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define CIA402_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define CIA402_CONTROLLER_PUBLIC +#define CIA402_CONTROLLER_LOCAL +#endif +#define CIA402_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ETHERCAT_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/package.xml b/ethercat_controllers/ethercat_generic_cia402_controller/package.xml new file mode 100644 index 00000000..f08e8964 --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/package.xml @@ -0,0 +1,29 @@ + + + + ethercat_generic_cia402_controller + 1.1.0 + EtherCAT CiA402 drive controllers for EcCiA402Drive. + Maciej Bednarczyk + + Apache License 2.0 + + ament_cmake + + controller_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ethercat_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp b/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp new file mode 100644 index 00000000..c720998b --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp @@ -0,0 +1,259 @@ +// 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 +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ethercat_controllers/generic_cia402_controller.hpp" + +namespace ethercat_controllers +{ +using hardware_interface::LoanedCommandInterface; + +CiA402Controller::CiA402Controller() +: controller_interface::ControllerInterface(), + rt_drive_state_publisher_(nullptr) +{ +} + +CallbackReturn CiA402Controller::on_init() +{ + try { + // definition of the parameters that need to be queried from the + // controller configuration file with default values + auto_declare>("dofs", std::vector()); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CiA402Controller::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // getting the names of the dofs to be controlled + dof_names_ = get_node()->get_parameter("dofs").as_string_array(); + + if (dof_names_.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'dofs' parameter was empty"); + return CallbackReturn::FAILURE; + } + + mode_ops_.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + control_words_.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + reset_faults_.resize(dof_names_.size(), false); + + try { + // register data publisher + drive_state_publisher_ = get_node()->create_publisher( + "~/drive_states", rclcpp::SystemDefaultsQoS()); + rt_drive_state_publisher_ = std::make_unique(drive_state_publisher_); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Exception thrown during publisher creation at configure stage" + "with message : %s \n", + e.what() + ); + return CallbackReturn::ERROR; + } + + using namespace std::placeholders; + moo_srv_ptr_ = get_node()->create_service( + "~/switch_mode_of_operation", std::bind(&CiA402Controller::switch_moo_callback, this, _1, _2)); + reset_fault_srv_ptr_ = get_node()->create_service( + "~/reset_fault", std::bind(&CiA402Controller::reset_fault_callback, this, _1, _2)); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +CiA402Controller::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_names_.size() * 2); + for (const auto & dof_name : dof_names_) { + conf.names.push_back(dof_name + "/" + "control_word"); + conf.names.push_back(dof_name + "/" + "mode_of_operation"); + conf.names.push_back(dof_name + "/" + "reset_fault"); + } + return conf; +} + +controller_interface::InterfaceConfiguration +CiA402Controller::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_names_.size() * 2); + for (const auto & dof_name : dof_names_) { + conf.names.push_back(dof_name + "/" + "mode_of_operation"); + conf.names.push_back(dof_name + "/" + "status_word"); + } + return conf; +} + +CallbackReturn CiA402Controller::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +CallbackReturn CiA402Controller::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type CiA402Controller::update( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (rt_drive_state_publisher_ && rt_drive_state_publisher_->trylock()) { + auto & msg = rt_drive_state_publisher_->msg_; + msg.header.stamp = get_node()->now(); + msg.dof_names.resize(dof_names_.size()); + msg.modes_of_operation.resize(dof_names_.size()); + msg.status_words.resize(dof_names_.size()); + msg.drive_states.resize(dof_names_.size()); + + for (auto i = 0ul; i < dof_names_.size(); i++) { + msg.dof_names[i] = dof_names_[i]; + msg.modes_of_operation[i] = mode_of_operation_str(state_interfaces_[2 * i].get_value()); + msg.status_words[i] = state_interfaces_[2 * i + 1].get_value(); + msg.drive_states[i] = device_state_str(state_interfaces_[2 * i + 1].get_value()); + } + + rt_drive_state_publisher_->unlockAndPublish(); + } + + // getting the data from services using the rt pipe + auto moo_request = rt_moo_srv_ptr_.readFromRT(); + auto reset_fault_request = rt_reset_fault_srv_ptr_.readFromRT(); + + for (auto i = 0ul; i < dof_names_.size(); i++) { + if (!moo_request || !(*moo_request)) { + mode_ops_[i] = state_interfaces_[2 * i].get_value(); + } else { + if (dof_names_[i] == (*moo_request)->dof_name) { + mode_ops_[i] = (*moo_request)->mode_of_operation; + } + } + + if (reset_fault_request && (*reset_fault_request)) { + if (dof_names_[i] == (*reset_fault_request)->dof_name) { + reset_faults_[i] = true; + rt_reset_fault_srv_ptr_.reset(); + } + } + + command_interfaces_[2 * i + 1].set_value(mode_ops_[i]); // mode_of_operation + command_interfaces_[2 * i + 2].set_value(reset_faults_[i]); // reset_fault + reset_faults_[i] = false; + } + + return controller_interface::return_type::OK; +} + +/** returns device state based upon the status_word */ +std::string CiA402Controller::device_state_str(uint16_t status_word) +{ + if ((status_word & 0b01001111) == 0b00000000) { + return "STATE_NOT_READY_TO_SWITCH_ON"; + } else if ((status_word & 0b01001111) == 0b01000000) { + return "STATE_SWITCH_ON_DISABLED"; + } else if ((status_word & 0b01101111) == 0b00100001) { + return "STATE_READY_TO_SWITCH_ON"; + } else if ((status_word & 0b01101111) == 0b00100011) { + return "STATE_SWITCH_ON"; + } else if ((status_word & 0b01101111) == 0b00100111) { + return "STATE_OPERATION_ENABLED"; + } else if ((status_word & 0b01101111) == 0b00000111) { + return "STATE_QUICK_STOP_ACTIVE"; + } else if ((status_word & 0b01001111) == 0b00001111) { + return "STATE_FAULT_REACTION_ACTIVE"; + } else if ((status_word & 0b01001111) == 0b00001000) { + return "STATE_FAULT"; + } + return "STATE_UNDEFINED"; +} + +/** returns mode str based upon the mode_of_operation value */ +std::string CiA402Controller::mode_of_operation_str(double mode_of_operation) +{ + if (mode_of_operation == 0) { + return "MODE_NO_MODE"; + } else if (mode_of_operation == 1) { + return "MODE_PROFILED_POSITION"; + } else if (mode_of_operation == 3) { + return "MODE_PROFILED_VELOCITY"; + } else if (mode_of_operation == 4) { + return "MODE_PROFILED_TORQUE"; + } else if (mode_of_operation == 6) { + return "MODE_HOMING"; + } else if (mode_of_operation == 7) { + return "MODE_INTERPOLATED_POSITION"; + } else if (mode_of_operation == 8) { + return "MODE_CYCLIC_SYNC_POSITION"; + } else if (mode_of_operation == 9) { + return "MODE_CYCLIC_SYNC_VELOCITY"; + } else if (mode_of_operation == 10) { + return "MODE_CYCLIC_SYNC_TORQUE"; + } + return "MODE_UNDEFINED"; +} + +void CiA402Controller::switch_moo_callback( + const std::shared_ptr request, + std::shared_ptr response +) +{ + if (find(dof_names_.begin(), dof_names_.end(), request->dof_name) != dof_names_.end()) { + rt_moo_srv_ptr_.writeFromNonRT(request); + response->return_message = "Request transmitted to drive at dof:" + request->dof_name; + } else { + response->return_message = "Abort. DoF " + request->dof_name + " not configured."; + } +} + +void CiA402Controller::reset_fault_callback( + const std::shared_ptr request, + std::shared_ptr response +) +{ + if (find(dof_names_.begin(), dof_names_.end(), request->dof_name) != dof_names_.end()) { + rt_reset_fault_srv_ptr_.writeFromNonRT(request); + response->return_message = "Request transmitted to drive at dof:" + request->dof_name; + } else { + response->return_message = "Abort. DoF " + request->dof_name + " not configured."; + } +} + +} // namespace ethercat_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ethercat_controllers::CiA402Controller, controller_interface::ControllerInterface) diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/test/test_load_cia402_controller.cpp b/ethercat_controllers/ethercat_generic_cia402_controller/test/test_load_cia402_controller.cpp new file mode 100644 index 00000000..111cbcdb --- /dev/null +++ b/ethercat_controllers/ethercat_generic_cia402_controller/test/test_load_cia402_controller.cpp @@ -0,0 +1,41 @@ +// 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 "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadCiA402Controller, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller( + "test_cia402_controller", + "ethercat_controllers/CiA402Controller")); + + rclcpp::shutdown(); +} diff --git a/ethercat_msgs/CMakeLists.txt b/ethercat_msgs/CMakeLists.txt index e905d9a3..2668f329 100644 --- a/ethercat_msgs/CMakeLists.txt +++ b/ethercat_msgs/CMakeLists.txt @@ -7,11 +7,16 @@ endif() find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Cia402DriveStates.msg" "srv/SetSdo.srv" "srv/GetSdo.srv" + "srv/SwitchDriveModeOfOperation.srv" + "srv/ResetDriveFault.srv" DEPENDENCIES + std_msgs ) if(BUILD_TESTING) diff --git a/ethercat_msgs/msg/Cia402DriveStates.msg b/ethercat_msgs/msg/Cia402DriveStates.msg new file mode 100644 index 00000000..c4b3456b --- /dev/null +++ b/ethercat_msgs/msg/Cia402DriveStates.msg @@ -0,0 +1,38 @@ +# This message presents the current state of a CiA402 drive on multiple DoFs. + +# Drive States +uint8 STATE_UNDEFINED=0 +uint8 STATE_START=1 +uint8 STATE_NOT_READY_TO_SWITCH_ON=2 +uint8 STATE_SWITCH_ON_DISABLED=3 +uint8 STATE_READY_TO_SWITCH_ON=4 +uint8 STATE_SWITCH_ON=5 +uint8 STATE_OPERATION_ENABLED=6 +uint8 STATE_QUICK_STOP_ACTIVE=7 +uint8 STATE_FAULT_REACTION_ACTIVE=8 +uint8 STATE_FAULT=9 + +# Modes of Operation +uint8 MODE_NO_MODE=0 +uint8 MODE_PROFILED_POSITION=1 +uint8 MODE_PROFILED_VELOCITY=3 +uint8 MODE_PROFILED_TORQUE=4 +uint8 MODE_HOMING=6 +uint8 MODE_INTERPOLATED_POSITION=7 +uint8 MODE_CYCLIC_SYNC_POSITION=8 +uint8 MODE_CYCLIC_SYNC_VELOCITY=9 +uint8 MODE_CYCLIC_SYNC_TORQUE=10 + +std_msgs/Header header + +# DoF name +string[] dof_names + +# Current state of the drive +string[] drive_states + +# Current mode of operation +string[] modes_of_operation + +# Current Status Word +int16[] status_words diff --git a/ethercat_msgs/package.xml b/ethercat_msgs/package.xml index 8109f96f..8636d792 100644 --- a/ethercat_msgs/package.xml +++ b/ethercat_msgs/package.xml @@ -7,6 +7,7 @@ mcbed Apache-2.0 + std_msgs ament_cmake rosidl_default_generators rosidl_default_runtime diff --git a/ethercat_msgs/srv/ResetDriveFault.srv b/ethercat_msgs/srv/ResetDriveFault.srv new file mode 100644 index 00000000..b9cd7b04 --- /dev/null +++ b/ethercat_msgs/srv/ResetDriveFault.srv @@ -0,0 +1,7 @@ +# This service triggers the fault reset for a CiA402 drive. + +# DoF name +string dof_name +--- +# Return message +string return_message diff --git a/ethercat_msgs/srv/SwitchDriveModeOfOperation.srv b/ethercat_msgs/srv/SwitchDriveModeOfOperation.srv new file mode 100644 index 00000000..48816284 --- /dev/null +++ b/ethercat_msgs/srv/SwitchDriveModeOfOperation.srv @@ -0,0 +1,21 @@ +# This service handles the setting up of the mode of operation for a CiA402 drive. + +# Modes of Operation +uint8 MODE_NO_MODE=0 +uint8 MODE_PROFILED_POSITION=1 +uint8 MODE_PROFILED_VELOCITY=3 +uint8 MODE_PROFILED_TORQUE=4 +uint8 MODE_HOMING=6 +uint8 MODE_INTERPOLATED_POSITION=7 +uint8 MODE_CYCLIC_SYNC_POSITION=8 +uint8 MODE_CYCLIC_SYNC_VELOCITY=9 +uint8 MODE_CYCLIC_SYNC_TORQUE=10 + +# DoF name +string dof_name + +# Mode of Operation +int32 mode_of_operation +--- +# Return message +string return_message