move_joint.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from __future__ import print_function
00004 import argparse
00005 import sys
00006 import signal
00007 from time import time, sleep
00008 from uuid import uuid4
00009 import rospy
00010 from actionlib import SimpleActionClient, GoalStatus
00011 from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal, PlayMotionResult
00012 
00013 # pretty - A miniature library that provides a Python print and stdout
00014 # wrapper that makes colored terminal text easier to use (eg. without
00015 # having to mess around with ANSI escape sequences). This code is public
00016 # domain - there is no license except that you must leave this header.
00017 #
00018 # Copyright (C) 2008 Brian Nez <thedude at bri1 dot com>
00019 #
00020 # With modifications
00021 # (C) 2013 Paul M <pmathieu@willowgarage.com>
00022 # (C) 2014 Adolfo Rodriguez Tsouroukdissian <adolfo.rodriguez@pal-robotics.com>
00023 
00024 codeCodes = {
00025     'black':    '0;30', 'bright gray':  '0;37',
00026     'blue':     '0;34', 'white':        '1;37',
00027     'green':    '0;32', 'bright blue':  '1;34',
00028     'cyan':     '0;36', 'bright green': '1;32',
00029     'red':      '0;31', 'bright cyan':  '1;36',
00030     'purple':   '0;35', 'bright red':   '1;31',
00031     'yellow':   '0;33', 'bright purple':'1;35',
00032     'dark gray':'1;30', 'bright yellow':'1;33',
00033     'normal':   '0'
00034 }
00035 
00036 def printc(text, color, file = sys.stdout):
00037     """Print in color."""
00038     if ((file == sys.stdout and sys.stdout.isatty()) or
00039         (file == sys.stderr and sys.stderr.isatty())):
00040         print("\033["+codeCodes[color]+"m"+text+"\033[0m", file = file)
00041     else:
00042         print(text)
00043 
00044 def bold(msg):      return "\033[0;1m"+msg+"\033[0;22m"
00045 def print_err(msg): printc(msg, 'red', file = sys.stderr)
00046 def print_ok(msg):  printc(msg, 'green')
00047 
00048 class MotionData:
00049     def __init__(self):
00050         self.motion_name = None
00051         self.joint_name  = None
00052         self.position    = None
00053         self.duration    = None
00054 
00055 class MoveJointException(Exception):
00056     pass
00057 
00058 # Globals
00059 motion_data = MotionData()
00060 
00061 def parse_args(args=None):
00062     parser = argparse.ArgumentParser(description='Move individual joints.')
00063     parser.add_argument('name',
00064                         help='Name of the joint to move.')
00065     parser.add_argument('position', type=float,
00066                         help='Desired joint position in radians/meters.')
00067     parser.add_argument('duration', type=float, default=0.0, nargs='?',
00068                         help='Motion duration in seconds.')
00069     return parser.parse_args(args=args)
00070 
00071 def wait_for_clock():
00072     timeout = time() + 2.0 # Wall-clock time
00073     while not rospy.is_shutdown() and rospy.Time.now().is_zero():
00074         if time() > timeout:
00075             raise MoveJointException('Timed out waiting for a valid time. Is the /clock topic being published?.')
00076         sleep(0.1) # Wall-clock sleep
00077 
00078 def play_motion_client(action_ns):
00079     client = SimpleActionClient(action_ns, PlayMotionAction)
00080     if not client.wait_for_server(rospy.Duration(1)):
00081         raise MoveJointException("No play_motion server running (namespace '{}')".format(action_ns))
00082     return client
00083 
00084 def load_motion(motion_namespace, motion_data):
00085     motion_param = motion_namespace + '/' + motion_data.motion_name
00086     rospy.set_param(motion_param + '/joints', [motion_data.joint_name])
00087     rospy.set_param(motion_param + '/points', [{'positions': [motion_data.position],
00088                                                 'time_from_start': motion_data.duration}])
00089 
00090 def unload_motion(motion_namespace, motion_name):
00091     motion_param = motion_namespace + '/' + motion_name
00092     if rospy.has_param(motion_param):
00093         rospy.delete_param(motion_param)
00094 
00095 def active_cb():
00096     msg = 'Moving joint ' + bold(motion_data.joint_name) + ' to position ' + bold(str(motion_data.position))
00097     if motion_data.duration > 0.0:
00098         msg += ' in ' + bold(str(motion_data.duration)) + 's'
00099     print(msg)
00100 
00101 def move_joint():
00102     global motion_data
00103 
00104     args = parse_args(rospy.myargv()[1:])
00105     rospy.init_node('move_joint', anonymous=True)
00106 
00107     motion_data.motion_name = args.name + '_' + uuid4().hex
00108     motion_data.joint_name  = args.name
00109     motion_data.position    = args.position
00110     motion_data.duration    = args.duration
00111 
00112     play_motion_ns = 'play_motion' # TODO: How to resolve names?
00113     motion_ns      = play_motion_ns + '/motions'
00114 
00115     try:
00116         wait_for_clock()
00117         client = play_motion_client(play_motion_ns)
00118     except MoveJointException, e:
00119         print_err(str(e))
00120 
00121     def cancel(signum, frame):
00122       client.cancel_goal()
00123       rospy.signal_shutdown("goal canceled")
00124     signal.signal(signal.SIGINT, cancel)
00125 
00126     load_motion(motion_ns, motion_data)
00127     goal = PlayMotionGoal(motion_name = motion_data.motion_name, skip_planning = True)
00128     client.send_goal(goal, None, active_cb)
00129     client.wait_for_result()
00130 
00131     unload_motion(motion_ns, motion_data.motion_name)
00132     al_res = client.get_state()
00133     pm_res = client.get_result()
00134     if al_res != GoalStatus.SUCCEEDED or pm_res.error_code != PlayMotionResult.SUCCEEDED:
00135         print_err("Execution failed with status {}. {}".format(GoalStatus.to_string(al_res), pm_res.error_string))
00136         sys.exit(1)
00137 
00138 if __name__ == "__main__":
00139     move_joint()


play_motion
Author(s): Paul Mathieu , Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Jun 8 2019 20:26:22