cob_pick_place_action: cob_place_action

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import random
00019 from math import pi
00020 
00021 import rospy
00022 import actionlib
00023 from geometry_msgs.msg import PoseStamped
00024 from tf.transformations import *
00025 import cob_pick_place_action.msg
00026 
00027 def cob_place_action_client():
00028         # Creates the SimpleActionClient, passing the type of the action
00029         # (CobPlaceAction) to the constructor.
00030         place_action_client = actionlib.SimpleActionClient('cob_place_action', cob_pick_place_action.msg.CobPlaceAction)
00031 
00032         # Waits until the action server has started up and started
00033         # listening for goals.
00034         place_action_client.wait_for_server()
00035 
00036         # Creates a goal to send to the action server.
00037         goal = cob_pick_place_action.msg.CobPlaceGoal()
00038         #goal.object_class = 18
00039         #goal.object_name = "yellowsaltcube"
00040         goal.object_class = 50
00041         goal.object_name = "instantsoup"
00042 
00043         pose = PoseStamped()
00044 
00045         pose.header.stamp = rospy.Time.now()
00046         pose.header.frame_id = "base_footprint"
00047         #pose.pose.position.x = random.uniform(-0.8, -0.6)
00048         #pose.pose.position.y = random.uniform(-0.3,  0.3)
00049         #pose.pose.position.z = random.uniform( 0.8,  1.1)
00050         #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))
00051         pose.pose.position.x = -0.7
00052         pose.pose.position.y = -0.5
00053         pose.pose.position.z = 0.9
00054         pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,0,0)
00055         goal.destinations.append(pose)
00056 
00057 
00058         # Sends the goal to the action server.
00059         place_action_client.send_goal(goal)
00060 
00061         # Waits for the server to finish performing the action.
00062         finished_before_timeout=place_action_client.wait_for_result(rospy.Duration(300, 0))
00063 
00064         if finished_before_timeout:
00065                 state=place_action_client.get_state()
00066                 print "Action finished: %s"%state
00067         # Prints out the result of executing the action
00068         return state  # State after waiting for CobPlaceAction
00069 
00070 if __name__ == '__main__':
00071         try:
00072                 # Initializes a rospy node so that the SimpleActionClient can
00073                 # publish and subscribe over ROS.
00074                 rospy.init_node('CobPlaceAction_client_py')
00075                 result = cob_place_action_client()
00076         except rospy.ROSInterruptException:
00077                 print "program interrupted before completion"