Cartesian twist controller

cartesian_controllers/CMakeLists.txt

@@ -0,0 +1,103 @@
+cmake_minimum_required(VERSION 3.8)
+project(cartesian_controllers)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+
+add_library(twist_controller src/twist_controller.cpp)
+target_include_directories(twist_controller PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  twist_controller
+  "controller_interface"
+  "hardware_interface"
+  "rclcpp"
+  "rclcpp_lifecycle"
+  "realtime_tools"
+  "geometry_msgs"
+  "std_msgs"
+  "pluginlib"
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(twist_controller PRIVATE "CARTESIAN_CONTROLLERS_BUILDING_LIBRARY")
+pluginlib_export_plugin_description_file(controller_interface twist_controller_plugin.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  TARGETS twist_controller
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_load_twist_controller test/test_load_twist_controller.cpp)
+  target_include_directories(test_load_twist_controller PRIVATE include)
+  ament_target_dependencies(
+          test_load_twist_controller
+          controller_manager
+          hardware_interface
+          ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_twist_controller test/test_twist_controller.cpp)
+  target_include_directories(test_twist_controller PRIVATE include)
+  target_link_libraries(test_twist_controller twist_controller)
+  ament_target_dependencies(
+          test_twist_controller
+          controller_interface
+          hardware_interface
+  )
+endif()
+
+ament_export_dependencies(
+  controller_interface
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_msgs
+  geometry_msgs
+)
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  twist_controller
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

cartesian_controllers/include/cartesian_controllers/twist_controller.hpp

@@ -0,0 +1,72 @@
+// Copyright 2021, PickNik Inc.
+//
+// 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 CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_
+#define CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "cartesian_controllers/visibility_control.h"
+#include "realtime_tools/realtime_buffer.h"
+
+namespace cartesian_controllers
+{
+using CmdType = geometry_msgs::msg::TwistStamped;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class TwistController : public controller_interface::ControllerInterface
+{
+public:
+  CARTESIAN_CONTROLLERS_PUBLIC
+  TwistController();
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  CallbackReturn on_init() override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_CONTROLLERS_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::string joint_name_;
+  std::vector<std::string> interface_names_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
+  rclcpp::Subscription<CmdType>::SharedPtr twist_command_subscriber_;
+
+  std::string logger_name_;
+};
+
+}  // namespace cartesian_controllers
+
+#endif  // CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_

cartesian_controllers/include/cartesian_controllers/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright 2021, PickNik Inc.
+//
+// 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 CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_
+#define CARTESIAN_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 CARTESIAN_CONTROLLERS_EXPORT __attribute__((dllexport))
+#define CARTESIAN_CONTROLLERS_IMPORT __attribute__((dllimport))
+#else
+#define CARTESIAN_CONTROLLERS_EXPORT __declspec(dllexport)
+#define CARTESIAN_CONTROLLERS_IMPORT __declspec(dllimport)
+#endif
+#ifdef CARTESIAN_CONTROLLERS_BUILDING_LIBRARY
+#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_EXPORT
+#else
+#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_IMPORT
+#endif
+#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE CARTESIAN_CONTROLLERS_PUBLIC
+#define CARTESIAN_CONTROLLERS_LOCAL
+#else
+#define CARTESIAN_CONTROLLERS_EXPORT __attribute__((visibility("default")))
+#define CARTESIAN_CONTROLLERS_IMPORT
+#if __GNUC__ >= 4
+#define CARTESIAN_CONTROLLERS_PUBLIC __attribute__((visibility("default")))
+#define CARTESIAN_CONTROLLERS_LOCAL __attribute__((visibility("hidden")))
+#else
+#define CARTESIAN_CONTROLLERS_PUBLIC
+#define CARTESIAN_CONTROLLERS_LOCAL
+#endif
+#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE
+#endif
+
+#endif  // CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_

cartesian_controllers/package.xml

@@ -0,0 +1,28 @@
+<?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>cartesian_controllers</name>
+  <version>0.0.0</version>
+  <description>Cartesain controllers for ROS2.</description>
+  <maintainer email="lovro.ivanov@gmail.com">Lovro Ivanov</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>