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