Effort controllers (ros-controls#102)

* joint effort controller initial class version with tests

* Set effort to zero when deactivating controller

* Renaming to joint_group_effort_controller

effort_controllers/CMakeLists.txt

@@ -0,0 +1,88 @@
+cmake_minimum_required(VERSION 3.5)
+project(effort_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(forward_command_controller REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_library(effort_controllers
+  SHARED
+  src/joint_group_effort_controller.cpp
+)
+target_include_directories(effort_controllers PRIVATE include)
+ament_target_dependencies(effort_controllers
+  forward_command_controller
+  pluginlib
+  rclcpp
+)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS
+  effort_controllers
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(test_robot_hardware REQUIRED)
+
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(
+    test_load_joint_group_effort_controller
+    test/test_load_joint_group_effort_controller.cpp
+  )
+  target_include_directories(test_load_joint_group_effort_controller PRIVATE include)
+  ament_target_dependencies(test_load_joint_group_effort_controller
+    controller_manager
+    test_robot_hardware
+  )
+
+  ament_add_gtest(
+    test_joint_group_effort_controller
+    test/test_joint_group_effort_controller.cpp
+  )
+  target_include_directories(test_joint_group_effort_controller PRIVATE include)
+  target_link_libraries(test_joint_group_effort_controller
+    effort_controllers
+  )
+  ament_target_dependencies(test_joint_group_effort_controller
+    test_robot_hardware
+  )
+endif()
+
+ament_export_dependencies(
+  forward_command_controller
+)
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  effort_controllers
+)
+ament_package()

effort_controllers/effort_controllers_plugins.xml

@@ -0,0 +1,7 @@
+<library path="effort_controllers">
+  <class name="effort_controllers/JointGroupEffortController" type="effort_controllers::JointGroupEffortController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      The joint effort controller commands a group of joints through the effort interface
+    </description>
+  </class>
+</library>

effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp

@@ -0,0 +1,51 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// 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 EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
+#define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
+
+#include "forward_command_controller/forward_command_controller.hpp"
+#include "effort_controllers/visibility_control.h"
+
+namespace effort_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of effort controlled joints (linear or angular).
+ *
+ * This class forwards the commanded efforts down to a set of joints.
+ *
+ * \param joints Names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::msg::Float64MultiArray) : The effort commands to apply.
+ */
+class JointGroupEffortController : public forward_command_controller::ForwardCommandController
+{
+public:
+  EFFORT_CONTROLLERS_PUBLIC
+  JointGroupEffortController();
+
+  EFFORT_CONTROLLERS_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  EFFORT_CONTROLLERS_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+};
+
+}  // namespace effort_controllers
+
+#endif  // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_

effort_controllers/include/effort_controllers/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// 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 EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
+#define EFFORT_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 EFFORT_CONTROLLERS_EXPORT __attribute__ ((dllexport))
+    #define EFFORT_CONTROLLERS_IMPORT __attribute__ ((dllimport))
+  #else
+    #define EFFORT_CONTROLLERS_EXPORT __declspec(dllexport)
+    #define EFFORT_CONTROLLERS_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef EFFORT_CONTROLLERS_BUILDING_DLL
+    #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT
+  #else
+    #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT
+  #endif
+  #define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC
+  #define EFFORT_CONTROLLERS_LOCAL
+#else
+  #define EFFORT_CONTROLLERS_EXPORT __attribute__ ((visibility("default")))
+  #define EFFORT_CONTROLLERS_IMPORT
+  #if __GNUC__ >= 4
+    #define EFFORT_CONTROLLERS_PUBLIC __attribute__ ((visibility("default")))
+    #define EFFORT_CONTROLLERS_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define EFFORT_CONTROLLERS_PUBLIC
+    #define EFFORT_CONTROLLERS_LOCAL
+  #endif
+  #define EFFORT_CONTROLLERS_PUBLIC_TYPE
+#endif
+
+#endif  // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_

effort_controllers/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>effort_controllers</name>
+  <version>0.0.0</version>
+  <description>Generic controller for forwarding commands.</description>
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+  <maintainer email="[email protected]">Jordan Palacios</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>forward_command_controller</depend>
+  <depend>rclcpp</depend>
+
+  <build_depend>pluginlib</build_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>test_robot_hardware</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

effort_controllers/src/joint_group_effort_controller.cpp

@@ -0,0 +1,69 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// 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 "effort_controllers/joint_group_effort_controller.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/parameter.hpp"
+
+namespace
+{
+constexpr auto kJECLoggerName = "joint effort controller";
+}
+
+namespace effort_controllers
+{
+using CallbackReturn = JointGroupEffortController::CallbackReturn;
+
+JointGroupEffortController::JointGroupEffortController()
+: forward_command_controller::ForwardCommandController()
+{
+  logger_name_ = kJECLoggerName;
+}
+
+CallbackReturn JointGroupEffortController::on_configure(
+  const rclcpp_lifecycle::State & previous_state)
+{
+  rclcpp::Parameter interface_param;
+  if (!lifecycle_node_->get_parameter("interface_name", interface_param)) {
+    lifecycle_node_->declare_parameter("interface_name", "effort_command");
+  } else {
+    if (interface_param.as_string() != "effort_command") {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger(
+          kJECLoggerName), "'interface_name' already set with an invalid value");
+      return CallbackReturn::ERROR;
+    }
+  }
+  return ForwardCommandController::on_configure(previous_state);
+}
+
+CallbackReturn JointGroupEffortController::on_deactivate(
+  const rclcpp_lifecycle::State & previous_state)
+{
+  auto ret = ForwardCommandController::on_deactivate(previous_state);
+
+  // stop all joints
+  for (auto & joint_handle : joint_cmd_handles_) {
+    joint_handle.set_value(0.0);
+  }
+
+  return ret;
+}
+
+}  // namespace effort_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  effort_controllers::JointGroupEffortController, controller_interface::ControllerInterface)