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()