def goal_cb(self, userdata, goal):
cc = ControllerClient()
cc.set_controller_names('left')
cc.switch_to_joint_mode()
cc.set_controller_names('right')
cc.switch_to_joint_mode()
goal = TuckArmsGoal()
goal.tuck_left = userdata['tuck_left']
goal.tuck_right = userdata['tuck_right']
rospy.loginfo("Sending tuck arms goal and waiting for result")
return goal
def main():
# as simulation starts, arm controller are probably not ready without this delay
rospy.sleep(7)
action_name = 'tuck_arms'
goal = TuckArmsGoal()
goal.tuck_left = True
goal.tuck_right = True
tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
rospy.loginfo('Waiting for action server to start')
tuck_arm_client.wait_for_server()
rospy.loginfo('Sending goal to action server')
tuck_arm_client.send_goal_and_wait(goal)
def __init__(self):
self._last_move = 0
self._last_trans_pose = None
self.tf = tf.TransformListener()
self.tuck_arm_client = actionlib.SimpleActionClient("catch_me_tuck_arms", TuckArmsAction)
rospy.loginfo("Waiting for tuck arms action server to start")
self.tuck_arm_client.wait_for_server()
rospy.loginfo("Connected to tuck arms action server")
goal = TuckArmsGoal()
goal.tuck_left = True
goal.tuck_right = True
self.tuck_arm_client.send_goal(goal)
rospy.loginfo("Waiting for catch_me_destination_service")
rospy.wait_for_service("catch_me_destination_service")
rospy.loginfo("Connected to catch_me_destination_service")
# self.destination_service = rospy.ServiceProxy('catch_me_destination_service', srv.DestinationService)
rospy.loginfo("Waiting for move_base action to come up")
self.base_client = actionlib.SimpleActionClient("move_base_local", MoveBaseAction)
# self.base_client.wait_for_server()
rospy.loginfo("Connected to move_base action server")
rospy.loginfo("Starting head-searching action")
self.head_s_client = actionlib.SimpleActionClient("local_search", LocalSearchAction)
# self.head_s_client.wait_for_server()
rospy.loginfo("Head-search action client started")
rospy.loginfo("Starting base_rotate action")
self.base_r_client = actionlib.SimpleActionClient("base_rotate", BaseRotateAction)
# self.base_r_client.wait_for_server()
rospy.loginfo("Base_rotate action client started")
rospy.loginfo("Starting kill action")
self.kill_client = actionlib.SimpleActionClient("kill", KillAction)
self.kill_client.wait_for_server()
rospy.loginfo("Kill action client started")
# Don't start until arms are tucked!
self.tuck_arm_client.wait_for_result(rospy.Duration(30.0))
threading.Timer(0, self.follow_marker).start()
def tuck_arms (self, tuck):
self.tuck_arms_client.wait_for_server(rospy.Duration(10.0))
print "waiting for server"
g=TuckArmsGoal()
g.tuck_left=tuck
g.tuck_right=tuck
self.tuck_arms_client.send_goal_and_wait(g, rospy.Duration(30.0), rospy.Duration(5.0))
status=self.tuck_arms_client.get_result()
print (status)
if tuck:
if status.tuck_left and status.tuck_right:
print"Both arms tucked successfully"
return True
elif not tuck:
if not status.tuck_left and not status.tuck_right:
print "Both arms untucked successfully"
return True
print"Tucking arms action client failed"
return False
def main():
action_name = 'tuck_arms'
quit_when_finished = False
# check for command line arguments, and send goal to action server if
# required
myargs = rospy.myargv()[1:]
if len(myargs):
goal = TuckArmsGoal()
goal.tuck_left = True
goal.tuck_right = True
opts, args = getopt.getopt(myargs, 'hql:r:', ['quit', 'left', 'right'])
for arm, action in opts:
if arm in ('-l', '--left'):
if action in ('tuck', 't'):
goal.tuck_left = True
elif action in ('untuck', 'u'):
goal.tuck_left = False
else:
rospy.logerr('Invalid action for right arm: %s' % action)
rospy.signal_shutdown("ERROR")
if arm in ('-r', '--right'):
if action in ('tuck', 't'):
goal.tuck_right = True
elif action in ('untuck', 'u'):
goal.tuck_right = False
else:
rospy.logerr('Invalid action for left arm: %s' % action)
rospy.signal_shutdown("ERROR")
if arm in ('--quit', '-q'):
quit_when_finished = True
if arm in ('--help', '-h'):
usage()
tuck_arm_client = actionlib.SimpleActionClient(action_name,
TuckArmsAction)
rospy.logdebug('Waiting for action server to start')
tuck_arm_client.wait_for_server(rospy.Duration())
rospy.logdebug('Sending goal to action server')
tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0),
rospy.Duration(5.0))
if quit_when_finished:
rospy.signal_shutdown("Quitting")
import trajectory_msgs
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import actionlib
from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction
from geometry_msgs.msg import PointStamped
from pr2_common_action_msgs.msg import TuckArmsGoal, TuckArmsAction
if __name__ == '__main__':
rospy.init_node('tuck_arms_and_scan')
rospy.loginfo("Tucking arms...")
tuck_arm_client = actionlib.SimpleActionClient("tuck_arms", TuckArmsAction)
goal = TuckArmsGoal()
goal.tuck_left = True
goal.tuck_right = True
tuck_arm_client.wait_for_server(rospy.Duration(5.0))
tuck_arm_client.send_goal(goal)
tuck_arm_client.wait_for_result(rospy.Duration.from_sec(30.0))
#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
rospy.loginfo("Head scanning... ")
goalH = PointHeadGoal()
goalH.target.header.frame_id = "base_link"
goalH.target.point.x = 1.7
goalH.target.point.y = -3.0
goalH.target.point.z = 0.0
goalH.pointing_frame = "high_def_frame"
def execute_cb(goal):
rospy.loginfo("Received a goal")
# check for network cable
if network_connected:
server.set_aborted(None, "Dangerous to navigate with network cable connected.")
return
# check for power cable
if ac_power_connected:
server.set_aborted(None, "Dangerous to navigate with AC power cable connected.")
return
# check for power cable
# TODO
# start the arms tucking
tuck_arms_client = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction)
tuck_arms_client.wait_for_server()
tuck_goal = TuckArmsGoal()
tuck_goal.tuck_left=True
tuck_goal.tuck_right=True
tuck_arms_client.send_goal(tuck_goal)
# start the torso lowering
torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
torso_client.wait_for_server()
torso_down_goal = SingleJointPositionGoal()
torso_down_goal.position = 0
torso_client.send_goal(torso_down_goal)
# configure the tilting laser
if not set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
server.set_aborted(MoveBaseResult(), "Couldn't set the profile on the laser")
return
configure_laser()
configure_head()
# wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
if server.is_preempt_requested():
if not server.is_new_goal_available():
tuck_arms_client.cancel_goal()
tuck_state = tuck_arms_client.get_state()
if tuck_state != GoalStatus.SUCCEEDED:
if tuck_state == GoalStatus.PREEMPTED:
server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
elif tuck_state == GoalStatus.ABORTED:
server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
return
# Now everything should be ready so send the navigation goal.
move_base_client.send_goal(goal, None, None, feedback_cb)
while not move_base_client.wait_for_result(rospy.Duration(0.1)):
#in the unlikely event of network cable being plugged in while moving, stop moving.
if network_connected:
move_base_client.cancel_goal()
server.set_aborted(None, "Dangerous to navigate with network cable connected.")
return
#in the unlikely event of ac power cable being plugged in while moving, stop moving.
if ac_power_connected:
move_base_client.cancel_goal()
server.set_aborted(None, "Dangerous to navigate with ac power cable connected.")
return
if server.is_preempt_requested():
if not server.is_new_goal_available():
rospy.loginfo("Preempt requested without new goal, cancelling move_base goal.")
move_base_client.cancel_goal()
server.set_preempted(MoveBaseResult(), "Got preempted by a new goal")
return
terminal_state = move_base_client.get_state()
result = move_base_client.get_result()
if terminal_state == GoalStatus.PREEMPTED:
server.set_preempted(result)
elif terminal_state == GoalStatus.SUCCEEDED:
server.set_succeeded(result)
elif terminal_state == GoalStatus.ABORTED:
server.set_aborted(result)
else:
server.set_aborted(result, "Unknown result from move_base")
def execute_cb(goal):
rospy.loginfo("Received a goal")
# check for network cable
if network_connected:
server.set_aborted(
None, "Dangerous to navigate with network cable connected.")
return
# check for power cable
if ac_power_connected:
server.set_aborted(
None, "Dangerous to navigate with AC power cable connected.")
return
# check for power cable
# TODO
# start the arms tucking
tuck_arms_client = actionlib.SimpleActionClient('tuck_arms',
TuckArmsAction)
tuck_arms_client.wait_for_server()
tuck_goal = TuckArmsGoal()
tuck_goal.tuck_left = True
tuck_goal.tuck_right = True
tuck_arms_client.send_goal(tuck_goal)
# start the torso lowering
torso_client = actionlib.SimpleActionClient(
'torso_controller/position_joint_action', SingleJointPositionAction)
torso_client.wait_for_server()
torso_down_goal = SingleJointPositionGoal()
torso_down_goal.position = 0
torso_client.send_goal(torso_down_goal)
# configure the tilting laser
#if not set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
if not set_tilt_profile([0.1, 0.1, 0.1], [0.0, 2, 4]):
server.set_aborted(MoveBaseResult(),
"Couldn't set the profile on the laser")
return
configure_laser()
configure_head()
# wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
if server.is_preempt_requested():
if not server.is_new_goal_available():
tuck_arms_client.cancel_goal()
tuck_state = tuck_arms_client.get_state()
if tuck_state != GoalStatus.SUCCEEDED:
if tuck_state == GoalStatus.PREEMPTED:
server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
elif tuck_state == GoalStatus.ABORTED:
server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
return
# Now everything should be ready so send the navigation goal.
move_base_client.send_goal(goal, None, None, feedback_cb)
while not move_base_client.wait_for_result(rospy.Duration(0.1)):
#in the unlikely event of network cable being plugged in while moving, stop moving.
if network_connected:
move_base_client.cancel_goal()
server.set_aborted(
None, "Dangerous to navigate with network cable connected.")
return
#in the unlikely event of ac power cable being plugged in while moving, stop moving.
if ac_power_connected:
move_base_client.cancel_goal()
server.set_aborted(
None, "Dangerous to navigate with ac power cable connected.")
return
if server.is_preempt_requested():
if not server.is_new_goal_available():
rospy.loginfo(
"Preempt requested without new goal, cancelling move_base goal."
)
move_base_client.cancel_goal()
server.set_preempted(MoveBaseResult(),
"Got preempted by a new goal")
return
terminal_state = move_base_client.get_state()
result = move_base_client.get_result()
if terminal_state == GoalStatus.PREEMPTED:
server.set_preempted(result)
elif terminal_state == GoalStatus.SUCCEEDED:
server.set_succeeded(result)
elif terminal_state == GoalStatus.ABORTED:
server.set_aborted(result)
else:
server.set_aborted(result, "Unknown result from move_base")
def tuck_arm(self, msg):
tuck_goal = TuckArmsGoal()
if (msg.data == 'right' or msg.data == 'left'):
recover = True
recover_goal = JointTrajectoryActionGoal()
else:
recover = False
if msg.data == 'right':
tuck_goal.tuck_left = False
tuck_goal.tuck_right = True
print "Tucking Right Arm Only"
self.wt_log_out.publish(data="Tucking Right Arm Only")
recover_goal.goal.trajectory.joint_names = self.joint_state_l.joint_names
recover_position = self.joint_state_l.actual
recover_position.time_from_start = rospy.Duration(5)
recover_publisher = self.joints_out_l
elif msg.data == 'left':
tuck_goal.tuck_left = True
tuck_goal.tuck_right = False
print "Tucking Left Arm Only"
self.wt_log_out.publish(data="Tucking Left Arm Only")
recover_goal.goal.trajectory.joint_names = self.joint_state_r.joint_names
recover_position = self.joint_state_r.actual
recover_position.time_from_start = rospy.Duration(5)
recover_publisher = self.joints_out_r
elif msg.data == 'both':
tuck_goal.tuck_left = True
tuck_goal.tuck_right = True
print "Tucking Both Arms"
self.wt_log_out.publish(data="Tucking Both Arms")
else:
print "Bad input to Tuck Arm Intermediary"
finished_within_time = False
self.tuck.send_goal(tuck_goal)
finished_within_time = self.tuck.wait_for_result(rospy.Duration(30))
if not (finished_within_time):
self.tuck.cancel_goal()
self.wt_log_out.publish(data="Timed out tucking right arm")
rospy.loginfo("Timed out tucking right arm")
else:
state = self.tuck.get_state()
success = (state == 'SUCCEEDED')
if (success):
rospy.loginfo("Action Finished: %s" %state)
self.wt_log_out.publish(data="Tuck Right Arm: Succeeded")
else:
rospy.loginfo("Action failed: %s" %state)
self.wt_log_out.publish(data="Tuck Right Arm: Failed: %s" %state)
if (recover):
recover_goal.goal.trajectory.points.append(recover_position)
recover_publisher.publish(recover_goal);
print "Sending Recovery Goal to restore initial position of non-tucked arm"
self.wt_log_out.publish(data="Sending Recovery Goal to restore initial position of non-tucked arm")
def tuck_arm(self, msg):
tuck_goal = TuckArmsGoal()
if (msg.data == 'right' or msg.data == 'left'):
recover = True
recover_goal = JointTrajectoryActionGoal()
else:
recover = False
if msg.data == 'right':
tuck_goal.tuck_left = False
tuck_goal.tuck_right = True
print "Tucking Right Arm Only"
self.wt_log_out.publish(data="Tucking Right Arm Only")
recover_goal.goal.trajectory.joint_names = self.joint_state_l.joint_names
recover_position = self.joint_state_l.actual
recover_position.time_from_start = rospy.Duration(5)
recover_publisher = self.joints_out_l
elif msg.data == 'left':
tuck_goal.tuck_left = True
tuck_goal.tuck_right = False
print "Tucking Left Arm Only"
self.wt_log_out.publish(data="Tucking Left Arm Only")
recover_goal.goal.trajectory.joint_names = self.joint_state_r.joint_names
recover_position = self.joint_state_r.actual
recover_position.time_from_start = rospy.Duration(5)
recover_publisher = self.joints_out_r
elif msg.data == 'both':
tuck_goal.tuck_left = True
tuck_goal.tuck_right = True
print "Tucking Both Arms"
self.wt_log_out.publish(data="Tucking Both Arms")
else:
print "Bad input to Tuck Arm Intermediary"
finished_within_time = False
self.tuck.send_goal(tuck_goal)
finished_within_time = self.tuck.wait_for_result(rospy.Duration(30))
if not (finished_within_time):
self.tuck.cancel_goal()
self.wt_log_out.publish(data="Timed out tucking right arm")
rospy.loginfo("Timed out tucking right arm")
else:
state = self.tuck.get_state()
success = (state == 'SUCCEEDED')
if (success):
rospy.loginfo("Action Finished: %s" % state)
self.wt_log_out.publish(data="Tuck Right Arm: Succeeded")
else:
rospy.loginfo("Action failed: %s" % state)
self.wt_log_out.publish(data="Tuck Right Arm: Failed: %s" %
state)
if (recover):
recover_goal.goal.trajectory.points.append(recover_position)
recover_publisher.publish(recover_goal)
print "Sending Recovery Goal to restore initial position of non-tucked arm"
self.wt_log_out.publish(
data=
"Sending Recovery Goal to restore initial position of non-tucked arm"
)
def goal_cb(self, userdata, goal):
goal = TuckArmsGoal()
goal.tuck_left = userdata['tuck_left']
goal.tuck_right = userdata['tuck_right']
rospy.loginfo("Sending tuck arms goal and waiting for result")
return goal
