katana_object_manipulator: katana_object_manipulator: 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 
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):
00057         self.marker_pub = rospy.Publisher(topic, Marker)
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.]):
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         return marker
00097 
00098 
00099     ##draw a set of points (3xn or 4xn scipy matrix) in rviz
00100     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):
00101         marker = self.create_marker(Marker.POINTS, [size, size, size], frame, ns, id, duration, color, opaque)
00102 
00103         for point_ind in range(scipy.shape(points)[1]):
00104             new_point = Point()
00105             new_point.x = points[0, point_ind]
00106             new_point.y = points[1, point_ind]
00107             new_point.z = points[2, point_ind]
00108             marker.points.append(new_point)
00109 
00110         self.marker_pub.publish(marker)
00111         rospy.loginfo("published points")
00112 
00113 
00114     ##draw a set of axes in rviz with arrows of varying lengths
00115     #pose is a 4x4 scipy matrix
00116     def draw_rviz_axes(self, pose_mat, frame, lengths = [.05, .01, .01], ns = 'axes', id = 0, duration = 300.):
00117 
00118         marker = self.create_marker(Marker.ARROW, [.01, .02, 0], frame, ns, id, duration)
00119         marker.color.a = 1.0
00120 
00121         #find the arrow endpoints
00122         start = pose_mat[0:3, 3]
00123         x_end = (pose_mat[:,0][0:3]*lengths[0] + start).T.tolist()[0]
00124         y_end = (pose_mat[:,1][0:3]*lengths[1] + start).T.tolist()[0]
00125         z_end = (pose_mat[:,2][0:3]*lengths[2] + start).T.tolist()[0]
00126         start = start.T.tolist()[0]
00127 
00128         #draw the arrows (x=red, y=green, z=blue)
00129         marker.id = id
00130         marker.points = [Point(*start), Point(*x_end)]
00131         marker.color.r = 1.0
00132         marker.color.g = 0.0
00133         marker.color.b = 0.0
00134         self.marker_pub.publish(marker)
00135         marker.id = id+1
00136         marker.points = [Point(*start), Point(*y_end)]
00137         marker.color.r = 0.0
00138         marker.color.g = 1.0
00139         marker.color.b = 0.0
00140         self.marker_pub.publish(marker)
00141         marker.id = id+2
00142         marker.points = [Point(*start), Point(*z_end)]
00143         marker.color.r = 0.0
00144         marker.color.g = 0.0
00145         marker.color.b = 1.0
00146         self.marker_pub.publish(marker)
00147 
00148 
00149     ##draw a sphere in rviz at pose_mat (4x4 scipy matrix) with radius r
00150     def draw_rviz_sphere(self, pose_mat, r, frame = 'object_frame', ns = 'spheres', id = 0, duration = 60., color = [1,0,0], opaque = 0.5):
00151 
00152         (pos, quat) = mat_to_pos_and_quat(pose_mat)
00153         marker = self.create_marker(Marker.SPHERE, [r*2., r*2., r*2.], frame, ns, id, duration, color, opaque, pos, quat)
00154         self.marker_pub.publish(marker)
00155 
00156 
00157     ##draw a box in rviz at pose_mat (4x4 scipy matrix) defined by either:
00158     #    2-lists (min, max) of 3-lists (x,y,z) of corner coords
00159     #    or a 3-list of dimensions (x,y,z)
00160     #in frame_id frame (defaults to the object frame), id number id, and RGB color
00161     def draw_rviz_box(self, pose_mat, ranges, frame = 'object_frame', ns = 'boxes', id = 0, duration = 60., color = [1,0,0], opaque = 0.5):
00162         if len(ranges) == 2:
00163             dims = [upper-lower for (upper, lower) in list(zip(ranges[0], ranges[1]))]
00164             center = [(upper-lower)/2+lower for (upper, lower) in list(zip(ranges[0], ranges[1]))]
00165 
00166         elif len(ranges) == 3:
00167             dims = ranges
00168             center = [0., 0., 0.]
00169 
00170         #rotate the box center to frame
00171         center = scipy.matrix(center + [1])
00172         transformed_center = pose_mat * center.T
00173 
00174         quat = tf.transformations.quaternion_from_matrix(pose_mat)
00175 
00176         marker = self.create_marker(Marker.CUBE, dims, frame, ns, id, duration, color, opaque, transformed_center[0:3, 0], quat)
00177         self.marker_pub.publish(marker)
00178 
00179 
00180     ##draw a cylinder in rviz at pose_mat (4x4 scipy matrix, z-axis is cylinder axis) with radius r and length l
00181     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):
00182 
00183         (pos, quat) = mat_to_pos_and_quat(pose_mat)
00184         marker = self.create_marker(Marker.CYLINDER, [r*2., r*2., l], frame, ns, id, duration, color, opaque, pos, quat)
00185         self.marker_pub.publish(marker)
00186 
00187 
00188 
00189     ##clear all the currently drawn grasps by redrawing them tiny and short-lived
00190     def clear_grasps(self, ns = 'grasps', num = 150, frame = '/base_link'):
00191 
00192         marker = Marker()
00193         marker.header.frame_id = frame
00194         marker.header.stamp = rospy.Time.now()
00195         marker.ns = ns
00196         marker.type = Marker.ARROW
00197         marker.action = Marker.DELETE
00198         for i in range(num):
00199             marker.id = i
00200             self.marker_pub.publish(marker)
00201 
00202 
00203     ##draw a set of grasps (wrist Poses) as x and y-axis arrows in rviz,
00204     #with the x-axis long compared to y
00205     def draw_grasps(self, grasps, frame, ns = 'grasps', pause = 0):
00206 
00207         marker = Marker()
00208         marker.header.frame_id = frame
00209         marker.header.stamp = rospy.Time.now()
00210         marker.ns = ns
00211         marker.type = Marker.ARROW
00212         marker.action = Marker.ADD
00213         marker.color.a = 1.0
00214         marker.lifetime = rospy.Duration(0)
00215 
00216         for (grasp_num, grasp) in enumerate(grasps):
00217             if grasp_num == 0:
00218                 marker.scale.x = 0.015
00219                 marker.scale.y = 0.025
00220                 length_fact = 1.5
00221 
00222             else:
00223                 marker.scale.x = 0.01
00224                 marker.scale.y = 0.015
00225                 length_fact = 1.0
00226 
00227             orientation = grasp.orientation
00228             quat = [orientation.x, orientation.y, orientation.z, orientation.w]
00229             mat = tf.transformations.quaternion_matrix(quat)
00230             start = [grasp.position.x, grasp.position.y, grasp.position.z]
00231             x_end = list(mat[:,0][0:3]*.05*length_fact + scipy.array(start))
00232             y_end = list(mat[:,1][0:3]*.02*length_fact + scipy.array(start))
00233             marker.id = grasp_num*3
00234             marker.points = [Point(*start), Point(*x_end)]
00235             marker.color.r = 1.0
00236             marker.color.g = 0.0
00237             marker.color.b = 0.0
00238             self.marker_pub.publish(marker)
00239             marker.id = grasp_num*3+1
00240             marker.points = [Point(*start), Point(*y_end)]
00241             marker.color.r = 0.0
00242             marker.color.g = 1.0
00243             marker.color.b = 0.0
00244             self.marker_pub.publish(marker)
00245             marker.id = grasp_num*3+2
00246             if pause:
00247                 print "press enter to continue"
00248                 raw_input()
00249         time.sleep(.5)
00250 
00251 
00252 #test script
00253 if __name__ == "__main__":
00254     rospy.init_node('draw_functions', anonymous=True)
00255 
00256     pose_mat = scipy.matrix(scipy.identity(4))
00257     pose_mat[2, 3] = -1.0
00258 
00259     draw_functions = DrawFunctions('visualization_marker')
00260 
00261     while(not rospy.is_shutdown()):
00262 
00263         points = scipy.matrix([[.8, 0., -1.0],
00264                                [.9, 0., -1.0],
00265                                [1.0, 0., -1.0],
00266                                [.9, .1, -1.0]]).T
00267         draw_functions.draw_rviz_points(points, frame = 'base_footprint')
00268 
00269         pose_mat[0, 3] = .3
00270         draw_functions.draw_rviz_box(pose_mat, [.1, .2, .3], frame = 'base_footprint')
00271 
00272         pose_mat[0, 3] = 0
00273         draw_functions.draw_rviz_cylinder(pose_mat, .1, .3, frame = 'base_footprint')
00274 
00275         pose_mat[0, 3] = -.3
00276         draw_functions.draw_rviz_sphere(pose_mat, .1, frame = 'base_footprint')
00277 
00278         pose_mat[0, 3] = .6
00279         pose = mat_to_pose(pose_mat)
00280         draw_functions.draw_grasps([pose], frame = 'base_footprint')
00281 
00282         print "drew shapes, press enter to re-draw"
00283 
00284         raw_input()
draw_functions.py
