Update teach.py

scripts/teach.py

@@ -11,6 +11,10 @@ class Master:
     def __init__(self):
         self.rs = RS30X.RS304MD()
         self.pub = rospy.Publisher("master_joint_state", JointState, queue_size=10)
+        for i in range(1,6):
+            self.rs.setTorque(i, True)
+            self.rs.setAngleInTime(i, self.rs.readAngle(i), 0)
+            rospy.sleep(0.01)
         for i in range(1,6):
             self.rs.setTorque(i, False)
             rospy.sleep(0.01)
@@ -33,8 +37,9 @@ def __init__(self):
         rospy.loginfo("servo initialized")
 
     def joint_callback(self, msg):
-        for i in range(1, 6):
-            self.rs.setAngle(i, msg.position[i-1])
+        if len(msg.position) > 0:
+            for i in range(1, 6):
+                self.rs.setAngle(i, msg.position[i-1])
         rospy.sleep(0.01)
 
     def shutdown(self):
@@ -52,13 +57,11 @@ def __init__(self):
     def joint_callback(self, msg):
         self.jointstate.name = ["joint{}".format(i) for i in range(1,6)]
         self.jointstate.position = []
-        for i in range(1,6):
-            self.jointstate.position.append(msg.position[i-1])
+        if len(msg.position) > 0:
+            for i in range(1,6):
+                self.jointstate.position.append(msg.position[i-1])
 
     def run(self):
-        #print self.jointstate
-        #return self.jointstate
-        #self.jointlog.write('/raspigibbon/master_joint_state', self.jointstate.name, self.jointstate.position, self.jointstate.header.stamp)
         self.jointlog.write('/raspigibbon/master_joint_state', self.jointstate)
 
     def close(self):
@@ -72,39 +75,35 @@ def run(self, msg):
         if not rospy.is_shutdown():
             js = JointState()
             js.name=["joint{}".format(i) for i in range(1,6)]
-            try:
+            if len(msg.position) > 0:
                 for i in range(1,6):
                     js.position.append(msg.position[i-1])
-            except:
-                pass
-            js.header.stamp=rospy.Time.now()
             self.pub.publish(js)
-            print js
-            rospy.sleep(0.01)
+            rospy.sleep(0.05)
 
 if __name__ == "__main__":
     rospy.init_node("raspigibbon_master_slave")
     print 'init node'
     master = Master()
-    timer = 14.0
+    timer = 10.0
     js = JointState()
     recoder = Recoder()
     start = time.time()
     print 'start timer'
 
-    while time.time() < (timer + start):
-        if not rospy.is_shutdown():
-            master.run()
-            recoder.run()
-    recoder.close()
-
-    slave = Slave()
-    rospy.on_shutdown(slave.shutdown)
     try:
-        if not rospy.is_shutdown():
-            player = Player()
-            for topic, msg, t in rosbag.Bag('jointlog.bag').read_messages():
-                player.run(msg)
+        while time.time() < (timer + start):
+            if not rospy.is_shutdown():
+                master.run()
+                recoder.run()
+        recoder.close()
+
+        slave = Slave()
+        rospy.on_shutdown(slave.shutdown)
+        player = Player()
+        for topic, msg, t in rosbag.Bag('jointlog.bag').read_messages():
+            player.run(msg)
+
     except rospy.ROSInterruptException:
         pass