hdm_test_2011_initial.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  
00053     start_pose = Pose()
00054     
00055     start_pose.position.x = -3.0;
00056     start_pose.position.y = -0.2;
00057     start_pose.position.z = 1.02;
00058     start_pose.orientation.x = 0.0;
00059     start_pose.orientation.y = 0.0;
00060     start_pose.orientation.z = 0.0;
00061     start_pose.orientation.w = 0.0;
00062         
00063     start_twist = Twist()
00064     start_twist.linear.x = 0.0;
00065     start_twist.linear.y = 0.0;
00066     start_twist.linear.z = 0.0;
00067     start_twist.angular.x = 0.0;
00068     start_twist.angular.y = 0.0;
00069     start_twist.angular.z = 0.0;
00070         
00071     modelstate = gazebo.ModelState
00072     modelstate.model_name = "milk_box";
00073     modelstate.reference_frame = "world";
00074     modelstate.pose = start_pose;
00075     modelstate.twist = start_twist;
00076         
00077     try:    
00078         move_milk = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
00079         move_milk(modelstate)  
00080     except rospy.ServiceException, e:
00081         error_message = "%s"%e
00082         rospy.logerr("have you put the milk into the simulation?")
00083         
00084         #setmodelstate = gazebo
00085         
00086         #setmodelstate.request.model_state = modelstate
00087         
00088       
00089        
00090     handle_tray = sss.move("tray", "down", False)  
00091     handle_tray.wait()
00092     
00093     _goal=xmsg.ExecutionGoal()
00094     _goal.action="move"
00095     _goal.parameter="[0.5 -1.6 1.57]"
00096     _goal.priority=0
00097     client.send_goal(_goal)  
00098     client.wait_for_result()
00099     sss.say(["back to charging station, I am ready for new tasks"],False)
00100     
00101     return client.get_result()
00102 
00103 
00104 
00105 
00106 
00107 if __name__ == '__main__':
00108     try:
00109         # Initializes a rospy node so that the SimpleActionClient can
00110         # publish and subscribe over ROS.
00111         rospy.init_node('dm_client1')
00112         result = DM_client()
00113         rospy.loginfo('result %s',result)
00114         # print ("Result:" result)
00115     except rospy.ROSInterruptException:
00116         print "program interrupted before completion"


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