moveit_commander_cmdline.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib
4 import rospy
5 import readline
6 import sys
7 import os
8 import signal
9 
10 import argparse
11 from moveit_commander import MoveGroupCommandInterpreter, MoveGroupInfoLevel, roscpp_initialize, roscpp_shutdown
12 
13 class bcolors:
14  HEADER = '\033[95m'
15  OKBLUE = '\033[94m'
16  OKGREEN = '\033[92m'
17  WARNING = '\033[93m'
18  FAIL = '\033[91m'
19  ENDC = '\033[0m'
20 
21 class SimpleCompleter(object):
22 
23  def __init__(self, options):
24  self.options = options
25 
26  def set_options(self, options):
27  self.options = options
28 
29  def complete(self, text, state):
30  response = None
31  cmds = readline.get_line_buffer().split()
32  prefix = ""
33  if len(cmds) > 0:
34  prefix = cmds[0]
35  if not self.options.has_key(prefix):
36  prefix = ""
37 
38  if state == 0:
39  # This is the first time for this text, so build a match list.
40  if text:
41  if len(prefix) == 0:
42  self.matches = sorted([s
43  for s in self.options.keys()
44  if s and s.startswith(text)])
45  else:
46  self.matches = sorted([s
47  for s in self.options[prefix]
48  if s and s.startswith(text)])
49  else:
50  if len(prefix) == 0:
51  self.matches = sorted(self.options.keys())
52  else:
53  self.matches = self.options[prefix]
54 
55  # Return the state'th item from the match list,
56  # if we have that many.
57  try:
58  response = self.matches[state] + " "
59  except IndexError:
60  response = None
61  return response
62 
63 def print_message(level, msg):
64  if level == MoveGroupInfoLevel.FAIL:
65  print bcolors.FAIL + msg + bcolors.ENDC
66  elif level == MoveGroupInfoLevel.WARN:
67  print bcolors.WARNING + msg + bcolors.ENDC
68  elif level == MoveGroupInfoLevel.SUCCESS:
69  print bcolors.OKGREEN + msg + bcolors.ENDC
70  elif level == MoveGroupInfoLevel.DEBUG:
71  print bcolors.OKBLUE + msg + bcolors.ENDC
72  else:
73  print msg
74 
75 def get_context_keywords(interpreter):
76  kw = interpreter.get_keywords()
77  kw["quit"] = []
78  return kw
79 
80 def run_interactive(group_name):
81  c = MoveGroupCommandInterpreter()
82  if len(group_name) > 0:
83  c.execute("use " + group_name)
85  readline.set_completer(completer.complete)
86 
87  print
88  print bcolors.HEADER + "Waiting for commands. Type 'help' to get a list of known commands." + bcolors.ENDC
89  print
90  readline.parse_and_bind('tab: complete')
91 
92  while not rospy.is_shutdown():
93  cmd = ""
94  try:
95  name = ""
96  ag = c.get_active_group()
97  if ag != None:
98  name = ag.get_name()
99  cmd = raw_input(bcolors.OKBLUE + name + '> ' + bcolors.ENDC)
100  except:
101  break
102  cmdorig = cmd.strip()
103  if cmdorig == "":
104  continue
105  cmd = cmdorig.lower()
106 
107  if cmd == "q" or cmd == "quit" or cmd == "exit":
108  break
109  if cmd == "host":
110  print_message(MoveGroupInfoLevel.INFO, "Master is '" + os.environ['ROS_MASTER_URI'] + "'")
111  continue
112 
113  (level, msg) = c.execute(cmdorig)
114  print_message(level, msg)
115  # update the set of keywords
116  completer.set_options(get_context_keywords(c))
117 
118 def run_service(group_name):
119  c = MoveGroupCommandInterpreter()
120  if len(group_name) > 0:
121  c.execute("use " + group_name)
122  # add service stuff
123  print "Running ROS service"
124  rospy.spin()
125 
126 def stop_ros(reason):
127  rospy.signal_shutdown(reason)
128  roscpp_shutdown()
129 
130 def sigint_handler(signal, frame):
131  stop_ros("Ctrl+C pressed")
132  # this won't actually exit, but trigger an exception to terminate raw_input
133  sys.exit(0)
134 
135 if __name__=='__main__':
136 
137  signal.signal(signal.SIGINT, sigint_handler)
138 
139  roscpp_initialize(sys.argv)
140 
141  rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True)
142 
143  parser = argparse.ArgumentParser(usage = """%(prog)s [options] [<group_name>]""",
144  description = "Command Line Interface to MoveIt!")
145  parser.add_argument("-i", "--interactive", action="store_true", dest="interactive", default=True,
146  help="Run the command processing script in interactive mode (default)")
147  parser.add_argument("-s", "--service", action="store_true", dest="service", default=False,
148  help="Run the command processing script as a ROS service")
149  parser.add_argument("group_name", type=str, default="", nargs='?', help="Group name to initialize the CLI for.")
150 
151  opt = parser.parse_args(rospy.myargv()[1:])
152 
153  if opt.service:
154  run_service(opt.group_name)
155  else:
156  run_interactive(opt.group_name)
157 
158  stop_ros("Done")
159 
160  print "Bye bye!"


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:52:26