moveit_commander_cmdline.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 import rospy
00005 import readline
00006 import sys
00007 import os
00008 import signal 
00009 
00010 import argparse
00011 from moveit_commander import MoveGroupCommandInterpreter, MoveGroupInfoLevel, roscpp_initialize, roscpp_shutdown
00012 
00013 class bcolors:
00014     HEADER = '\033[95m'
00015     OKBLUE = '\033[94m'
00016     OKGREEN = '\033[92m'
00017     WARNING = '\033[93m'
00018     FAIL = '\033[91m'
00019     ENDC = '\033[0m'
00020 
00021 class SimpleCompleter(object):
00022     
00023     def __init__(self, options):
00024         self.options = options
00025 
00026     def set_options(self, options):
00027         self.options = options
00028 
00029     def complete(self, text, state):
00030         response = None
00031         cmds = readline.get_line_buffer().split()
00032         prefix = ""
00033         if len(cmds) > 0:
00034             prefix = cmds[0]
00035             if not self.options.has_key(prefix):
00036                 prefix = ""
00037 
00038         if state == 0:
00039             # This is the first time for this text, so build a match list.
00040             if text:
00041                 if len(prefix) == 0:
00042                     self.matches = sorted([s 
00043                                            for s in self.options.keys()
00044                                            if s and s.startswith(text)])
00045                 else:
00046                     self.matches = sorted([s 
00047                                            for s in self.options[prefix]
00048                                            if s and s.startswith(text)])
00049             else:
00050                 if len(prefix) == 0:
00051                     self.matches = sorted(self.options.keys())
00052                 else:
00053                     self.matches = self.options[prefix]
00054         
00055         # Return the state'th item from the match list,
00056         # if we have that many.
00057         try:
00058             response = self.matches[state] + " "
00059         except IndexError:
00060             response = None
00061         return response
00062 
00063 def print_message(level, msg):
00064     if level == MoveGroupInfoLevel.FAIL:
00065         print bcolors.FAIL + msg + bcolors.ENDC
00066     elif level == MoveGroupInfoLevel.WARN:
00067         print bcolors.WARNING + msg + bcolors.ENDC
00068     elif level == MoveGroupInfoLevel.SUCCESS:
00069         print bcolors.OKGREEN + msg + bcolors.ENDC
00070     elif level == MoveGroupInfoLevel.DEBUG:
00071         print bcolors.OKBLUE + msg + bcolors.ENDC
00072     else:
00073         print msg
00074 
00075 def get_context_keywords(interpreter):
00076     kw = interpreter.get_keywords()            
00077     kw["quit"] = []
00078     return kw
00079 
00080 def run_interactive(group_name):
00081     c = MoveGroupCommandInterpreter()
00082     if len(group_name) > 0:
00083         c.execute("use " + group_name)
00084     completer = SimpleCompleter(get_context_keywords(c))
00085     readline.set_completer(completer.complete)
00086 
00087     print
00088     print bcolors.HEADER + "Waiting for commands. Type 'help' to get a list of known commands." + bcolors.ENDC
00089     print
00090     readline.parse_and_bind('tab: complete')
00091 
00092     while not rospy.is_shutdown():
00093         cmd = ""
00094         try:  
00095             name = ""
00096             ag = c.get_active_group()
00097             if ag != None:
00098                 name = ag.get_name()
00099             cmd = raw_input(bcolors.OKBLUE + name + '> ' + bcolors.ENDC)
00100         except:
00101             break
00102         cmdorig = cmd.strip()
00103         if cmdorig == "":
00104             continue
00105         cmd = cmdorig.lower()
00106 
00107         if cmd == "q" or cmd == "quit" or cmd == "exit":
00108             break
00109         if cmd == "host":
00110             print_message(MoveGroupInfoLevel.INFO, "Master is '" + os.environ['ROS_MASTER_URI'] + "'")
00111             continue
00112 
00113         (level, msg) = c.execute(cmdorig)
00114         print_message(level, msg)
00115         # update the set of keywords
00116         completer.set_options(get_context_keywords(c))
00117             
00118 def run_service(group_name): 
00119     c = MoveGroupCommandInterpreter()
00120     if len(group_name) > 0:
00121         c.execute("use " + group_name)
00122     # add service stuff
00123     print "Running ROS service"
00124     rospy.spin()
00125 
00126 def stop_ros(reason):
00127     rospy.signal_shutdown(reason)
00128     roscpp_shutdown()
00129 
00130 def sigint_handler(signal, frame):
00131     stop_ros("Ctrl+C pressed")
00132     # this won't actually exit, but trigger an exception to terminate raw_input
00133     sys.exit(0)
00134 
00135 if __name__=='__main__':
00136     
00137     signal.signal(signal.SIGINT, sigint_handler)
00138 
00139     roscpp_initialize(sys.argv)
00140 
00141     rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True)
00142 
00143     parser = argparse.ArgumentParser(usage = """%(prog)s [options] [<group_name>]""",
00144                                      description = "Command Line Interface to MoveIt!")
00145     parser.add_argument("-i", "--interactive", action="store_true", dest="interactive", default=True,
00146                         help="Run the command processing script in interactive mode (default)")
00147     parser.add_argument("-s", "--service", action="store_true", dest="service", default=False,
00148                         help="Run the command processing script as a ROS service")
00149     parser.add_argument("group_name", type=str, default="", nargs='?', help="Group name to initialize the CLI for.")
00150     
00151     opt = parser.parse_args(rospy.myargv()[1:])
00152 
00153     if opt.service:
00154         run_service(opt.group_name)
00155     else:
00156         run_interactive(opt.group_name)
00157 
00158     stop_ros("Done")
00159 
00160     print "Bye bye!"


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:57