def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
rospy.loginfo('Gripper in position')
else:
rospy.loginfo('Gripper positioning failed')
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
rospy.loginfo('Arm tucked')
return True
else:
rospy.loginfo('Failed to tuck arm')
return False
def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
rospy.loginfo('Gripper in position')
else:
rospy.loginfo('Gripper positioning failed')
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
rospy.loginfo('Arm tucked')
return True
else:
rospy.loginfo('Failed to tuck arm')
return False
def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success: print "Gripper in position"
else: print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm tucked"
return True
else:
print "Failed to tuck arm"
return False
def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
print "Gripper in position"
else:
print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm tucked"
return True
else:
print "Failed to tuck arm"
return False
def swat_object(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
print "Gripper in position"
else:
print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm UP"
else:
print "Failed to raise arm"
goal.target_joints = [0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm ready to swat"
else:
print "Failed to position the arm for swatting"
t = rospy.Time.now()
if self.fileOut != None:
self.fileOut.write("Swatting action at " + str(t.secs) + "." + str(t.nsecs) + "\n")
# self.sh_pan_speed_srv(3.0);
self.inProgress = True
goal.target_joints = [-0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm just swatted"
else:
print "Arm failed to swat"
def swat_object(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success: print "Gripper in position"
else: print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success: print "Arm UP"
else: print "Failed to raise arm"
goal.target_joints = [0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success: print "Arm ready to swat"
else: print "Failed to position the arm for swatting"
t = rospy.Time.now()
if (self.fileOut != None):
self.fileOut.write("Swatting action at " + str(t.secs) + "." +
str(t.nsecs) + "\n")
#self.sh_pan_speed_srv(3.0);
self.inProgress = True
goal.target_joints = [-0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success: print "Arm just swatted"
else: print "Arm failed to swat"
