cob_pick_place_action: cob_place_action

Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('cob_pick_place_action')
00004 import rospy
00005 import actionlib
00006 import random
00007 from math import pi
00008 from tf.transformations import *
00009 
00010 from geometry_msgs.msg import PoseStamped
00011 import cob_pick_place_action.msg 
00012 
00013 def cob_place_action_client():
00014         # Creates the SimpleActionClient, passing the type of the action
00015         # (CobPlaceAction) to the constructor.
00016         place_action_client = actionlib.SimpleActionClient('cob_place_action', cob_pick_place_action.msg.CobPlaceAction)
00017 
00018         # Waits until the action server has started up and started
00019         # listening for goals.
00020         place_action_client.wait_for_server()
00021         
00022         # Creates a goal to send to the action server.
00023         goal = cob_pick_place_action.msg.CobPlaceGoal()
00024         goal.object_class = 18
00025         goal.object_name = "yellowsaltcube"
00026         #goal.object_class = 50
00027         #goal.object_name = "instantsoup"
00028         
00029         pose = PoseStamped()
00030         
00031         pose.header.stamp = rospy.Time.now()
00032         pose.header.frame_id = "base_footprint"
00033         #pose.pose.position.x = random.uniform(-0.8, -0.6)
00034         #pose.pose.position.y = random.uniform(-0.3,  0.3)
00035         #pose.pose.position.z = random.uniform( 0.8,  1.1)
00036         #pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2)) 
00037         pose.pose.position.x = -0.7 
00038         pose.pose.position.y = 0.0  
00039         pose.pose.position.z = 0.85
00040         pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,0,0) 
00041         goal.destinations.append(pose)
00042         
00043         
00044         # Sends the goal to the action server.
00045         place_action_client.send_goal(goal)
00046 
00047         # Waits for the server to finish performing the action.
00048         finished_before_timeout=place_action_client.wait_for_result(rospy.Duration(300, 0))
00049 
00050         if finished_before_timeout:
00051                 state=place_action_client.get_state()
00052                 print "Action finished: %s"%state
00053         # Prints out the result of executing the action
00054         return state  # State after waiting for CobPlaceAction
00055 
00056 if __name__ == '__main__':
00057         try:
00058                 # Initializes a rospy node so that the SimpleActionClient can
00059                 # publish and subscribe over ROS.
00060                 rospy.init_node('CobPlaceAction_client_py')
00061                 result = cob_place_action_client()
00062         except rospy.ROSInterruptException:
00063                 print "program interrupted before completion"