Go to the documentation of this file.00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import sensor_msgs.msg as sm
00004 import hrl_lib.rutils as ru
00005 import numpy as np
00006 import pr2_msgs.msg as pm
00007 import geometry_msgs.msg as gm
00008 import tf
00009 import hrl_lib.tf_utils as tfu
00010 import tf.transformations as tr
00011 import time
00012 import hrl_lib.util as ut
00013 import pdb
00014
00015 def np_to_pointcloud(points_mat, frame):
00016 pc = sm.PointCloud()
00017 pc.header.stamp = rospy.get_rostime()
00018 pc.header.frame_id = frame
00019
00020 for i in range(points_mat.shape[1]):
00021 p32 = gm.Point32()
00022 p32.x = points_mat[0,i]
00023 p32.y = points_mat[1,i]
00024 p32.z = points_mat[2,i]
00025 pc.points.append(p32)
00026 return pc
00027
00028 if __name__ == '__main__':
00029
00030 import sys
00031
00032 pname = sys.argv[1]
00033
00034
00035 scene, contact_points = ut.load_pickle(pname)
00036
00037
00038
00039
00040
00041 rospy.init_node('test10')
00042 contact_pub = rospy.Publisher('contact_cloud', sm.PointCloud)
00043 touchll_pub = rospy.Publisher('touch_ll', sm.PointCloud)
00044 touchlr_pub = rospy.Publisher('touch_lr', sm.PointCloud)
00045
00046 left_contact, right_contact = zip(*[(np.matrix(l[1][2]).T, np.matrix(l[1][3]).T) for l in contact_points])
00047 left_contact = np.column_stack(left_contact)
00048 right_contact = np.column_stack(right_contact)
00049
00050 scene_pc = np_to_pointcloud(scene, 'base_footprint')
00051 left_con_pc = np_to_pointcloud(left_contact, 'base_footprint')
00052 right_con_pc = np_to_pointcloud(right_contact, 'base_footprint')
00053
00054 r = rospy.Rate(10)
00055 rospy.loginfo('test10: publishing')
00056 while not rospy.is_shutdown():
00057 contact_pub.publish(scene_pc)
00058 touchll_pub.publish(left_con_pc)
00059 touchlr_pub.publish(right_con_pc)
00060 r.sleep()
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100