Go to the documentation of this file.00001
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
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
00056
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
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
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
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!"