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


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jul 8 2020 03:53:24