🚀 Add PID controller 🎉

doc/controllers_index.rst

@@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use
    Forward Command Controller <../forward_command_controller/doc/userdoc.rst>
    Gripper Controller <../gripper_controllers/doc/userdoc.rst>
    Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst>
+   PID Controller <../pid_controller/doc/userdoc.rst>
    Position Controllers <../position_controllers/doc/userdoc.rst>
    Velocity Controllers <../velocity_controllers/doc/userdoc.rst>
 

pid_controller/CMakeLists.txt

@@ -0,0 +1,108 @@
+cmake_minimum_required(VERSION 3.16)
+project(pid_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  angles
+  control_msgs
+  control_toolbox
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  parameter_traits
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_srvs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(pid_controller_parameters
+  src/pid_controller.yaml
+)
+
+add_library(pid_controller SHARED
+  src/pid_controller.cpp
+)
+target_compile_features(pid_controller PUBLIC cxx_std_17)
+target_include_directories(pid_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/pid_controller>
+)
+target_link_libraries(pid_controller PUBLIC
+  pid_controller_parameters
+)
+ament_target_dependencies(pid_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(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface pid_controller.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_rostest_with_parameters_gmock(
+    test_pid_controller
+    test/test_pid_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml
+  )
+  target_include_directories(test_pid_controller PRIVATE include)
+  target_link_libraries(test_pid_controller pid_controller)
+  ament_target_dependencies(
+    test_pid_controller
+    controller_interface
+    hardware_interface
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_pid_controller_preceding
+    test/test_pid_controller_preceding.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml
+  )
+  target_include_directories(test_pid_controller_preceding PRIVATE include)
+  target_link_libraries(test_pid_controller_preceding pid_controller)
+  ament_target_dependencies(
+    test_pid_controller_preceding
+    controller_interface
+    hardware_interface
+  )
+
+  ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp)
+  target_include_directories(test_load_pid_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_pid_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/pid_controller
+)
+
+install(TARGETS
+    pid_controller
+    pid_controller_parameters
+  EXPORT export_pid_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

pid_controller/README.md

@@ -0,0 +1,7 @@
+pid_controller
+==========================================
+
+Controller based on PID implementation from control_toolbox package.
+
+Pluginlib-Library: pid_controller
+Plugin: pid_controller/PidController (controller_interface::ControllerInterface)

pid_controller/doc/userdoc.rst

@@ -0,0 +1,86 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst
+
+.. _pid_controller_userdoc:
+
+PID Controller
+--------------------------------
+
+PID Controller implementation that uses PidROS implementation from `control_toolbox <https://github.com/ros-controls/control_toolbox/>`_ package.
+The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers.
+It also enables to use the first derivative of the reference and its feedback to have second-order PID control.
+
+Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example:
+
+- reference/state POSITION; command VELOCITY --> PI CONTROLLER
+- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER
+
+- reference/state VELOCITY; command POSITION --> PD CONTROLLER
+- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER
+
+- reference/state POSITION; command POSITION --> PID CONTROLLER
+- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER
+- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER
+- reference/state EFFORT; command EFFORT --> PID CONTROLLER
+
+.. note::
+
+   Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)<joint_trajectory_controller_userdoc>` for the same purpose by sending only one reference point into it.
+   Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that.
+   PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware.
+
+Execution logic of the controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics.
+If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type.
+For example a valid combination would be ``position`` and ``velocity`` interface types.
+
+Using the controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Pluginlib-Library: pid_controller
+Plugin name: pid_controller/PidController
+
+Description of controller's interfaces
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+References (from a preceding controller)
+,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
+- <reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]>  [double]
+  **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``.
+
+Commands
+,,,,,,,,,
+- <dof_names[i]>/<command_interface>  [double]
+
+States
+,,,,,,,
+- <reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]>  [double]
+  **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``.
+
+
+Subscribers
+,,,,,,,,,,,,
+If controller is not in chained mode (``in_chained_mode == false``):
+
+- <controller_name>/reference  [control_msgs/msg/MultiDOFCommand]
+
+If controller parameter ``use_external_measured_states`` is true:
+
+- <controller_name>/measured_state  [control_msgs/msg/MultiDOFCommand]
+
+Services
+,,,,,,,,,,,
+
+- <controller_name>/set_feedforward_control  [std_srvs/srv/SetBool]
+
+Publishers
+,,,,,,,,,,,
+- <controller_name>/controller_state  [control_msgs/msg/MultiDOFStateStamped]
+
+Parameters
+,,,,,,,,,,,
+
+The PID controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters.
+
+.. generate_parameter_library_details:: ../src/pid_controller.yaml

pid_controller/include/pid_controller/pid_controller.hpp

@@ -0,0 +1,141 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+//
+// Authors: Daniel Azanov, Dr. Denis
+//
+
+#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_
+#define PID_CONTROLLER__PID_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "control_msgs/msg/multi_dof_command.hpp"
+#include "control_msgs/msg/multi_dof_state_stamped.hpp"
+#include "control_toolbox/pid_ros.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "pid_controller/visibility_control.h"
+#include "pid_controller_parameters.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 "std_srvs/srv/set_bool.hpp"
+
+#include "control_msgs/msg/joint_controller_state.hpp"
+#include "control_msgs/msg/joint_jog.hpp"
+
+#include "control_msgs/msg/pid_state.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+namespace pid_controller
+{
+
+enum class feedforward_mode_type : std::uint8_t
+{
+  OFF = 0,
+  ON = 1,
+};
+
+class PidController : public controller_interface::ChainableControllerInterface
+{
+public:
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  PidController();
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand;
+  using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand;
+  using ControllerModeSrvType = std_srvs::srv::SetBool;
+  using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped;
+
+protected:
+  std::shared_ptr<pid_controller::ParamListener> param_listener_;
+  pid_controller::Params params_;
+
+  std::vector<std::string> reference_and_state_dof_names_;
+  size_t dof_;
+  std::vector<double> measured_state_values_;
+
+  using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
+  std::vector<PidPtr> pids_;
+  // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+  std::vector<double> feedforward_gain_;
+
+  // Command subscribers and Controller State publisher
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
+
+  rclcpp::Subscription<ControllerMeasuredStateMsg>::SharedPtr measured_state_subscriber_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerMeasuredStateMsg>> measured_state_;
+
+  rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
+  realtime_tools::RealtimeBuffer<feedforward_mode_type> control_mode_;
+
+  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+  std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+  // override methods from ChainableControllerInterface
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  bool on_set_chained_mode(bool chained_mode) override;
+
+  // internal methods
+  void update_parameters();
+  controller_interface::CallbackReturn configure_parameters();
+
+private:
+  // callback for topic interface
+  PID_CONTROLLER__VISIBILITY_LOCAL
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+};
+
+}  // namespace pid_controller
+
+#endif  // PID_CONTROLLER__PID_CONTROLLER_HPP_