def torso_down(self, wait=False):
self.torso_client = SimpleActionClient(
'/torso_controller/position_joint_action',
SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(
rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Torso movement goal failed with error code: " +
str(state))
def __init__(self):
ns = 'robot/head/head_action'
self._client = actionlib.SimpleActionClient(ns,
SingleJointPositionAction)
self._goal = SingleJointPositionGoal()
# Wait 10 Seconds for the head action server to start or exit
if not self._client.wait_for_server(rospy.Duration(10.0)):
rospy.logerr("Exiting - Head Action Server Not Found")
rospy.signal_shutdown("Action Server not found")
sys.exit(1)
self.clear()
def clear(self):
self._goal = SingleJointPositionGoal()