draw_functions.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 ## @package draw_functions
00037 #helper functions for drawing things in rviz
00038 
00039 from __future__ import division
00040 import roslib
00041 roslib.load_manifest('object_manipulator')
00042 import rospy
00043 
00044 from visualization_msgs.msg import Marker
00045 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
00046 import tf.transformations
00047 import scipy
00048 import math
00049 import time
00050 import pdb
00051 from convert_functions import *
00052 
00053 ##class to draw stuff to rviz
00054 class DrawFunctions():
00055 
00056     def __init__(self, topic, latch = False):
00057         self.marker_pub = rospy.Publisher(topic, Marker, latch = latch)
00058 
00059     ##clear the current set of points
00060     def clear_rviz_points(self, ns = 'points', id = 0):
00061 
00062         marker = Marker()
00063         marker.header.frame_id = "base_link"
00064         marker.header.stamp = rospy.Time.now()
00065         marker.ns = 'points'
00066         marker.type = Marker.POINTS
00067         marker.action = Marker.DELETE
00068         marker.id = id
00069         self.marker_pub.publish(marker)
00070 
00071 
00072     ##fill in a Marker message
00073     def create_marker(self, type, dims, frame, ns, id, duration = 60., color = [1,0,0], opaque = 0.5, pos = [0.,0.,0.], quat = [0.,0.,0.,1.], frame_locked = False):
00074         marker = Marker()
00075         marker.header.frame_id = frame
00076         marker.header.stamp = rospy.Time.now()
00077         marker.ns = ns
00078         marker.type = type
00079         marker.action = Marker.ADD
00080         marker.scale.x = dims[0]
00081         marker.scale.y = dims[1]
00082         marker.scale.z = dims[2]
00083         marker.color.a = opaque
00084         marker.color.r = color[0]
00085         marker.color.g = color[1]
00086         marker.color.b = color[2]
00087         marker.lifetime = rospy.Duration(duration)
00088         marker.id = id
00089         marker.pose.position.x = pos[0]
00090         marker.pose.position.y = pos[1]
00091         marker.pose.position.z = pos[2]
00092         marker.pose.orientation.x = quat[0]
00093         marker.pose.orientation.y = quat[1]
00094         marker.pose.orientation.z = quat[2]
00095         marker.pose.orientation.w = quat[3]       
00096         marker.frame_locked = frame_locked
00097         return marker
00098  
00099 
00100     ##draw a set of points (3xn or 4xn scipy matrix) in rviz
00101     def draw_rviz_points(self, points, frame = 'narrow_stereo_optical_frame', size = .005, ns = 'points', id = 0, duration = 20., color = [0,0,1], opaque = 1.0, pose_mat = None, frame_locked = False):
00102         if pose_mat != None:
00103             (pos, quat) = mat_to_pos_and_quat(pose_mat)
00104             marker = self.create_marker(Marker.POINTS, [size, size, size], frame, ns, id, duration, color, opaque, pos, quat, frame_locked = frame_locked)
00105         else:
00106             marker = self.create_marker(Marker.POINTS, [size, size, size], frame, ns, id, duration, color, opaque, frame_locked = frame_locked)
00107 
00108         for point_ind in range(scipy.shape(points)[1]):
00109             new_point = Point()
00110             new_point.x = points[0, point_ind]
00111             new_point.y = points[1, point_ind]
00112             new_point.z = points[2, point_ind]
00113             marker.points.append(new_point)
00114 
00115         self.marker_pub.publish(marker)
00116 
00117 
00118     ##draw a set of axes in rviz with arrows of varying lengths
00119     #pose is a 4x4 scipy matrix
00120     def draw_rviz_axes(self, pose_mat, frame, lengths = [.05, .01, .01], ns = 'axes', id = 0, duration = 300., frame_locked = False):
00121         
00122         marker = self.create_marker(Marker.ARROW, [.01, .02, 0], frame, ns, id, duration, frame_locked = frame_locked)
00123         marker.color.a = 1.0
00124 
00125         #find the arrow endpoints
00126         start = pose_mat[0:3, 3]
00127         x_end = (pose_mat[:,0][0:3]*lengths[0] + start).T.tolist()[0]    
00128         y_end = (pose_mat[:,1][0:3]*lengths[1] + start).T.tolist()[0]
00129         z_end = (pose_mat[:,2][0:3]*lengths[2] + start).T.tolist()[0]
00130         start = start.T.tolist()[0]
00131 
00132         #draw the arrows (x=red, y=green, z=blue)
00133         marker.id = id
00134         marker.points = [Point(*start), Point(*x_end)]
00135         marker.color.r = 1.0
00136         marker.color.g = 0.0
00137         marker.color.b = 0.0
00138         self.marker_pub.publish(marker)
00139         marker.id = id+1
00140         marker.points = [Point(*start), Point(*y_end)]
00141         marker.color.r = 0.0
00142         marker.color.g = 1.0
00143         marker.color.b = 0.0
00144         self.marker_pub.publish(marker)
00145         marker.id = id+2
00146         marker.points = [Point(*start), Point(*z_end)]
00147         marker.color.r = 0.0
00148         marker.color.g = 0.0
00149         marker.color.b = 1.0
00150         self.marker_pub.publish(marker)
00151 
00152 
00153     ##draw a sphere in rviz at pose_mat (4x4 scipy matrix) with radius r
00154     def draw_rviz_sphere(self, pose_mat, r, frame = 'object_frame', ns = 'spheres', id = 0, duration = 60., color = [1,0,0], opaque = 0.5, frame_locked = False):
00155         
00156         (pos, quat) = mat_to_pos_and_quat(pose_mat)
00157         marker = self.create_marker(Marker.SPHERE, [r*2., r*2., r*2.], frame, ns, id, duration, color, opaque, pos, quat, frame_locked = frame_locked)
00158         self.marker_pub.publish(marker)
00159 
00160 
00161     ##draw a box in rviz at pose_mat (4x4 scipy matrix) defined by either:
00162     #    2-lists (min, max) of 3-lists (x,y,z) of corner coords
00163     #    or a 3-list of dimensions (x,y,z)
00164     #in frame_id frame (defaults to the object frame), id number id, and RGB color
00165     def draw_rviz_box(self, pose_mat, ranges, frame = 'object_frame', ns = 'boxes', id = 0, duration = 60., color = [1,0,0], opaque = 0.5, frame_locked = False):
00166         if len(ranges) == 2:
00167             dims = [upper-lower for (upper, lower) in list(zip(ranges[0], ranges[1]))]
00168             center = [(upper-lower)/2+lower for (upper, lower) in list(zip(ranges[0], ranges[1]))]
00169 
00170         elif len(ranges) == 3:
00171             dims = ranges
00172             center = [0., 0., 0.]
00173 
00174         #rotate the box center to frame
00175         center = scipy.matrix(center + [1])
00176         transformed_center = pose_mat * center.T
00177                                     
00178         quat = tf.transformations.quaternion_from_matrix(pose_mat)
00179             
00180         marker = self.create_marker(Marker.CUBE, dims, frame, ns, id, duration, color, opaque, transformed_center[0:3, 0], quat, frame_locked = frame_locked)
00181         self.marker_pub.publish(marker)
00182 
00183 
00184     ##draw a cylinder in rviz at pose_mat (4x4 scipy matrix, z-axis is cylinder axis) with radius r and length l
00185     def draw_rviz_cylinder(self, pose_mat, r, l, frame = 'object_frame', ns = 'cylinders', id = 0, duration = 60., color = [1,0,0], opaque = 0.5, frame_locked = False):
00186         
00187         (pos, quat) = mat_to_pos_and_quat(pose_mat)
00188         marker = self.create_marker(Marker.CYLINDER, [r*2., r*2., l], frame, ns, id, duration, color, opaque, pos, quat, frame_locked = frame_locked)
00189         self.marker_pub.publish(marker)
00190 
00191 
00192     ##draw a line strip in rviz, with origin at pose_mat (4x4 scipy matrix) and points (3xn or 4xn scipy matrix) relative to that origin
00193     def draw_rviz_line_strip(self, pose_mat, points, frame, size = .005, ns = 'line strip', id = 0, duration = 20., color = [0,1,1], opaque = 1.0, frame_locked = False):
00194         (pos, quat) = mat_to_pos_and_quat(pose_mat)
00195         marker = self.create_marker(Marker.LINE_STRIP, [size, 0, 0], frame, ns, id, duration, color, opaque, pos, quat, frame_locked = frame_locked)
00196         for point_ind in range(scipy.shape(points)[1]):
00197             new_point = Point()
00198             new_point.x = points[0, point_ind]
00199             new_point.y = points[1, point_ind]
00200             new_point.z = points[2, point_ind]
00201             marker.points.append(new_point)
00202 
00203         self.marker_pub.publish(marker)
00204 
00205 
00206     ##draw a circle in rviz (pose_mat z-axis is normal to the circle plane) with radius r, thickness size, and number of facets num_facets
00207     def draw_rviz_circle(self, pose_mat, r, frame, num_facets = 20, size = .005, ns = 'circle', id = 0, duration = 20., color = [0,1,1], opaque = 1.0, frame_locked = False):
00208         angle_step = math.pi*2./num_facets
00209         points = scipy.matrix(scipy.zeros([3, num_facets+1]))
00210         for (ind, angle) in enumerate(scipy.arange(0, math.pi*2.+angle_step, angle_step)):
00211             points[0,ind] = math.cos(angle)*r
00212             points[1,ind] = math.sin(angle)*r
00213         self.draw_rviz_line_strip(pose_mat, points, frame, size, ns, id, duration, color, opaque, frame_locked)
00214 
00215 
00216     ##clear all the currently drawn grasps by redrawing them tiny and short-lived
00217     def clear_grasps(self, ns = 'grasps', num = 150, frame = '/base_link'):
00218 
00219         marker = Marker()
00220         marker.header.frame_id = frame
00221         marker.header.stamp = rospy.Time.now()
00222         marker.ns = ns
00223         marker.type = Marker.ARROW
00224         marker.action = Marker.DELETE
00225         for i in range(num):
00226             marker.id = i
00227             self.marker_pub.publish(marker)
00228 
00229 
00230     ##draw a set of grasps (wrist Poses) as x and y-axis arrows in rviz, 
00231     #with the x-axis long compared to y
00232     def draw_grasps(self, grasps, frame, ns = 'grasps', pause = 0, frame_locked = False):
00233 
00234         marker = Marker()
00235         marker.header.frame_id = frame
00236         marker.header.stamp = rospy.Time.now()
00237         marker.ns = ns
00238         marker.type = Marker.ARROW
00239         marker.action = Marker.ADD
00240         marker.color.a = 1.0
00241         marker.lifetime = rospy.Duration(0)
00242         marker.frame_locked = frame_locked
00243 
00244         for (grasp_num, grasp) in enumerate(grasps):
00245             if grasp_num == 0:
00246                 marker.scale.x = 0.015
00247                 marker.scale.y = 0.025
00248                 length_fact = 1.5
00249 
00250             else:
00251                 marker.scale.x = 0.01
00252                 marker.scale.y = 0.015
00253                 length_fact = 1.0
00254 
00255             orientation = grasp.orientation
00256             quat = [orientation.x, orientation.y, orientation.z, orientation.w]
00257             mat = tf.transformations.quaternion_matrix(quat)
00258             start = [grasp.position.x, grasp.position.y, grasp.position.z]
00259             x_end = list(mat[:,0][0:3]*.05*length_fact + scipy.array(start))    
00260             y_end = list(mat[:,1][0:3]*.02*length_fact + scipy.array(start))
00261             marker.id = grasp_num*3
00262             marker.points = [Point(*start), Point(*x_end)]
00263             marker.color.r = 1.0
00264             marker.color.g = 0.0
00265             marker.color.b = 0.0
00266             self.marker_pub.publish(marker)
00267             marker.id = grasp_num*3+1
00268             marker.points = [Point(*start), Point(*y_end)]
00269             marker.color.r = 0.0
00270             marker.color.g = 1.0
00271             marker.color.b = 0.0
00272             self.marker_pub.publish(marker)
00273             marker.id = grasp_num*3+2
00274             if pause:
00275                 print "press enter to continue"
00276                 raw_input()
00277         time.sleep(.5)
00278 
00279 
00280 #test script
00281 if __name__ == "__main__":
00282     rospy.init_node('draw_functions', anonymous=True)
00283 
00284     pose_mat = scipy.matrix(scipy.identity(4))
00285     pose_mat[2, 3] = -1.0
00286 
00287     draw_functions = DrawFunctions('visualization_marker')
00288 
00289     while(not rospy.is_shutdown()):
00290 
00291         points = scipy.matrix([[.8, 0., -1.0],
00292                                [.9, 0., -1.0],
00293                                [1.0, 0., -1.0],
00294                                [.9, .1, -1.0]]).T
00295         draw_functions.draw_rviz_points(points, frame = 'base_footprint')
00296 
00297         pose_mat[0, 3] = .3
00298         draw_functions.draw_rviz_box(pose_mat, [.1, .2, .3], frame = 'base_footprint')
00299         
00300         pose_mat[0, 3] = 0
00301         draw_functions.draw_rviz_cylinder(pose_mat, .1, .3, frame = 'base_footprint')
00302         
00303         pose_mat[0, 3] = -.3
00304         draw_functions.draw_rviz_sphere(pose_mat, .1, frame = 'base_footprint')
00305 
00306         pose_mat[0, 3] = .6
00307         pose = mat_to_pose(pose_mat)
00308         draw_functions.draw_grasps([pose], frame = 'base_footprint')
00309         
00310         print "drew shapes, press enter to re-draw"
00311 
00312         raw_input()


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04