From 21731507cff0b1b40caedec5a8c60a088edbdbe6 Mon Sep 17 00:00:00 2001
From: jerryjerui <jerryjerui@163.com>
Date: Tue, 25 May 2021 14:42:31 +0800
Subject: [PATCH] fix: Service cannot process error.

---
 CMakeLists.txt              |  1 +
 scripts/mycobot_services.py | 19 +++++++++++++++++++
 srv/GetAngles.srv           |  3 ---
 srv/GripperStatus.srv       |  4 +++-
 srv/PumpStatus.srv          |  5 +++++
 srv/SetAngles.srv           |  4 +++-
 srv/SetCoords.srv           |  4 +++-
 7 files changed, 34 insertions(+), 6 deletions(-)
 create mode 100644 srv/PumpStatus.srv

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b135055..6754309 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,6 +25,7 @@ add_service_files(FILES
   GetCoords.srv
   SetCoords.srv
   GripperStatus.srv
+  PumpStatus.srv
 )
 
 ## Generate added messages and services
diff --git a/scripts/mycobot_services.py b/scripts/mycobot_services.py
index b6f2940..5bd0cb3 100755
--- a/scripts/mycobot_services.py
+++ b/scripts/mycobot_services.py
@@ -24,6 +24,7 @@ def create_services():
     rospy.Service('set_joint_coords', SetCoords, set_coords)
     rospy.Service('get_joint_coords', GetCoords, get_coords)
     rospy.Service('switch_gripper_status', GripperStatus, switch_status)
+    rospy.Service('switch_pump_status', PumpStatus, toggle_pump)
     rospy.loginfo('ready')
     rospy.spin()
 
@@ -42,6 +43,8 @@ def set_angles(req):
     if mc:
         mc.send_angles(angles, sp)
 
+    return SetAnglesResponse(True)
+
 
 def get_angles(req):
     if mc:
@@ -63,6 +66,8 @@ def set_coords(req):
 
     if mc:
         mc.send_coords(coords, sp, mod)
+    
+    return SetCoordsResponse(True)
 
 
 def get_coords(req):
@@ -78,6 +83,20 @@ def switch_status(req):
         else:
             mc.set_gripper_state(1, 80)
 
+    return GripperStatusResponse(True)
+
+
+def toggle_pump(req):
+    if mc:
+        if req.Status:
+            mc.set_basic_output(2, 0)
+            mc.set_basic_output(5, 0)
+        else:
+            mc.set_basic_output(2, 1)
+            mc.set_basic_output(5, 1)
+
+    return PumpStatusResponse(True)
+
 
 robot_msg = '''
 MyCobot Status
diff --git a/srv/GetAngles.srv b/srv/GetAngles.srv
index 6992699..9aa7d97 100644
--- a/srv/GetAngles.srv
+++ b/srv/GetAngles.srv
@@ -1,6 +1,3 @@
-
----
-
 float32 joint_1
 float32 joint_2
 float32 joint_3
diff --git a/srv/GripperStatus.srv b/srv/GripperStatus.srv
index 424572f..3887d75 100644
--- a/srv/GripperStatus.srv
+++ b/srv/GripperStatus.srv
@@ -1,3 +1,5 @@
 bool Status
 
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/PumpStatus.srv b/srv/PumpStatus.srv
new file mode 100644
index 0000000..3887d75
--- /dev/null
+++ b/srv/PumpStatus.srv
@@ -0,0 +1,5 @@
+bool Status
+
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/SetAngles.srv b/srv/SetAngles.srv
index 1ad93cd..6f8d877 100644
--- a/srv/SetAngles.srv
+++ b/srv/SetAngles.srv
@@ -7,4 +7,6 @@ float32 joint_6
 
 int8 speed
 
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file
diff --git a/srv/SetCoords.srv b/srv/SetCoords.srv
index b264760..e5b1e52 100644
--- a/srv/SetCoords.srv
+++ b/srv/SetCoords.srv
@@ -8,4 +8,6 @@ float32 rz
 int8 speed
 int8 model
 
----
\ No newline at end of file
+---
+
+bool Flag
\ No newline at end of file