From 91019e5ce2e8943afabb37aaac302c34abb649e0 Mon Sep 17 00:00:00 2001
From: Chris Paxton <cpaxton3@jhu.edu>
Date: Wed, 5 Apr 2017 14:51:43 -0700
Subject: [PATCH] fix issues with new polymorphic gripper manager

---
 .../src/gripper_manager/costar_gripper.py     |  9 ++++-
 .../src/gripper_robotiq/c_model_server.py     |  4 +-
 .../src/gripper_robotiq/s_model_server.py     | 40 +++++--------------
 3 files changed, 19 insertions(+), 34 deletions(-)

diff --git a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py
index 669e0090..5673d6c3 100644
--- a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py
+++ b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py
@@ -1,4 +1,7 @@
-from costar_component import MyComponent
+from costar_component import CostarComponent
+from os.path import join
+import rospy
+from std_srvs.srv import Empty
 
 # The CoSTAR Gripper component contains a lot less special logic than the
 # CoSTAR Arm does. It mostly just has a few tools.
@@ -15,7 +18,9 @@ def __init__(self,
       *args, **kwargs):
 
     self.verbose = verbose
-    self.predicator = GripperPredicator(start_subscriber=False,publish_predicates=True,
+    self.predicator = GripperPredicatorType(
+        start_subscriber=False,
+        publish_predicates=True,
         gripper_name=name)
 
     self.sub = rospy.Subscriber(input_topic, InputMsgType, self.status_cb)
diff --git a/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py b/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py
index f0f99439..4a9e1f72 100644
--- a/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py
+++ b/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py
@@ -39,8 +39,6 @@
 import rospy
 from robotiq_c_model_control.msg import _CModel_robot_output  as outputMsg
 from robotiq_c_model_control.msg import _CModel_robot_input  as inputMsg
-from os.path import join
-from std_srvs.srv import Empty
 from predicator_robotiq import CModelPredicator
 
 class SimpleCModelServer(CostarGripper):
@@ -65,7 +63,7 @@ def getDefaultMsg(cls):
         return command
 
     def activate(self,msg=None):
-        self.command = SimpleCModelServer.getDefaultMsg()
+        self.command = self.getDefaultMsg()
         self.pub.publish(self.command)
         return []
 
diff --git a/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py b/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py
index 2dfaa612..00a52a08 100644
--- a/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py
+++ b/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py
@@ -34,44 +34,26 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
+from gripper_manager import CostarGripper
+
 import rospy
 from robotiq_s_model_control.msg import _SModel_robot_output  as outputMsg
 from robotiq_s_model_control.msg import _SModel_robot_input  as inputMsg
-from os.path import join
-from std_srvs.srv import Empty
 from predicator_robotiq import SModelPredicator
 
-class SimpleSModelServer:
+class SimpleSModelServer(CostarGripper):
 
-    def __init__(self,ns="/costar/gripper",verbose=False)
-        super(SimpleCModelServer, self).__init__(
+    def __init__(self, ns="/costar/gripper", verbose=False):
+        super(SimpleSModelServer, self).__init__(
                 "s_model",
-                input_topic="CModelRobotInput",
-                output_topic="CModelRobotOutput",
-                InputMsgType=inputMsg.CModel_robot_input,
-                OutputMsgType=outputMsg.CModel_robot_output,
-                GripperPredicatorType=CModelPredicator,
+                input_topic="SModelRobotInput",
+                output_topic="SModelRobotOutput",
+                InputMsgType=inputMsg.SModel_robot_input,
+                OutputMsgType=outputMsg.SModel_robot_output,
+                GripperPredicatorType=SModelPredicator,
                 ns=ns,
                 verbose=verbose)
 
-        self.sub = rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, self.status_cb)
-        self.pub = rospy.Publisher('SModelRobotOutput', outputMsg.SModel_robot_output)
-        self.open = rospy.Service(join(ns,"open"), Empty, self.open_gripper)
-        self.close = rospy.Service(join(ns,"close"), Empty, self.close_gripper)
-        self.wide_mode_srv = rospy.Service(join(ns,"wide_mode"), Empty, self.wide_mode)
-        self.pinch_mode_srv = rospy.Service(join(ns,"pinch_mode"), Empty, self.pinch_mode)
-        self.basic_mode_srv = rospy.Service(join(ns,"basic_mode"), Empty, self.basic_mode)
-        self.scissor_mode_srv = rospy.Service(join(ns,"scissor_mode"), Empty, self.scissor_mode)
-        self.reactivate_srv = rospy.Service(join(ns,"activate"), Empty, self.activate)
-        self.reset_srv = rospy.Service(join(ns,"reset"), Empty, self.reset)
-        self.command = getDefaultMsg()
-
-        self.predicator = SModelPredicator(start_subscriber=False,publish_predicates=True,gripper_name="s_model")
-
-        self.activated = True;
-
-        self.activate()
-
     def getDefaultMsg(cls):
         command = outputMsg.SModel_robot_output();
         command.rACT = 1
@@ -81,7 +63,7 @@ def getDefaultMsg(cls):
         return command
 
     def activate(self,msg=None):
-        self.command = SimpleSModelServer.getDefaultMsg()
+        self.command = self.getDefaultMsg()
         self.pub.publish(self.command)
         return []