Skip to content

Commit

Permalink
🚀 Add PID controller 🎉 (backport #434, #975, #899, #1084, #951) (#1163)
Browse files Browse the repository at this point in the history
  • Loading branch information
mergify[bot] authored Jun 18, 2024
1 parent 36ce104 commit 014848c
Show file tree
Hide file tree
Showing 20 changed files with 2,055 additions and 0 deletions.
1 change: 1 addition & 0 deletions ackermann_steering_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

<build_depend>generate_parameter_library</build_depend>

<depend>backward_ros</depend>
<depend>control_msgs</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
Expand Down
1 change: 1 addition & 0 deletions bicycle_steering_controller/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

<build_depend>generate_parameter_library</build_depend>

<depend>backward_ros</depend>
<depend>control_msgs</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
Expand Down
1 change: 1 addition & 0 deletions doc/controllers_index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,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>

Expand Down
108 changes: 108 additions & 0 deletions pid_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
7 changes: 7 additions & 0 deletions pid_controller/README.md
Original file line number Diff line number Diff line change
@@ -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)
103 changes: 103 additions & 0 deletions pid_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
: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.

List of parameters
=========================
.. generate_parameter_library_details:: ../src/pid_controller.yaml


An example parameter file
=========================


An example parameter file for this controller can be found in `the test folder (standalone) <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/test/pid_controller_params.yaml>`_:

.. literalinclude:: ../test/pid_controller_params.yaml
:language: yaml

or as `preceding controller <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/test/pid_controller_preceding_params.yaml>`_:

.. literalinclude:: ../test/pid_controller_preceding_params.yaml
:language: yaml
140 changes: 140 additions & 0 deletions pid_controller/include/pid_controller/pid_controller.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
// 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/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_
Loading

0 comments on commit 014848c

Please sign in to comment.