hdm_test_2011.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('srs_decision_making')
00004 import rospy
00005 
00006 # Brings in the SimpleActionClient
00007 import actionlib
00008 
00009 # Brings in the messages used by the SRS DM action, including the
00010 # goal message and the result message.
00011 import srs_decision_making.msg as xmsg
00012 # include script server, to move the robot
00013 from simple_script_server import simple_script_server
00014 sss = simple_script_server()
00015 
00016 import geometry_msgs.msg as geomery
00017 import gazebo.msg as gazebo
00018 from gazebo.srv import SetModelState
00019 # msg imports
00020 from geometry_msgs.msg import *
00021 
00022 from cob_object_detection_msgs.srv import *
00023 from cob_object_detection_msgs.msg import *
00024 from gazebo.srv import *
00025 import gazebo.msg as gazebo
00026 
00027 
00028 #! /usr/bin/env python
00029 
00030 import roslib; roslib.load_manifest('srs_decision_making')
00031 import rospy
00032 
00033 # Brings in the SimpleActionClient
00034 import actionlib
00035 
00036 # Brings in the messages used by the SRS DM action, including the
00037 # goal message and the result message.
00038 import srs_decision_making.msg as xmsg
00039 
00040 def DM_client():
00041     # Creates the SimpleActionClient, passing the type of the action
00042     # constructor.
00043     client = actionlib.SimpleActionClient('srs_decision_making_actions', xmsg.ExecutionAction)
00044 
00045     # Waits until the action server has started up and started
00046     # listening for goals.
00047     client.wait_for_server()
00048     
00049     # creates action sequence for the action server
00050     
00051     # Creates a goal to send to the action server.
00052     _goal=xmsg.ExecutionGoal()
00053     _goal.action="move"
00054     _goal.parameter="[0.5 -1.6 1.57]"
00055     _goal.priority=0
00056     client.send_goal(_goal)  
00057     client.wait_for_result()
00058     print("ready to start")
00059     sss.wait_for_input()
00060 
00061  
00062     _goal.action="get"# msg imports
00063     _goal.parameter="milk"
00064     _goal.priority=0
00065     client.send_goal(_goal)  
00066     client.wait_for_result()
00067     print("milk ready")
00068     sss.wait_for_input()
00069 
00070 
00071     _goal.action="move"
00072     _goal.parameter="[1.47 -0.7 0.75]"
00073     _goal.priority=0
00074     client.send_goal(_goal)  
00075     client.wait_for_result()
00076     print("milk delivered")
00077     sss.say(["Here is your milk, Please help yourself."],False)
00078     sss.wait_for_input()
00079     
00080     
00081     start_pose = Pose()
00082     
00083     start_pose.position.x = -3.0;
00084     start_pose.position.y = -0.2;
00085     start_pose.position.z = 1.02;
00086     start_pose.orientation.x = 0.0;
00087     start_pose.orientation.y = 0.0;
00088     start_pose.orientation.z = 0.0;
00089     start_pose.orientation.w = 0.0;
00090         
00091     start_twist = Twist()
00092     start_twist.linear.x = 0.0;
00093     start_twist.linear.y = 0.0;
00094     start_twist.linear.z = 0.0;
00095     start_twist.angular.x = 0.0;
00096     start_twist.angular.y = 0.0;
00097     start_twist.angular.z = 0.0;
00098         
00099     modelstate = gazebo.ModelState
00100     modelstate.model_name = "milk_box";
00101     modelstate.reference_frame = "world";
00102     modelstate.pose = start_pose;
00103     modelstate.twist = start_twist;
00104         
00105         
00106     move_milk = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
00107         
00108         #setmodelstate = gazebo
00109         
00110         #setmodelstate.request.model_state = modelstate
00111         
00112     move_milk(modelstate)    
00113        
00114     print("confirm milk has been taken")
00115     sss.wait_for_input()
00116     client.wait_for_result()
00117     handle_tray = sss.move("tray", "down", False)  
00118     handle_tray.wait()
00119     
00120     _goal.action="move"
00121     _goal.parameter="[1 -1.6 1.57]"
00122     _goal.priority=0
00123     client.send_goal(_goal)  
00124     #client.wait_for_result()
00125     print("back to charge")
00126     
00127     return client.get_result()
00128 
00129 
00130 
00131 
00132 
00133 if __name__ == '__main__':
00134     try:
00135         # Initializes a rospy node so that the SimpleActionClient can
00136         # publish and subscribe over ROS.
00137         rospy.init_node('dm_client1')
00138         result = DM_client()
00139         rospy.loginfo('result %s',result)
00140         # print ("Result:" result)
00141     except rospy.ROSInterruptException:
00142         print "program interrupted before completion"


srs_decision_making
Author(s): Renxi Qiu
autogenerated on Sun Jan 5 2014 12:08:52