Controller for cia402 drives

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+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()

ethercat_controllers/ethercat_generic_cia402_controller/controller_plugin.xml

@@ -0,0 +1,7 @@
+<library path="ethercat_generic_cia402_controller">
+  <class  name="ethercat_controllers/CiA402Controller"
+          type="ethercat_controllers::CiA402Controller"
+          base_class_type="controller_interface::ControllerInterface">
+    <description> The CiA402 controller controls EtherCAT CiA402 Drives using the ethercat_generic_plugins/EcCiA402Drive plugin.</description>
+  </class>
+</library>

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 <memory>
+#include <string>
+#include <vector>
+
+#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<std::string> dof_names_;
+  std::vector<int> mode_ops_;
+  std::vector<double> control_words_;
+  std::vector<bool> reset_faults_;
+
+
+  using DriveStatePublisher = realtime_tools::RealtimePublisher<DriveStateMsgType>;
+  rclcpp::Publisher<DriveStateMsgType>::SharedPtr drive_state_publisher_;
+  std::unique_ptr<DriveStatePublisher> rt_drive_state_publisher_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<SwitchMOOSrv::Request>> rt_moo_srv_ptr_;
+  rclcpp::Service<SwitchMOOSrv>::SharedPtr moo_srv_ptr_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ResetFaultSrv::Request>> rt_reset_fault_srv_ptr_;
+  rclcpp::Service<ResetFaultSrv>::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<SwitchMOOSrv::Request> request,
+    std::shared_ptr<SwitchMOOSrv::Response> response
+  );
+
+  void reset_fault_callback(
+    const std::shared_ptr<ResetFaultSrv::Request> request,
+    std::shared_ptr<ResetFaultSrv::Response> response
+  );
+};
+
+}  // namespace ethercat_controllers
+
+#endif  // ETHERCAT_CONTROLLERS__GENERIC_CIA402_CONTROLLER_HPP_

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_

ethercat_controllers/ethercat_generic_cia402_controller/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ethercat_generic_cia402_controller</name>
+  <version>1.1.0</version>
+  <description>EtherCAT CiA402 drive controllers for EcCiA402Drive.</description>
+  <maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>ethercat_msgs</depend>
+
+  <build_depend>pluginlib</build_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>