Merge pull request #4 from dash-robotics/feature/integrate_state_machine

Add logic for hand over

Author: sohils

scripts/controller.py

@@ -1,5 +1,7 @@
 import numpy as np
 import rospy
+import time
+from collections import deque
 
 from std_msgs.msg import Float64, Empty
 from trac_ik_python.trac_ik import IK
@@ -18,6 +20,8 @@ def __init__(self):
         rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
         self.tf_listener = tf.TransformListener()
 
+        self.history = {'timestamp': deque(),
+                        'joint_feedback': deque()}
         # global variables
         self.current_joint_state = None
         self.current_gripper_state = None
@@ -56,6 +60,19 @@ def set_arm_joint_angles(self, joint_target):
         self.arm_pub.publish(joint_state)
 
     def get_joint_state(self, data):
+        # TODO: Change this when required
+        moveable_joints = [1,2,3]
+        # Add timestamp
+        self.history['timestamp'].append(time.time())
+        if(len(self.history['timestamp']) > 20):
+            self.history['timestamp'].popleft()
+
+        # Add Joint Feedback
+        joint_angles = np.array(data.position)[moveable_joints]
+        self.history['joint_feedback'].append(joint_angles)
+        if(len(self.history['joint_feedback']) > 20):
+            self.history['joint_feedback'].popleft()
+
         self.current_joint_state = data.position[0:5]
         self.current_gripper_state = data.position[7:9]
 

scripts/machine.py

@@ -4,13 +4,15 @@
 import rospy
 import smach
 import smach_ros
+import numpy as np
 
 from controller import Controller
 from geometry_msgs.msg import PoseStamped
 from dynamixel_workbench_msgs.srv import SetPID
 
 # Initialize controller
 ctrl = Controller()
+MIN_JOINT_MOTION_FOR_HAND_OVER = 0.1
 # define state Idle
 class Idle(smach.State):
     def __init__(self):
@@ -155,9 +157,17 @@ def __init__(self):
 
     def execute(self, userdata):
         ## Detect when to Open gripper
-        self.open_gripper()
         rospy.loginfo('Executing state OpenGripper')
-        return 'opened'
+        while(True):
+            joint_len = len(ctrl.history['joint_feedback'][0])
+            joint_sum = np.zeros(ctrl.history['joint_feedback'][0].shape)
+            for joint_feedback in ctrl.history['joint_feedback']:
+                joint_sum += joint_feedback
+            avg_joint_sum = joint_sum/joint_len
+            if(np.sum(avg_joint_sum) < MIN_JOINT_MOTION_FOR_HAND_OVER):
+                ctrl.open_gripper()
+                return 'opened'
+
 
 def main():
     rospy.init_node('attention_bot')