diff --git a/gz_gripper_plugin/CMakeLists.txt b/gz_gripper_plugin/CMakeLists.txt
index b6d2fc194e4b7503c0e6798d1a7f3fa3c6ee1fcf..5dff16c70916f87b0cead65c435b904fd6c6b3d6 100644
--- a/gz_gripper_plugin/CMakeLists.txt
+++ b/gz_gripper_plugin/CMakeLists.txt
@@ -7,11 +7,14 @@ add_definitions(-std=c++11)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  roscpp
   actionlib
   gazebo_plugins
   gazebo_ros
   iri_common_drivers_msgs
   tf
+  std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -56,11 +59,10 @@ find_package(gazebo REQUIRED)
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  CheckGrasped.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -70,10 +72,10 @@ find_package(gazebo REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   iri_common_drivers_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -107,7 +109,7 @@ find_package(gazebo REQUIRED)
 catkin_package(
 #  INCLUDE_DIRS include
   LIBRARIES gz_gripper_plugin
-#  CATKIN_DEPENDS actionlib gazebo_plugins gazebo_ros iri_common_drivers_msgs tf
+  CATKIN_DEPENDS actionlib gazebo_plugins gazebo_ros iri_common_drivers_msgs tf std_msgs message_runtime
 #  DEPENDS gazebo
 )
 
diff --git a/gz_gripper_plugin/package.xml b/gz_gripper_plugin/package.xml
index da5a8764522d07a2ae53afe402a043c14672d660..71430c2fa32d46ed8afe0b4b4a9f00e31b62731c 100644
--- a/gz_gripper_plugin/package.xml
+++ b/gz_gripper_plugin/package.xml
@@ -47,12 +47,16 @@
   <build_depend>gazebo_ros</build_depend>
   <build_depend>iri_common_drivers_msgs</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
   <run_depend>actionlib</run_depend>
   <run_depend>gazebo</run_depend>
   <run_depend>gazebo_plugins</run_depend>
   <run_depend>gazebo_ros</run_depend>
   <run_depend>iri_common_drivers_msgs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/gz_gripper_plugin/srv/CheckGrasped.srv b/gz_gripper_plugin/srv/CheckGrasped.srv
new file mode 100644
index 0000000000000000000000000000000000000000..6bfb6d0a17033cc9b5103e281b9fcb95eb61527b
--- /dev/null
+++ b/gz_gripper_plugin/srv/CheckGrasped.srv
@@ -0,0 +1,2 @@
+---
+std_msgs/String grasped