diff --git a/CMakeLists.txt b/CMakeLists.txt
index d6d5ca231ddee8214e23b56d411d6a24d653a497..98513eb2df7f2c26a25a320ae8ec941e0263ad2a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,4 +59,17 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
 
 add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
 
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp)
+#    add_executable(tests_grasping test/test.cpp)
+    target_link_libraries(tests_grasping ${catkin_LIBRARIES})
+    add_dependencies(tests_grasping panda_grasping_generate_messages_cpp)
+endif()
 
diff --git a/test/grasping.test b/test/grasping.test
new file mode 100644
index 0000000000000000000000000000000000000000..4625e3f870f202276c3cdccd1833a05efe8e745d
--- /dev/null
+++ b/test/grasping.test
@@ -0,0 +1,12 @@
+<launch>
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+          args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen">
+        <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    </node>
+
+    <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/>
+
+    <test test-name="tests_grasping" pkg="panda_grasping" type="tests_grasping" time-limit="3600.0"/>
+</launch>
\ No newline at end of file
diff --git a/test/plan_visualizer.rviz b/test/plan_visualizer.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..39129263ac47e7041802e815971ee4fe76b20300
--- /dev/null
+++ b/test/plan_visualizer.rviz
@@ -0,0 +1,296 @@
+Panels:
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Goal_Tolerance: 0
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: true
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: false
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: true
+      Velocity_Scaling_Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 2.9803097248077393
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.0864897966384888
+        Y: 0.3387787342071533
+        Z: 2.2351798634190345e-7
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.270203560590744
+      Target Frame: panda_link0
+      Value: XYOrbit (rviz)
+      Yaw: 0.42677485942840576
+    Saved: ~
+Window Geometry:
+  Height: 1404
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000200000000000002a200000526fc0200000007fb000000100044006900730070006c006100790073010000003b000003e60000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000024d000003140000017200fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004f0000000710000000000000000000000030000067f0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000000000000067f0000010000ffffff0000067f000004e100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1663
+  X: 0
+  Y: 18
diff --git a/test/test.cpp b/test/test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..44b752c928db8b1ee60372d5d0f60ee8b2380872
--- /dev/null
+++ b/test/test.cpp
@@ -0,0 +1,144 @@
+#pragma clang diagnostic push
+#pragma ide diagnostic ignored "cert-err58-cpp"
+//
+// Created by Johannes Mey on 22.11.20.
+//
+
+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+
+#include "panda_grasping/PickObject.h"
+#include "panda_grasping/PlaceObject.h"
+
+#include <gtest/gtest.h>
+
+ros::ServiceClient pickClient;
+ros::ServiceClient placeClient;
+
+std::vector<tf2::Vector3> objects{
+        tf2::Vector3(0.01, 0.1, 0.1),
+        tf2::Vector3(0.05, 0.1, 0.1),
+        tf2::Vector3(0.005, 0.1, 0.1)
+};
+
+std::vector<tf2::Vector3> points{
+        tf2::Vector3(0.5, 0.3, 0.2),
+        tf2::Vector3(-0.5, -0.3, 0.3),
+        tf2::Vector3(0.5, 0.3, 0),
+        tf2::Vector3(0.4, 0.5, 0.4)
+};
+
+
+std::vector<double> angles{
+        M_PI_2,
+        1,
+        -M_PI_4
+};
+
+
+void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) {
+    ROS_INFO_STREAM("starting test");
+    tf2::Quaternion orientation;
+    {
+        orientation.setRPY(0, 0, fromAngle);
+
+        panda_grasping::PickObject pickObjectSrv;
+        pickObjectSrv.request.dimensions = tf2::toMsg(size);
+        tf2::toMsg(from, pickObjectSrv.request.pose.position);
+        pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+
+        ROS_INFO_STREAM("starting to pick");
+
+        EXPECT_TRUE(pickClient.exists());
+        EXPECT_TRUE(pickClient.call(pickObjectSrv));
+    }
+    // wait a little while
+    ros::Duration(0.5).sleep();
+    {
+        orientation.setRPY(0, 0, toAngle);
+
+        panda_grasping::PlaceObject placeObjectSrv;
+        placeObjectSrv.request.dimensions = tf2::toMsg(size);
+        tf2::toMsg(to, placeObjectSrv.request.pose.position);
+        placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation);
+
+        ROS_INFO_STREAM("starting to place");
+
+        EXPECT_TRUE(placeClient.exists());
+        EXPECT_TRUE(placeClient.call(placeObjectSrv));
+
+
+        ROS_INFO_STREAM("pick and place finished");
+    }
+}
+
+
+TEST(Grasping, Grasping_WithoutRotation_Object_0) {
+    auto object = objects[0];
+
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    pickAndPlace(object, points[2], 0, points[3], 0);
+}
+
+TEST(Grasping, Grasping_WithoutRotation_Object_1) {
+    auto object = objects[1];
+
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    pickAndPlace(object, points[2], 0, points[3], 0);
+}
+
+TEST(Grasping, Grasping_WithoutRotation_Object_2) {
+    auto object = objects[2];
+
+    pickAndPlace(object, points[0], 0, points[1], 0);
+    pickAndPlace(object, points[2], 0, points[3], 0);
+}
+
+TEST(Grasping, GraspingWithRotation_Object_0_Rotations_00) {
+    auto object = objects[0];
+    auto fromAngle = angles[0];
+    auto toAngle = angles[0];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
+}
+
+TEST(Grasping, GraspingWithRotation_Object_0_Rotations_01) {
+    auto object = objects[0];
+    auto fromAngle = angles[0];
+    auto toAngle = angles[1];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
+}
+
+TEST(Grasping, GraspingWithRotation_Object_0_Rotations_12) {
+    auto object = objects[0];
+    auto fromAngle = angles[1];
+    auto toAngle = angles[2];
+
+    pickAndPlace(object, points[0], fromAngle, points[1], toAngle);
+    pickAndPlace(object, points[2], fromAngle, points[3], toAngle);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "grasping_test");
+    ros::NodeHandle n("panda_grasping");
+
+
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+    ros::Duration(3.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");
+
+
+    pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
+    placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
+
+    return RUN_ALL_TESTS();
+}
+
+#pragma clang diagnostic pop
\ No newline at end of file