diff --git a/.github/workflows/build-coverage.yml b/.github/workflows/build-coverage.yml
new file mode 100644
index 00000000..e80a5253
--- /dev/null
+++ b/.github/workflows/build-coverage.yml
@@ -0,0 +1,42 @@
+name: Build Source
+on:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 3 * * *'
+
+jobs:
+  source:
+    name: source build
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+    steps:
+      - uses: ros-tooling/setup-ros@0.0.26
+      - uses: ros-tooling/action-ros-ci@0.2
+        with:
+          target-ros2-distro: rolling
+          # build all packages listed in the meta package
+          package-name:
+            control_toolbox
+          vcs-repo-file-url: |
+            https://raw.githubusercontent.com/ros-controls/control_toolbox/ros2-master/control_toolbox.rosinstall
+          colcon-defaults: |
+            {
+              "build": {
+                "mixin": ["coverage-gcc"]
+              }
+            }
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
+      - uses: codecov/codecov-action@v1.0.14
+        with:
+          file: ros_ws/lcov/total_coverage.info
+          flags: unittests
+          name: codecov-umbrella
+      - uses: actions/upload-artifact@v1
+        with:
+          name: colcon-logs-${{ matrix.os }}
+          path: ros_ws/log
+
diff --git a/.github/workflows/build-foxy.yml b/.github/workflows/build-foxy.yml
new file mode 100644
index 00000000..ac109b13
--- /dev/null
+++ b/.github/workflows/build-foxy.yml
@@ -0,0 +1,56 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: Build
+on:
+  pull_request:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 6 * * *'
+
+jobs:
+  binary:
+    name: binary build
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: foxy, ROS_REPO: main}
+          - {ROS_DISTRO: foxy, ROS_REPO: testing}
+    env:
+      CCACHE_DIR: ${{ github.workspace }}/.ccache
+      BASEDIR: ${{ github.workspace }}/.work
+      CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+      DOWNSTREAM_WORKSPACE: https://raw.githubusercontent.com/ros-controls/ros2_control/master/ros2_control/ros2_control.repos
+    steps:
+      - uses: actions/checkout@v1
+      # The target directory cache doesn't include the source directory because
+      # that comes from the checkout.  See "prepare target_ws for cache" task below
+      - name: cache target_ws
+        if: ${{ ! matrix.env.CCOV }}
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
+          restore-keys: |
+            target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
+      - name: cache ccache
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ccache-${{ env.CACHE_PREFIX }}
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}
+      - name: prepare target_ws for cache
+        if: ${{ always() && ! matrix.env.CCOV }}
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws
diff --git a/.github/workflows/build-galactic.yml b/.github/workflows/build-galactic.yml
new file mode 100644
index 00000000..de2ce3a1
--- /dev/null
+++ b/.github/workflows/build-galactic.yml
@@ -0,0 +1,57 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: Build galactic
+on:
+  pull_request:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 6 * * *'
+
+jobs:
+  binary:
+    name: binary build
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: galactic, ROS_REPO: main}
+          - {ROS_DISTRO: galactic, ROS_REPO: testing}
+    env:
+      CCACHE_DIR: ${{ github.workspace }}/.ccache
+      BASEDIR: ${{ github.workspace }}/.work
+      CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+      DOWNSTREAM_WORKSPACE: https://raw.githubusercontent.com/ros-controls/ros2_control/master/ros2_control/ros2_control.repos
+    steps:
+      - uses: actions/checkout@v1
+      # The target directory cache doesn't include the source directory because
+      # that comes from the checkout.  See "prepare target_ws for cache" task below
+      - name: cache target_ws
+        if: ${{ ! matrix.env.CCOV }}
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
+          restore-keys: |
+            target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
+      - name: cache ccache
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ccache-${{ env.CACHE_PREFIX }}
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}
+      - name: prepare target_ws for cache
+        if: ${{ always() && ! matrix.env.CCOV }}
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws
+
diff --git a/.github/workflows/build-rolling.yml b/.github/workflows/build-rolling.yml
new file mode 100644
index 00000000..2809c9e9
--- /dev/null
+++ b/.github/workflows/build-rolling.yml
@@ -0,0 +1,57 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: Build rolling
+on:
+  pull_request:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 6 * * *'
+
+jobs:
+  binary:
+    name: binary build
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: rolling, ROS_REPO: main}
+          - {ROS_DISTRO: rolling, ROS_REPO: testing}
+    env:
+      CCACHE_DIR: ${{ github.workspace }}/.ccache
+      BASEDIR: ${{ github.workspace }}/.work
+      CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+      DOWNSTREAM_WORKSPACE: https://raw.githubusercontent.com/ros-controls/ros2_control/master/ros2_control/ros2_control.repos
+    steps:
+      - uses: actions/checkout@v1
+      # The target directory cache doesn't include the source directory because
+      # that comes from the checkout.  See "prepare target_ws for cache" task below
+      - name: cache target_ws
+        if: ${{ ! matrix.env.CCOV }}
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
+          restore-keys: |
+            target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
+      - name: cache ccache
+        uses: pat-s/always-upload-cache@v2.1.3
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ccache-${{ env.CACHE_PREFIX }}
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}
+      - name: prepare target_ws for cache
+        if: ${{ always() && ! matrix.env.CCOV }}
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws
+
diff --git a/.github/workflows/test.yml b/.github/workflows/build-source-foxy.yml
similarity index 55%
rename from .github/workflows/test.yml
rename to .github/workflows/build-source-foxy.yml
index 8c0bc684..a8df78d5 100644
--- a/.github/workflows/test.yml
+++ b/.github/workflows/build-source-foxy.yml
@@ -1,21 +1,21 @@
-name: Test control_toolbox
+name: Build Source foxy
 on:
-  pull_request:
   push:
     branches:
-      - ros2-master
+      - foxy-devel
   schedule:
-    # Run every morning to detect flakiness and broken dependencies
-    - cron: '17 8 * * *'
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 3 * * *'
 
 jobs:
-  build_and_test:
-    runs-on: ubuntu-18.04
+  source:
+    name: source build
+    runs-on: ubuntu-20.04
     strategy:
       fail-fast: false
     steps:
-      - uses: ros-tooling/setup-ros@v0.1
-      - uses: ros-tooling/action-ros-ci@v0.1
+      - uses: ros-tooling/setup-ros@0.0.26
+      - uses: ros-tooling/action-ros-ci@0.2
         with:
           vcs-repo-file-url: |
             https://raw.githubusercontent.com/ros-controls/control_toolbox/ros2-master/control_toolbox.rosinstall
@@ -24,14 +24,6 @@ jobs:
           colcon-mixin-name: coverage-gcc
           colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
           target-ros2-distro: foxy
-      - uses: codecov/codecov-action@v1
-        with:
-          token: ${{ secrets.CODECOV_TOKEN }}
-          file: ros_ws/lcov/total_coverage.info
-          flags: unittests
-          name: codecov-umbrella
-          yml: ./codecov.yml
-          fail_ci_if_error: true
       - uses: actions/upload-artifact@v1
         with:
           name: colcon-logs-${{ matrix.os }}
diff --git a/.github/workflows/build-source-galactic.yml b/.github/workflows/build-source-galactic.yml
new file mode 100644
index 00000000..bab748b0
--- /dev/null
+++ b/.github/workflows/build-source-galactic.yml
@@ -0,0 +1,31 @@
+name: Build Source galactic
+on:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 3 * * *'
+
+jobs:
+  source:
+    name: source build
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+    steps:
+      - uses: ros-tooling/setup-ros@0.0.26
+      - uses: ros-tooling/action-ros-ci@0.2
+        with:
+          vcs-repo-file-url: |
+            https://raw.githubusercontent.com/ros-controls/control_toolbox/ros2-master/control_toolbox.rosinstall
+            https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos
+          package-name: control_toolbox
+          colcon-mixin-name: coverage-gcc
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
+          target-ros2-distro: galactic
+      - uses: actions/upload-artifact@v1
+        with:
+          name: colcon-logs-${{ matrix.os }}
+          path: ros_ws/log
+
diff --git a/.github/workflows/build-source-rollling.yml b/.github/workflows/build-source-rollling.yml
new file mode 100644
index 00000000..a9932b8c
--- /dev/null
+++ b/.github/workflows/build-source-rollling.yml
@@ -0,0 +1,30 @@
+name: Build Source rolling
+on:
+  push:
+    branches:
+      - ros2-master
+  schedule:
+    # Run every day to detect flakiness and broken dependencies
+    - cron: '28 3 * * *'
+
+jobs:
+  source:
+    name: source build
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+    steps:
+      - uses: ros-tooling/setup-ros@0.2
+      - uses: ros-tooling/action-ros-ci@0.2
+        with:
+          vcs-repo-file-url: |
+            https://raw.githubusercontent.com/ros-controls/control_toolbox/ros2-master/control_toolbox.rosinstall
+            https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos
+          package-name: control_toolbox
+          colcon-mixin-name: coverage-gcc
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
+          target-ros2-distro: rolling
+      - uses: actions/upload-artifact@v1
+        with:
+          name: colcon-logs-${{ matrix.os }}
+          path: ros_ws/log
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
new file mode 100644
index 00000000..5ca42dd5
--- /dev/null
+++ b/.github/workflows/format.yml
@@ -0,0 +1,25 @@
+# This is a format job. Pre-commit has a first-party GitHub action, so we use
+# that: https://github.com/pre-commit/action
+
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - ros2-master
+
+jobs:
+  pre-commit:
+    name: Format
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v2
+    - uses: actions/setup-python@v2
+    - name: Install clang-format-10
+      run: sudo apt-get install clang-format-10 cppcheck
+    - uses: pre-commit/action@v2.0.3
+      with:
+        extra_args: --all-files --hook-stage manual
+        token: ${{ secrets.GITHUB_TOKEN }}
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
deleted file mode 100644
index e6b75c89..00000000
--- a/.github/workflows/lint.yml
+++ /dev/null
@@ -1,30 +0,0 @@
-name: Lint control_toolbox
-on:
-  pull_request:
-
-jobs:
-  ament_xmllint:
-    name: ament_xmllint
-    runs-on: ubuntu-18.04
-    steps:
-    - uses: actions/checkout@v1
-    - uses: ros-tooling/setup-ros@v0.1
-    - uses: ros-tooling/action-ros-lint@v0.1
-      with:
-        linter: xmllint
-        package-name: control_toolbox
-
-  ament_lint_cpp:
-    name: ament_${{ matrix.linter }}
-    runs-on: ubuntu-18.04
-    strategy:
-      fail-fast: false
-      matrix:
-          linter: [cppcheck, cpplint, uncrustify]
-    steps:
-    - uses: actions/checkout@v1
-    - uses: ros-tooling/setup-ros@v0.1
-    - uses: ros-tooling/action-ros-lint@v0.1
-      with:
-        linter: ${{ matrix.linter }}
-        package-name: control_toolbox
diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml
new file mode 100644
index 00000000..8b860855
--- /dev/null
+++ b/.github/workflows/prerelease.yaml
@@ -0,0 +1,26 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+name: pre-release
+
+on:
+  workflow_dispatch:
+
+jobs:
+  default:
+    strategy:
+      fail-fast: false
+      matrix:
+        distro: [foxy, galactic, rolling]
+
+    env:
+      ROS_DISTRO: ${{ matrix.distro }}
+      PRERELEASE: true
+      BASEDIR: ${{ github.workspace }}/.work
+
+    name: "${{ matrix.distro }}"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: industrial_ci
+        uses: ros-industrial/industrial_ci@master
diff --git a/.github/workflows/ros-lint.yml b/.github/workflows/ros-lint.yml
new file mode 100644
index 00000000..2f6f63e9
--- /dev/null
+++ b/.github/workflows/ros-lint.yml
@@ -0,0 +1,39 @@
+name: ROS2 Lint
+on:
+  pull_request:
+
+jobs:
+  ament_lint:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [cppcheck, copyright, lint_cmake]
+    steps:
+    - uses: actions/checkout@v1
+    - uses: ros-tooling/setup-ros@v0.2
+    - uses: ros-tooling/action-ros-lint@v0.1
+      with:
+        distribution: rolling
+        linter: ${{ matrix.linter }}
+        package-name:
+          control_toolbox
+
+  ament_lint_100:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [cpplint]
+    steps:
+    - uses: actions/checkout@v1
+    - uses: ros-tooling/setup-ros@v0.2
+    - uses: ros-tooling/action-ros-lint@v0.1
+      with:
+        distribution: rolling
+        linter: cpplint
+        arguments: "--linelength=100 --filter=-whitespace/newline"
+        package-name:
+          control_toolbox
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 00000000..d0a2af89
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,144 @@
+
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+      - id: fix-byte-order-marker
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v2.12.0
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  - repo: https://github.com/psf/black
+    rev: 20.8b1
+    hooks:
+      - id: black
+        args: ["--line-length=99"]
+
+  # PEP 257
+  - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257
+    rev: v0.3.3
+    hooks:
+    - id: pep257
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 3.9.0
+    hooks:
+    - id: flake8
+      args: ["--ignore=E501"]
+
+  # CPP hooks
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-10
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']
+  # The same options as in ament_cppcheck are used, but its not working...
+  #- repo: https://github.com/pocc/pre-commit-hooks
+    #rev: v1.1.1
+    #hooks:
+      #- id: cppcheck
+        #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose']
+
+  - repo: local
+    hooks:
+      - id: ament_cppcheck
+        name: ament_cppcheck
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cppcheck
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  # Maybe use https://github.com/cpplint/cpplint instead
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        stages: [commit]
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        stages: [commit]
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: 0.9.0a1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.8.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.0.0
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: \.(svg|pyc)$