diff --git a/.ci/build.sh b/.ci/build.sh
deleted file mode 100755
index f0f9fb9bd..000000000
--- a/.ci/build.sh
+++ /dev/null
@@ -1,21 +0,0 @@
-#!/bin/sh
-
-set -e
-set -x
-
-rm -f src/CMakeLists.txt
-catkin_init_workspace src
-
-rm -rf build-debug
-mkdir build-debug
-cd build-debug
-
-cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ../src
-. devel/setup.sh
-
-cmake --build . -- -j$(nproc)
-cmake --build . --target tests -- -j$(nproc)
-
-cmake --build . --target check-format
-cmake --build . --target check-pyformat
-cmake --build . --target check-tidy -- -j$(nproc)
diff --git a/.ci/test.sh b/.ci/test.sh
deleted file mode 100755
index 9c19817ad..000000000
--- a/.ci/test.sh
+++ /dev/null
@@ -1,21 +0,0 @@
-#!/bin/sh
-
-set -e
-set -x
-
-# check if build-debug folder exists and change directory
-if [ ! -d build-debug ]; then
-    2&> echo "build-debug folder does not exists"
-    exit 1
-fi
-cd build-debug
-
-# check if devel folder exists and source setup.sh
-if [ ! -d devel ]; then
-    2&> echo "devel folder does not exists"
-    exit 1
-fi
-. devel/setup.sh
-
-# testing
-ctest -V
diff --git a/Jenkinsfile b/Jenkinsfile
index 341647dd8..8d4e1a3e7 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -5,93 +5,134 @@ pipeline {
     }
     options {
         checkoutToSubdirectory('src/franka_ros')
+        parallelsAlwaysFailFast()
+    }
+    environment {
+        CMAKE_BUILD_PARALLEL_LEVEL=sh(script: 'nproc', returnStdout: true).trim().toInteger()
     }
     stages {
-        stage('Notify Stash') {
-            steps {
-                script {
-                    notifyBitbucket()
-                }
-            }
-        }
         stage('Build & Test') {
-            parallel {
-                stage('Melodic') {
-                    agent {
-                        dockerfile {
-                            filename '.ci/Dockerfile.melodic'
-                            dir 'src/franka_ros'
-                        }
+            matrix {
+                agent {
+                    dockerfile {
+                        filename ".ci/Dockerfile.${env.DISTRO}"
+                        dir 'src/franka_ros'
+                    }
+                }
+                axes {
+                    axis {
+                        name 'DISTRO'
+                        values 'melodic', 'noetic'
+                    }
+                    axis {
+                        name 'BUILD_TOOL'
+                        values 'catkin_make', 'catkin build'
                     }
-                    stages {
-                        stage('Build') {
-                            steps {
-                                sh ''' . /opt/ros/melodic/setup.sh
-                                    ./src/franka_ros/.ci/build.sh
-                                '''
+                }
+                stages {
+                    stage('Notify Stash') {
+                        when {
+                            allOf {
+                                environment name: 'DISTRO', value: 'noetic'
+                                environment name: 'BUILD_TOOL', value: 'catkin_make'
                             }
                         }
-                        stage('Test') {
-                            steps {
-                                sh ''' . /opt/ros/melodic/setup.sh
-                                    export HOME=$(pwd)
-                                    ./src/franka_ros/.ci/test.sh
-                                '''
-                            }
-                            post {
-                                always {
-                                    junit 'build-debug/test_results/**/*.xml'
-                                }
+                        steps {
+                            script {
+                                notifyBitbucket()
                             }
                         }
                     }
-                }
-                stage('Noetic') {
-                    agent {
-                        dockerfile {
-                            filename '.ci/Dockerfile.noetic'
-                            dir 'src/franka_ros'
+                    stage('Build w/ Catkin Make') {
+                        when {
+                            environment name: 'BUILD_TOOL', value: 'catkin_make'
+                        }
+                        steps {
+                            sh '''
+                                . /opt/ros/${DISTRO}/setup.sh
+                                rm -rf src/CMakeLists.txt build devel
+                                catkin_init_workspace src
+                                ${BUILD_TOOL} -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
+                            '''
                         }
                     }
-                    stages {
-                        stage('Build') {
-                            steps {
-                                sh ''' . /opt/ros/noetic/setup.sh
-                                    ./src/franka_ros/.ci/build.sh
-                                '''
-                            }
+                    stage('Build w/ Catkin Tools') {
+                        when {
+                            environment name: 'BUILD_TOOL', value: 'catkin build'
+                        }
+                        environment {
+                            HOME=sh(script: 'pwd', returnStdout: true).trim()
+                            SPACE_SUFFIX='_catkin_tools'
+                        }
+                        steps {
+                            sh '''
+                                . /opt/ros/${DISTRO}/setup.sh
+                                rm -rf *${SPACE_SUFFIX}
+                                catkin config --workspace . --init --extend /opt/ros/${DISTRO} \\
+                                    --space-suffix ${SPACE_SUFFIX}
+                                ${BUILD_TOOL} --no-status
+                            '''
+                        }
+                    }
+                    stage('Check Format') {
+                        when {
+                            environment name: 'BUILD_TOOL', value: 'catkin_make'
+                        }
+                        steps {
+                            sh '''
+                                cmake --build build --target check-format
+                            '''
+                        }
+                    }
+                    stage('Check Linting') {
+                        when {
+                            environment name: 'BUILD_TOOL', value: 'catkin_make'
+                        }
+                        steps {
+                            sh '''
+                                . /opt/ros/${DISTRO}/setup.sh
+                                cmake --build build --target check-tidy
+                            '''
+                        }
+                    }
+                    stage('Test') {
+                        when {
+                            environment name: 'BUILD_TOOL', value: 'catkin_make'
+                        }
+                        environment {
+                            HOME=sh(script: 'pwd', returnStdout: true).trim()
                         }
-                        stage('Test') {
-                            steps {
-                                sh ''' . /opt/ros/noetic/setup.sh
-                                    export HOME=$(pwd)
-                                    ./src/franka_ros/.ci/test.sh
-                                '''
+                        steps {
+                            sh ''' 
+                                . /opt/ros/${DISTRO}/setup.sh
+                                ${BUILD_TOOL} run_tests
+                                catkin_test_results
+                            '''
+                        }
+                        post {
+                            always {
+                                junit 'build/test_results/**/*.xml'
                             }
-                            post {
-                                always {
-                                    junit 'build-debug/test_results/**/*.xml'
-                                }
+                        }
+                    }
+                    stage('Check commit history sync') {
+                        when {
+                            allOf {
+                                environment name: 'DISTRO', value: 'noetic'
+                                environment name: 'BUILD_TOOL', value: 'catkin_make'
                             }
                         }
+                        steps {
+                            sh """
+                                cd src/franka_ros
+                                .ci/checkgithistory.sh \\
+                                    https://github.com/frankaemika/franka_ros.git develop
+                            """
+                        }
                     }
                 }
             }
         }
-        stage('Check commit history sync') {
-            agent {
-                dockerfile {
-                    filename '.ci/Dockerfile.noetic'
-                    dir 'src/franka_ros'
-                }
-            }
-            steps {
-                sh """
-                cd src/franka_ros
-                .ci/checkgithistory.sh https://github.com/frankaemika/franka_ros.git develop
-                """
-            }
-        }
     }
     post {
         always {