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


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:04:15