Some small changes in const poses

Author: ykarmesh

scripts/controller.py

@@ -33,20 +33,20 @@ def __init__(self):
         self.HOME_POS_MANIPULATOR_01 = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
         self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -0.142, 0.0]
         self.HOME_POS_CAMERA_01 = [0.0, 0.698]
-        self.HOME_POS_CAMERA_02 = [0.698, 0.0]
+        self.HOME_POS_CAMERA_02 = [-0.523, -0.523]
         self.IK_POSITION_TOLERANCE = 0.01
         self.IK_ORIENTATION_TOLERANCE = np.pi/9
         self.MIN_CLOSING_GAP = 0.002
 
     def set_camera_angles(self, angles):
         pan_msg = Float64()
         pan_msg.data = angles[0]
-        rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
+        rospy.loginfo('Going to camera pan: {} rad'.format(angles[0]))
         self.pan_pub.publish(pan_msg)
 
         tilt_msg = Float64()
         tilt_msg.data = angles[1]
-        rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
+        rospy.loginfo('Going to camera tilt: {} rad'.format(angles[1]))
         self.tilt_pub.publish(tilt_msg)
         rospy.sleep(4)
 
@@ -57,7 +57,7 @@ def set_arm_joint_angles(self, joint_target):
 
     def get_joint_state(self, data):
         self.current_joint_state = data.position[0:5]
-        self.current_gripper_state = data.positions[7:9]
+        self.current_gripper_state = data.position[7:9]
 
     def open_gripper(self):
         empty_msg = Empty()

scripts/ik_control.py

@@ -59,8 +59,8 @@ def get_joint_state(data):
     global current_joint_state
     global current_gripper_state
     # if len(data.position) == 9:
-    current_gripper_state = data.positions[7:9]
-    current_joint_state = data.positions[0:5]
+    current_gripper_state = data.position[7:9]
+    current_joint_state = data.position[0:5]
 
 def open_gripper(pub):
     empty_msg = Empty()
@@ -129,7 +129,7 @@ def main():
 
     # Homing of all servos
     home_joint = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
-    #home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
+    # home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
     set_arm_joint(arm_pub, home_joint)
     close_gripper(gripper_close_pub)
     set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*40.0) #, ck)

scripts/machine.py

@@ -6,6 +6,7 @@
 import smach_ros
 
 from controller import Controller
+from geometry_msgs.msg import PoseStamped
 
 # Initialize controller
 ctrl = Controller()
@@ -98,25 +99,24 @@ def execute(self, userdata):
 class FindAttention(smach.State):
 
     def __init__(self):
-        smach.State.__init__(self, outcomes=['giveTool'], output_keys=['frame_name'])
+        smach.State.__init__(self, outcomes=['giveTool'], output_keys=['head_pose'])
 
     def execute(self, userdata):
         ## Call service to get the frame id of the hand centroid
-        ## TODO: change the name of the variable frame_name
-        userdata.frame_name = "random"
+        posestamped = rospy.wait_for_message("/openface2/head_pose", PoseStamped)
         rospy.loginfo('Executing state FindAttention')
         return 'giveTool'
 
 # define state IK2
 class IK2(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['foundIK'], input_keys=['frame_name'])
+        smach.State.__init__(self, outcomes=['foundIK'], input_keys=['head_pose'])
 
     def execute(self, userdata):
         ## Wait till IK not found. Change tolerance and call compute IK again
         ## Break go to pose into find pose and compute IK functions
         ## Make a different function with gripper pointing upwards and call it from here
-        success = ctrl.go_to_pose(userdata.frame_name)
+        success = ctrl.go_to_pose(userdata.head_pose)
         # if not success
         rospy.loginfo('Executing state IK2')
         return 'foundIK'