def set_torso_state(self, jval, wait=False): """ Sets goal for torso using provided value""" if not (type(jval) is list): jval = [jval] if len(jval) == 0: rospy.logwarn("No torso target given!") self.torso_done = True return # HACK: it looks like with a value outside this range the actionserver doesn't return.. if jval[0] < 0.012: jval[0] = 0.2 if jval[0] > 0.3: jval[0] = 0.3 self.torso_done = False goal = SingleJointPositionGoal() goal.position = jval[0] goal.min_duration = rospy.Duration(self.time_to_reach) self.torso_client.send_goal(goal, done_cb=self.__torso_done_cb()) if wait: self.torso_client.wait_for_result()
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 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 clear(self): self._goal = SingleJointPositionGoal()