def run_service(group_name):
c = MoveGroupCommandInterpreter()
if len(group_name) > 0:
c.execute("use " + group_name)
# add service stuff
print("Running ROS service")
rospy.spin()
def run_service(group_names):
c = MoveGroupCommandInterpreter()
for g in group_names:
c.execute("use " + g)
# add service stuff
print "Running ROS service"
rospy.spin()
def run_service(group_names):
c = MoveGroupCommandInterpreter()
for g in group_names:
c.execute("use " + g)
# add service stuff
print "Running ROS service"
rospy.spin()
def run_service(group_name):
c = MoveGroupCommandInterpreter()
if len(group_name) > 0:
c.execute("use " + group_name)
# add service stuff
print("Running ROS service")
rospy.spin()
def run_interactive(group_names):
c = MoveGroupCommandInterpreter()
for g in group_names:
c.execute( "use " + g)
completer = SimpleCompleter(get_context_keywords(c))
readline.set_completer(completer.complete)
print
print bcolors.HEADER + "Waiting for commands. Type 'help' to get a list of known commands." + bcolors.ENDC
print
readline.parse_and_bind('tab: complete')
while not rospy.is_shutdown():
cmd = ""
try:
name = ""
ag = c.get_active_group()
if ag != None:
name = ag.get_name()
cmd = raw_input(bcolors.OKBLUE + name + '> ' + bcolors.ENDC)
except:
break
cmd = cmd.strip()
if cmd == "":
continue
cmd = cmd.lower()
if cmd == "q" or cmd == "quit" or cmd == "exit":
break
if cmd == "host":
print_message(MoveGroupInfoLevel.INFO, "Master is '" + os.environ['ROS_MASTER_URI'] + "'")
continue
(level, msg) = c.execute(cmd)
print_message(level, msg)
# update the set of keywords
completer.set_options(get_context_keywords(c))
def run_interactive(group_name):
c = MoveGroupCommandInterpreter()
if len(group_name) > 0:
c.execute("use " + group_name)
completer = SimpleCompleter(get_context_keywords(c))
readline.set_completer(completer.complete)
print()
print(
bcolors.HEADER +
"Waiting for commands. Type 'help' to get a list of known commands." +
bcolors.ENDC)
print()
readline.parse_and_bind("tab: complete")
while not rospy.is_shutdown():
cmd = ""
try:
name = ""
ag = c.get_active_group()
if ag != None:
name = ag.get_name()
cmd = input(bcolors.OKBLUE + name + "> " + bcolors.ENDC)
except:
break
cmdorig = cmd.strip()
if cmdorig == "":
continue
cmd = cmdorig.lower()
if cmd == "q" or cmd == "quit" or cmd == "exit":
break
if cmd == "host":
print_message(
MoveGroupInfoLevel.INFO,
"Master is '" + os.environ["ROS_MASTER_URI"] + "'",
)
continue
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
# update the set of keywords
completer.set_options(get_context_keywords(c))
#!/usr/bin/env python
import rospy
from moveit_commander import MoveGroupCommandInterpreter, MoveGroupInfoLevel
from std_msgs.msg import String
cmdInterpreter = MoveGroupCommandInterpreter()
last_cmd = "load pos.cfg"
class bcolors:
HEADER = '\033[95m'
OKBLUE = '\033[94m'
OKGREEN = '\033[92m'
WARNING = '\033[93m'
FAIL = '\033[91m'
ENDC = '\033[0m'
def print_message(level, msg):
if level == MoveGroupInfoLevel.FAIL:
print bcolors.FAIL + msg + bcolors.ENDC
elif level == MoveGroupInfoLevel.WARN:
print bcolors.WARNING + msg + bcolors.ENDC
elif level == MoveGroupInfoLevel.SUCCESS:
print bcolors.OKGREEN + msg + bcolors.ENDC
elif level == MoveGroupInfoLevel.DEBUG:
print bcolors.OKBLUE + msg + bcolors.ENDC
else:
print msg
def run_interactive(group_name):
c = MoveGroupCommandInterpreter()
if len(group_name) > 0:
c.execute("use " + group_name)
print("Group Name Set")
completer = SimpleCompleter(get_context_keywords(c))
readline.set_completer(completer.complete)
print()
print(
bcolors.HEADER +
"Waiting for commands. Type 'help' to get a list of known commands." +
bcolors.ENDC)
print()
readline.parse_and_bind('tab: complete')
while not rospy.is_shutdown():
cmd = ""
try:
name = ""
ag = c.get_active_group()
if ag != None:
name = ag.get_name()
cmd = input(bcolors.OKBLUE + name + '> ' + bcolors.ENDC)
except:
break
cmdorig = cmd.strip()
if cmdorig == "":
continue
cmd = cmdorig.lower()
if cmd == "q" or cmd == "quit" or cmd == "exit":
break
if cmd == "host":
print_message(MoveGroupInfoLevel.INFO,
"Master is '" + os.environ['ROS_MASTER_URI'] + "'")
continue
print("Recieved Command", cmdorig)
if (cmdorig == "ik"):
print("Give the x y z inputs")
x, y, z = raw_input().split(' ')
''' call inverse kinematics and get current state'''
cmdorig = "current"
(level, msg) = c.execute(cmdorig)
completer.set_options(get_context_keywords(c))
joints = inverse_kinematics(x, y, z, msg)
## rec c
cmdorig = "rec c"
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
completer.set_options(get_context_keywords(c))
## goal = c
cmdorig = "goal = c"
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
completer.set_options(get_context_keywords(c))
## setting joints
for i in range(0, len(joints)):
print("goal", i)
cmdorig = "goal[" + str(i) + "]" + "=" + str(joints[i])
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
completer.set_options(get_context_keywords(c))
## finally execute
cmdorig = "go goal"
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
completer.set_options(get_context_keywords(c))
else:
(level, msg) = c.execute(cmdorig)
print_message(level, msg)
# update the set of keywords
completer.set_options(get_context_keywords(c))
