draw_reachable_zones.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 """
00036 Draws circles around the pr2 shoulders to show distances to the wrist either continuously or when asked to
00037 """
00038 
00039 import roslib
00040 roslib.load_manifest('pr2_marker_control')
00041 import rospy
00042 import object_manipulator.draw_functions
00043 import scipy
00044 from std_msgs.msg import Empty
00045 import sys
00046 
00047 #draw horizontal circles at shoulder-level
00048 def draw_circles(duration):
00049     draw_functions.draw_rviz_circle(scipy.matrix(scipy.identity(4)), 0.82, frame = 'l_shoulder_pan_link', 
00050                                     ns = 'left_wrist', id = 0, duration = duration, color = [0,1,1], opaque = 1, 
00051                                     frame_locked = True)
00052     draw_functions.draw_rviz_circle(scipy.matrix(scipy.identity(4)), 0.82, frame = 'r_shoulder_pan_link', 
00053                                     ns = 'right_wrist', id = 0, duration = duration, color = [1,0,1], opaque = 1, 
00054                                     frame_locked = True)
00055 
00056 #draw continuously
00057 def draw_continuous():
00058     rate = rospy.Rate(2.0)
00059     while not rospy.is_shutdown():
00060         draw_circles(0.)
00061         rate.sleep()
00062 
00063 #callback to draw when you see a ping message
00064 def callback(msg):
00065     print "drawing reachable zones"
00066     draw_circles(20.)
00067 
00068 if __name__ == '__main__':
00069     rospy.init_node('draw_reachable_zones')
00070     marker_topic = "grasp_markers"
00071     draw_functions = object_manipulator.draw_functions.DrawFunctions(marker_topic, latch=False)
00072 
00073     if len(sys.argv) >= 2 and sys.argv[1] == "c":
00074         print 'drawing reachable zones continuously'
00075         draw_continuous()
00076     else:
00077         rospy.Subscriber("draw_reachable_zones/ping", Empty, callback)
00078         print "subscribed to draw_reachable_zones/ping"
00079         rospy.spin()
00080 


pr2_marker_control
Author(s): Adam Leeper
autogenerated on Mon Oct 6 2014 12:48:08