diff --git a/CMakeLists.txt b/CMakeLists.txt
index bbe74e6..ff23ff2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -73,6 +73,7 @@ set(msg_files
   "msg/WorkspaceParameters.msg"
   "msg/KinematicSolverInfo.msg"
   "msg/PositionIKRequest.msg"
+  "msg/ServoStatus.msg"
 )
 
 set(srv_files
@@ -97,8 +98,7 @@ set(srv_files
   "srv/CheckIfRobotStateExistsInWarehouse.srv"
   "srv/RenameRobotStateInWarehouse.srv"
   "srv/DeleteRobotStateFromWarehouse.srv"
-  "srv/ChangeControlDimensions.srv"
-  "srv/ChangeDriftDimensions.srv"
+  "srv/ServoCommandType.srv"
 )
 
 set(action_files
diff --git a/msg/ServoStatus.msg b/msg/ServoStatus.msg
new file mode 100644
index 0000000..ad10ec3
--- /dev/null
+++ b/msg/ServoStatus.msg
@@ -0,0 +1,13 @@
+# Message
+int8 code # will contains integer code
+string message # will contain explanatory message
+
+# Status types (should reflect StatusCode from moveit_servo/utils/datatype.hpp)
+int8 INVALID = -1
+int8 NO_WARNING = 0
+int8 DECELERATE_FOR_APPROACHING_SINGULARITY = 1
+int8 HALT_FOR_SINGULARITY = 2
+int8 DECELERATE_FOR_LEAVING_SINGULARITY = 3
+int8 DECELERATE_FOR_COLLISION = 4
+int8 HALT_FOR_COLLISION = 5
+int8 JOINT_BOUND = 6
diff --git a/srv/ChangeControlDimensions.srv b/srv/ChangeControlDimensions.srv
deleted file mode 100644
index c5eb914..0000000
--- a/srv/ChangeControlDimensions.srv
+++ /dev/null
@@ -1,12 +0,0 @@
-# For use with moveit_jog_arm Cartesian planner
-#
-# Turn on/off jogging along these dimensions.
-# Give 'true' to enable jogging in the direction, 'false' to disable
-bool control_x_translation
-bool control_y_translation
-bool control_z_translation
-bool control_x_rotation
-bool control_y_rotation
-bool control_z_rotation
----
-bool success
diff --git a/srv/ChangeDriftDimensions.srv b/srv/ChangeDriftDimensions.srv
deleted file mode 100644
index 78592f9..0000000
--- a/srv/ChangeDriftDimensions.srv
+++ /dev/null
@@ -1,17 +0,0 @@
-# For use with moveit_jog_arm Cartesian planner
-#
-# Allow the robot to drift along these dimensions in a smooth but unregulated way.
-# Give 'true' to enable drift in the direction, 'false' to disable.
-# For example, may allow wrist rotation by drift_x_rotation == true.
-bool drift_x_translation
-bool drift_y_translation
-bool drift_z_translation
-bool drift_x_rotation
-bool drift_y_rotation
-bool drift_z_rotation
-
-# Not implemented as of Jan 2020 (for now assumed to be the identity matrix). In the future it will allow us to transform
-# from the jog control frame to a unique drift frame, so the robot can drift along off-principal axes
-geometry_msgs/Transform transform_jog_frame_to_drift_frame
----
-bool success
diff --git a/srv/ServoCommandType.srv b/srv/ServoCommandType.srv
new file mode 100644
index 0000000..0dcdd61
--- /dev/null
+++ b/srv/ServoCommandType.srv
@@ -0,0 +1,10 @@
+# Request constants
+int8 JOINT_JOG = 0
+int8 TWIST = 1
+int8 POSE = 2
+
+# Request Field
+int8 command_type
+---
+# Response Field
+bool success