katana_manipulation_tutorials: katana_manipulation_tutorials: draw

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