rcommander_server.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('rcommander_core')
00002 
00003 import os.path
00004 import os
00005 
00006 import actionlib
00007 import rospy
00008 import pr2_interactive_manipulation.msg as pim 
00009 import geometry_msgs.msg as geo
00010 import sys
00011 
00012 #from srv import *
00013 from pr2_interactive_manipulation.srv import *
00014 import sm_thread_runner as smtr
00015 import graph_model as gm
00016 import point_tool as pt
00017 
00018 
00019 class PoseStampedScriptedActionServer:
00020 
00021     def __init__(self, action_name, path_to_action, robot):
00022         self.action_name = action_name
00023         self.path_to_action = path_to_action
00024         #rospy.loginfo('Starting server for %s with path %s' %(action_name, path_to_action))
00025         self.robot = robot
00026         #Setup ROS Action Server
00027         #self._action_name = action_name
00028         #self._as = actionlib.SimpleActionServer(self._action_name, pim.PoseStampedScriptedAction, execute_cb=self.execute_cb, auto_start=False)
00029         #self._as.start()
00030         #rospy.loginfo('%s server up!' % action_name)
00031 
00032     def setup_sm(self):
00033         #Setup state machine
00034         self.graph_model = gm.GraphModel.load(self.path_to_action, self.robot)
00035 
00036         #Find the first global node that is of the right type
00037         self.point_field_name = None
00038         for node_name in self.graph_model.global_nodes(None):
00039             node = self.graph_model.get_smach_state(node_name)
00040             if node.__class__ == pt.Point3DState:
00041                 self.point_field_name = node_name
00042         if self.point_field_name == None:
00043             raise RuntimeError('This statemachine doesn\'t have any nodes of type Point3DState')
00044 
00045     def execute(self, pose_stamped, actserver):
00046         self.setup_sm()
00047         r = rospy.Rate(30)
00048         position = [pose_stamped.pose.position.x, 
00049                     pose_stamped.pose.position.y, 
00050                     pose_stamped.pose.position.z]
00051         frame = pose_stamped.header.frame_id
00052         print 'Position', position, 'frame', frame
00053 
00054         self.graph_model.get_smach_state(self.point_field_name).set_info((position, frame))
00055         state_machine = self.graph_model.create_state_machine()
00056         rthread = smtr.ThreadRunSM(self.action_name, state_machine)
00057         rthread.start()
00058 
00059         #Do something
00060         while True:
00061             if actserver.is_preempt_requested():
00062                 actserver.set_preempted()
00063                 success = False
00064                 break
00065 
00066             if rthread.exception:
00067                 raise rthread.exception
00068 
00069             if rthread.outcome != None:
00070                 success = True
00071                 break
00072 
00073             if not rthread.isAlive():
00074                 raise RuntimeError("Thread died unexpectedly.")
00075 
00076             r.sleep()
00077 
00078         if success:
00079             state_machine_output = rthread.outcome
00080             #result = pim.PoseStampedScriptedResult(state_machine_output)
00081             result = pim.RunScriptResult(state_machine_output)
00082             rospy.loginfo("%s: succeeded with %s" % (self.action_name, state_machine_output))
00083             actserver.set_succeeded(result)
00084         else:
00085             actserver.set_aborted()
00086 
00087 
00088 # rviz talks to server, which loads and serves everything in a directory
00089 # assume all the actions take a click 
00090 class RCommanderServer:
00091 
00092     def __init__(self, robot):
00093         self.robot = robot
00094         self.action_dict = {}
00095         rospy.Service('list_rcommander_actions', ActionInfo, self.list_action_cb)
00096         self.actserv = actionlib.SimpleActionServer('run_rcommander_action', pim.RunScriptAction, execute_cb=self.execute_cb, auto_start=False)
00097         self.actserv.start()
00098 
00099     def execute_cb(self, goal):
00100         rospy.loginfo('Requested: group ' + goal.group_name + ' action: ' + goal.action_name)
00101         rospy.loginfo('waiting for click..')
00102         ps_msg = rospy.wait_for_message('/cloud_click_point', geo.PoseStamped)
00103         self.action_dict[goal.group_name][goal.action_name].execute(ps_msg, self.actserv)
00104 
00105     def load_actions_in_groups(self, action_groups):
00106         for group_name, path_name in action_groups:
00107             self.load_group(group_name, path_name)
00108 
00109     def load_group(self, group_name, path_name):
00110         rospy.loginfo('Loading group: ' + group_name)
00111         dirs = []#[d for d in os.listdir(path_name) if os.path.isdir(d)]
00112         for d in os.listdir(path_name):
00113             if os.path.isdir(os.path.join(path_name, d)):
00114                 dirs.append(d)
00115         rospy.loginfo('Found actions: ' + str(dirs))
00116         self.action_dict[group_name] = {}
00117         for action_pt in dirs:
00118             self.serve_action(action_pt, os.path.join(path_name, action_pt), group_name)
00119 
00120     def serve_action(self, action_name, action_path, group_name):
00121         #Actually, they're all pose stamped scripted actions...
00122         self.action_dict[group_name][action_name] = PoseStampedScriptedActionServer(action_name, action_path, self.robot)
00123 
00124     def list_action_cb(self, req):
00125         actions = self.action_dict[req.group_name].keys()
00126         actions.sort()
00127         return ActionInfoResponse(actions)
00128 
00129     #def start_server(self):
00130     #    pass
00131 
00132     #def stop_server(self):
00133     #    pass
00134 
00135     #def refresh_server(self):
00136     #    pass
00137 
00138 #class RCommanderGUI:
00139 
00140 #need to be robot specific
00141 #Can load from cmd line & gui 
00142 #cmd line version takes a path that contains everything you want to serve and serve it
00143 #gui allows cmd line too!
00144 def run(robot):
00145     from optparse import OptionParser
00146 
00147     p = OptionParser()
00148     p.add_option("-d", "--dir", dest="dir", action="append", default=[], help="directory to find saved actions from")
00149     p.add_option("-n", "--name", dest="name", action="append", default=[], help="name for action group")
00150     options, args = p.parse_args()
00151 
00152     server_man = RCommanderServer(robot)
00153     #print options.name, options.dir
00154     server_man.load_actions_in_groups(zip(options.name, options.dir))
00155     rospy.loginfo('server up!')
00156     rospy.spin()
00157 
00158 
00159 


rcommander
Author(s): Hai Nguyen (haidai@gmail.com)
autogenerated on Thu Nov 28 2013 11:46:34