diff --git a/CMakeLists.txt b/CMakeLists.txt
index 755d83f..ad2cf12 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,9 @@ find_package(dynmsg REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_servo REQUIRED)
+find_package(moveit_core REQUIRED)
 
 add_executable(benchmark_main src/benchmark_main.cpp
                               src/scenarios/scenario_perception_pipeline.cpp)
@@ -35,7 +38,32 @@ target_include_directories(
 target_link_libraries(benchmark_main PUBLIC "benchmark::benchmark"
                                             ${YAML_CPP_LIBRARIES})
 
-install(TARGETS benchmark_main DESTINATION lib/${PROJECT_NAME})
+add_executable(servo_benchmark_main src/servo_benchmark_main.cpp
+                                    src/scenarios/scenario_servo_pipeline.cpp)
+
+ament_target_dependencies(
+  servo_benchmark_main
+  PUBLIC
+  "moveit_ros_planning_interface"
+  "rclcpp"
+  "benchmark"
+  "dynmsg"
+  "nav_msgs"
+  "yaml-cpp"
+  "moveit_msgs"
+  "moveit_servo"
+  "moveit_core")
+
+target_include_directories(
+  servo_benchmark_main
+  PUBLIC $<INSTALL_INTERFACE:include>
+         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+target_link_libraries(servo_benchmark_main PUBLIC "benchmark::benchmark"
+                                                  ${YAML_CPP_LIBRARIES})
+
+install(TARGETS benchmark_main servo_benchmark_main
+        DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 
diff --git a/config/scenario_servo_pipeline_test_cases.yaml b/config/scenario_servo_pipeline_test_cases.yaml
new file mode 100644
index 0000000..b44ec3e
--- /dev/null
+++ b/config/scenario_servo_pipeline_test_cases.yaml
@@ -0,0 +1,6 @@
+test_cases:
+  - angular:
+      z : 1.2
+
+  - linear:
+      x : 1.2
diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp
new file mode 100644
index 0000000..233f771
--- /dev/null
+++ b/include/moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp
@@ -0,0 +1,113 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <moveit_msgs/srv/servo_command_type.hpp>
+#include <moveit_msgs/msg/servo_status.hpp>
+#include <moveit_servo/utils/datatypes.hpp>
+#include <thread>
+#include <chrono>
+#include <memory>
+#include <benchmark/benchmark.h>
+#include <thread>
+
+#include <dynmsg/msg_parser.hpp>
+#include <dynmsg/typesupport.hpp>
+#include <dynmsg/yaml_utils.hpp>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <unistd.h>
+#include <sys/wait.h>
+#include <sys/types.h>
+#include <signal.h>
+
+namespace
+{
+const std::string PLANNING_GROUP = "panda_arm";
+const std::string PACKAGE_SHARE_DIRECTORY = ament_index_cpp::get_package_share_directory("moveit_middleware_benchmark");
+const std::string TEST_CASES_YAML_FILE = PACKAGE_SHARE_DIRECTORY + "/config/scenario_servo_pipeline_test_cases.yaml";
+
+}  // namespace
+
+namespace moveit
+{
+
+namespace middleware_benchmark
+{
+
+// TODO @CihatAltiparmak : Move this class definition into separate place like utils directory
+class ProcessUtils
+{
+public:
+  ProcessUtils()
+  {
+  }
+  ProcessUtils(rclcpp::Node::SharedPtr);
+  void startROSControllers();
+  void killROSControllers();
+
+private:
+  pid_t ros2_controller_pid_;
+  rclcpp::Node::SharedPtr node_;
+};
+
+class ScenarioServoPipeline
+{
+public:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Client<moveit_msgs::srv::ServoCommandType>::SharedPtr servo_switch_command_type_client_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_command_pubisher_;
+  rclcpp::Subscription<moveit_msgs::msg::ServoStatus>::SharedPtr servo_status_subscriber_;
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> servo_status_executor_;
+  std::thread servo_status_executor_thread_;
+  rclcpp::Duration server_timeout_;
+
+  ScenarioServoPipeline(rclcpp::Node::SharedPtr node,
+                        const rclcpp::Duration& server_timeout = rclcpp::Duration::from_seconds(-1));
+  ~ScenarioServoPipeline();
+  void switchCommandType(const moveit_servo::CommandType& servo_command_type,
+                         const rclcpp::Duration& timeout = rclcpp::Duration::from_seconds(-1));
+  void sendTwistCommandToServo(const std::string& frame_id, const geometry_msgs::msg::Twist& target_twist);
+  moveit_servo::StatusCode getServoStatus();
+  void runTestCase(const geometry_msgs::msg::Twist& test_case);
+
+private:
+  moveit_servo::StatusCode latest_servo_status_code_;
+  std::string tested_link_;
+};
+
+class ScenarioServoPipelineFixture : public benchmark::Fixture
+{
+protected:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<moveit::middleware_benchmark::ScenarioServoPipeline> servo_pipeline_;
+
+  /* selected test case index for benchmarking */
+  size_t selected_test_case_index_;
+
+  /** the list of test cases to be tested */
+  std::vector<geometry_msgs::msg::Twist> test_cases_ = {};
+  pid_t pid_;
+  ProcessUtils process_utils_;
+
+public:
+  ScenarioServoPipelineFixture();
+  void SetUp(benchmark::State& /*state*/);
+  void TearDown(benchmark::State& /*state*/);
+
+  geometry_msgs::msg::Twist selectTestCase(size_t test_case_index);
+  void createTestCases();
+  void readTestCasesFromFile(const std::string& yaml_file_name);
+  geometry_msgs::msg::Twist getTestCaseFromYamlString(const std::string& yaml_string);
+};
+
+}  // namespace middleware_benchmark
+}  // namespace moveit
diff --git a/launch/servo_benchmark.launch.py b/launch/servo_benchmark.launch.py
new file mode 100644
index 0000000..63bb33b
--- /dev/null
+++ b/launch/servo_benchmark.launch.py
@@ -0,0 +1,174 @@
+import os
+import launch
+import launch_ros
+from ament_index_python.packages import get_package_share_directory
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_param_builder import ParameterBuilder
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .joint_limits(file_path="config/hard_joint_limits.yaml")
+        .to_moveit_configs()
+    )
+
+    # Launch Servo as a standalone node or as a "node component" for better latency/efficiency
+    launch_as_standalone_node = LaunchConfiguration(
+        "launch_as_standalone_node", default="false"
+    )
+
+    # Get parameters for the Servo node
+    servo_params = {
+        "moveit_servo": ParameterBuilder("moveit_servo")
+        .yaml("config/panda_simulated_config.yaml")
+        .to_dict()
+    }
+
+    # This sets the update rate and planning group name for the acceleration limiting filter.
+    acceleration_filter_update_period = {"update_period": 0.01}
+    planning_group_name = {"planning_group_name": "panda_arm"}
+
+    # RViz
+    rviz_config_file = (
+        get_package_share_directory("moveit_servo")
+        + "/config/demo_rviz_config_ros.rviz"
+    )
+
+    rviz_node = launch_ros.actions.Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+        ],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
+        output="screen",
+    )
+
+    joint_state_broadcaster_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager-timeout",
+            "300",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    panda_arm_controller_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    # Launch as much as possible in components
+    container = launch_ros.actions.ComposableNodeContainer(
+        name="moveit_servo_demo_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            # Example of launching Servo as a node component
+            # Launching as a node component makes ROS 2 intraprocess communication more efficient.
+            launch_ros.descriptions.ComposableNode(
+                package="moveit_servo",
+                plugin="moveit_servo::ServoNode",
+                name="servo_node",
+                parameters=[
+                    servo_params,
+                    acceleration_filter_update_period,
+                    planning_group_name,
+                    moveit_config.robot_description,
+                    moveit_config.robot_description_semantic,
+                    moveit_config.robot_description_kinematics,
+                    moveit_config.joint_limits,
+                ],
+                condition=UnlessCondition(launch_as_standalone_node),
+            ),
+            launch_ros.descriptions.ComposableNode(
+                package="robot_state_publisher",
+                plugin="robot_state_publisher::RobotStatePublisher",
+                name="robot_state_publisher",
+                parameters=[moveit_config.robot_description],
+            ),
+            launch_ros.descriptions.ComposableNode(
+                package="tf2_ros",
+                plugin="tf2_ros::StaticTransformBroadcasterNode",
+                name="static_tf2_broadcaster",
+                parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
+            ),
+        ],
+        output="screen",
+    )
+    # Launch a standalone Servo node.
+    # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
+    servo_node = launch_ros.actions.Node(
+        package="moveit_servo",
+        executable="servo_node",
+        name="servo_node",
+        parameters=[
+            servo_params,
+            acceleration_filter_update_period,
+            planning_group_name,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+        output="screen",
+        condition=IfCondition(launch_as_standalone_node),
+    )
+
+    benchmark_node = launch_ros.actions.Node(
+        package="moveit_middleware_benchmark",
+        executable="servo_benchmark_main",
+        name="servo_main_node",
+        parameters=[
+            servo_params,
+            acceleration_filter_update_period,
+            planning_group_name,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+            {"selected_test_case_index": 0},
+            {"selected_link": "panda_link4"},
+        ],
+        output="screen",
+        on_exit=launch.actions.Shutdown(),
+    )
+
+    return launch.LaunchDescription(
+        [
+            benchmark_node,
+            rviz_node,
+            # ros2_control_node,
+            # joint_state_broadcaster_spawner,
+            # panda_arm_controller_spawner,
+            servo_node,
+            container,
+        ]
+    )
diff --git a/src/scenarios/scenario_perception_pipeline.cpp b/src/scenarios/scenario_perception_pipeline.cpp
index c91f065..44d8cea 100644
--- a/src/scenarios/scenario_perception_pipeline.cpp
+++ b/src/scenarios/scenario_perception_pipeline.cpp
@@ -150,7 +150,9 @@ BENCHMARK_DEFINE_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_p
   }
 }
 
-BENCHMARK_REGISTER_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline);
+BENCHMARK_REGISTER_F(ScenarioPerceptionPipelineFixture, test_scenario_perception_pipeline)
+    ->MeasureProcessCPUTime()
+    ->UseRealTime();
 
 }  // namespace middleware_benchmark
 }  // namespace moveit
diff --git a/src/scenarios/scenario_servo_pipeline.cpp b/src/scenarios/scenario_servo_pipeline.cpp
new file mode 100644
index 0000000..8db27f7
--- /dev/null
+++ b/src/scenarios/scenario_servo_pipeline.cpp
@@ -0,0 +1,248 @@
+#include "moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp"
+#include <unistd.h>
+#include <sys/wait.h>
+#include <sys/types.h>
+#include <signal.h>
+#include <iostream>
+
+namespace moveit
+{
+namespace middleware_benchmark
+{
+
+ProcessUtils::ProcessUtils(rclcpp::Node::SharedPtr node) : node_(node)
+{
+}
+
+void ProcessUtils::startROSControllers()
+{
+  pid_t pid = fork();
+  if (pid == 0)
+  {
+    // child process
+    // TODO @CihatAltiparmak : Move this command arguments into parameter file
+    const std::string PANDA_PACKAGE_SHARE_DIRECTORY =
+        ament_index_cpp::get_package_share_directory("moveit_resources_panda_moveit_config");
+
+    const std::string ros2_control_param_file = PANDA_PACKAGE_SHARE_DIRECTORY + "/config/ros2_controllers.yaml";
+    char* ros2_control_param_file_cstr = new char[ros2_control_param_file.length() + 1];
+    std::strcpy(ros2_control_param_file_cstr, ros2_control_param_file.c_str());
+
+    char* command[] = { "/opt/ros/rolling/lib/controller_manager/ros2_control_node",
+                        "--ros-args",
+                        "--params-file",
+                        ros2_control_param_file_cstr,
+                        "--remap",
+                        "/controller_manager/robot_description:=/robot_description",
+                        NULL };
+
+    execvp("/opt/ros/rolling/lib/controller_manager/ros2_control_node", command);
+
+    RCLCPP_FATAL(node_->get_logger(), "A problem occurred while running controller manager node in child node!");
+    exit(1);
+  }
+  else if (pid > 0)
+  {
+    ros2_controller_pid_ = pid;
+
+    // TODO @CihatAltiparmak : Move this command arguments into parameter file
+    std::system("ros2 run controller_manager spawner joint_state_broadcaster --controller-manager-timeout 3000 "
+                "--controller-manager /controller_manager");
+    std::system("ros2 run controller_manager spawner panda_arm_controller --controller-manager-timeout 3000 "
+                "--controller-manager /controller_manager");
+  }
+  else
+  {
+    RCLCPP_FATAL(node_->get_logger(),
+                 "A problem occurred while creating a child process in order to run controller manager!");
+    exit(1);
+  }
+}
+
+void ProcessUtils::killROSControllers()
+{
+  RCLCPP_INFO(
+      node_->get_logger(),
+      "ProcessUtils::killROSControllers() started to kill the child process controller_manager is run with pid %d",
+      ros2_controller_pid_);
+  kill(ros2_controller_pid_, SIGTERM);
+  int status;
+  std::cout << "[ProcessUtils::killROSControllers] [ waiting for ] " << ros2_controller_pid_ << std::endl;
+  waitpid(ros2_controller_pid_, &status, 0);
+  RCLCPP_INFO(node_->get_logger(),
+              "ProcessUtils::killROSControllers() waiting the process with %d pid is finished! Got status : %d",
+              ros2_controller_pid_, status);
+}
+
+ScenarioServoPipeline::ScenarioServoPipeline(rclcpp::Node::SharedPtr node, const rclcpp::Duration& server_timeout)
+  : node_(node), server_timeout_(server_timeout), latest_servo_status_code_(moveit_servo::StatusCode::NO_WARNING)
+{
+  node_->get_parameter("selected_link", tested_link_);
+  ;
+
+  servo_switch_command_type_client_ =
+      node_->create_client<moveit_msgs::srv::ServoCommandType>("/servo_node/switch_command_type");
+  if (servo_switch_command_type_client_->wait_for_service(server_timeout_.to_chrono<std::chrono::duration<double>>()))
+  {
+    RCLCPP_INFO(node_->get_logger(), "SwitchCommandType Server is ready!");
+  }
+  else
+  {
+    RCLCPP_ERROR(node_->get_logger(), "SwitchCommandType Server is not ready!");
+  }
+
+  servo_command_pubisher_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("/servo_node/delta_twist_cmds",
+                                                                                      rclcpp::SystemDefaultsQoS());
+
+  servo_status_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive,
+                                                     false /* don't spin with node executor */);
+  auto options = rclcpp::SubscriptionOptions();
+  options.callback_group = callback_group;
+  servo_status_subscriber_ = node_->create_subscription<moveit_msgs::msg::ServoStatus>(
+      "/servo_node/status", rclcpp::SystemDefaultsQoS(),
+      [this](moveit_msgs::msg::ServoStatus::SharedPtr new_servo_status_msg) {
+        latest_servo_status_code_ = static_cast<moveit_servo::StatusCode>(new_servo_status_msg->code);
+      },
+      options);
+
+  servo_status_executor_->add_callback_group(callback_group, node_->get_node_base_interface());
+  servo_status_executor_thread_ = std::thread([this]() { servo_status_executor_->spin(); });
+}
+
+ScenarioServoPipeline::~ScenarioServoPipeline()
+{
+  if (servo_status_executor_)
+  {
+    servo_status_executor_->cancel();
+  }
+
+  if (servo_status_executor_thread_.joinable())
+  {
+    servo_status_executor_thread_.join();
+  }
+}
+
+void ScenarioServoPipeline::runTestCase(const geometry_msgs::msg::Twist& test_case)
+{
+  rclcpp::WallRate servo_frequency(10);
+  while (getServoStatus() == moveit_servo::StatusCode::NO_WARNING)
+  {
+    this->sendTwistCommandToServo(tested_link_, test_case);
+    servo_frequency.sleep();
+  }
+}
+
+void ScenarioServoPipeline::switchCommandType(const moveit_servo::CommandType& servo_command_type,
+                                              const rclcpp::Duration& timeout)
+{
+  auto servo_command_type_request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
+  servo_command_type_request->command_type = static_cast<int8_t>(servo_command_type);
+
+  auto result = servo_switch_command_type_client_->async_send_request(servo_command_type_request);
+  if (rclcpp::spin_until_future_complete(node_, result, timeout.to_chrono<std::chrono::duration<double>>()) ==
+      rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_INFO(node_->get_logger(), "Got the response from servo switch command server!");
+  }
+  else
+  {
+    RCLCPP_ERROR(node_->get_logger(), "A problem occurred while sending switch command to servo node!");
+  }
+}
+
+moveit_servo::StatusCode ScenarioServoPipeline::getServoStatus()
+{
+  return latest_servo_status_code_;
+}
+
+void ScenarioServoPipeline::sendTwistCommandToServo(const std::string& link,
+                                                    const geometry_msgs::msg::Twist& target_twist)
+{
+  geometry_msgs::msg::TwistStamped target_twist_stamped;
+  target_twist_stamped.twist = target_twist;
+  target_twist_stamped.header.stamp = node_->now();
+  target_twist_stamped.header.frame_id = link;
+
+  servo_command_pubisher_->publish(target_twist_stamped);
+}
+
+ScenarioServoPipelineFixture::ScenarioServoPipelineFixture()
+{
+  createTestCases();
+}
+
+void ScenarioServoPipelineFixture::SetUp(benchmark::State& /*state*/)
+{
+  process_utils_.startROSControllers();
+
+  if (node_.use_count() == 0)
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_servo_pipeline_node",
+                                           rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
+    // TODO @CihatAltiparmak : assign initial value for ros arguments
+    // for example, selected_test_case_index_ = 0;
+    node_->get_parameter("selected_test_case_index", selected_test_case_index_);
+    RCLCPP_INFO(node_->get_logger(), "SELECTED TEST CASE INDEX: %ld", selected_test_case_index_);
+
+    process_utils_ = ProcessUtils(node_);
+  }
+}
+
+void ScenarioServoPipelineFixture::TearDown(benchmark::State& /*state*/)
+{
+  process_utils_.killROSControllers();
+  RCLCPP_INFO(node_->get_logger(), "ScenarioServoPipelineFixture::TearDown() ROS 2 Controller is terminated!");
+  std::cout << "ROS 2 Controller is terminated!" << std::endl;
+}
+
+geometry_msgs::msg::Twist ScenarioServoPipelineFixture::selectTestCase(size_t test_case_index)
+{
+  return test_cases_.at(test_case_index);
+}
+
+void ScenarioServoPipelineFixture::createTestCases()
+{
+  readTestCasesFromFile(TEST_CASES_YAML_FILE);
+}
+
+void ScenarioServoPipelineFixture::readTestCasesFromFile(const std::string& yaml_file_name)
+{
+  YAML::Node config = YAML::LoadFile(yaml_file_name.c_str());
+  for (YAML::const_iterator it = config["test_cases"].begin(); it != config["test_cases"].end(); ++it)
+  {
+    const std::string yaml_string = dynmsg::yaml_to_string(*it);
+    geometry_msgs::msg::Twist test_case = getTestCaseFromYamlString(yaml_string);
+
+    test_cases_.push_back(test_case);
+  }
+}
+
+geometry_msgs::msg::Twist ScenarioServoPipelineFixture::getTestCaseFromYamlString(const std::string& yaml_string)
+{
+  geometry_msgs::msg::Twist twist_msg;
+  void* ros_message = reinterpret_cast<void*>(&twist_msg);
+
+  dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(dynmsg::cpp::get_type_info({ "geometry_msgs", "Twist" }), yaml_string,
+                                           ros_message);
+
+  return twist_msg;
+}
+
+BENCHMARK_DEFINE_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)(::benchmark::State& st)
+{
+  auto selected_test_case = selectTestCase(selected_test_case_index_);
+
+  servo_pipeline_ = std::make_shared<moveit::middleware_benchmark::ScenarioServoPipeline>(node_);
+  servo_pipeline_->switchCommandType(moveit_servo::CommandType::TWIST);
+
+  for (auto _ : st)
+  {
+    servo_pipeline_->runTestCase(selected_test_case);
+  }
+}
+
+BENCHMARK_REGISTER_F(ScenarioServoPipelineFixture, test_scenario_servo_pipeline)->MeasureProcessCPUTime()->UseRealTime();
+
+}  // namespace middleware_benchmark
+}  // namespace moveit
diff --git a/src/servo_benchmark_main.cpp b/src/servo_benchmark_main.cpp
new file mode 100644
index 0000000..d51acee
--- /dev/null
+++ b/src/servo_benchmark_main.cpp
@@ -0,0 +1,11 @@
+#include "moveit_middleware_benchmark/scenarios/scenario_servo_pipeline.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+  benchmark::Shutdown();
+  rclcpp::shutdown();
+  return 0;
+}