Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 from laser_camera_segmentation.srv._Segmentation import *
00031 import sensor_msgs
00032
00033 import numpy as np
00034
00035 import opencv as cv
00036 import hrl_lib.util as ut
00037 import opencv.highgui as highgui
00038
00039 from labeling.label_object import label_object
00040
00041 def convert_pointcloud_to_ROS(pts3d, intensities = None, labels = None):
00042 ROS_pointcloud = sensor_msgs.msg.PointCloud()
00043 ROS_pointcloud.points = []
00044 for point in np.asarray(pts3d.T):
00045 ROS_pointcloud.points += [geometry_msgs.msg.Point32(point[0],point[1],point[2])]
00046
00047 intensity_channel = sensor_msgs.msg.ChannelFloat32()
00048 intensity_channel.name = 'intensities'
00049 if intensities != None:
00050 for value in intensities:
00051 intensity_channel.values += [value]
00052
00053 label_channel = sensor_msgs.msg.ChannelFloat32()
00054 label_channel.name = 'labels'
00055 if labels != None:
00056 for value in labels:
00057 label_channel.values += [value]
00058
00059
00060 ROS_pointcloud.channels = [intensity_channel, label_channel]
00061 return ROS_pointcloud
00062
00063 def convert_ROS_pointcloud_to_pointcloud(ROS_pointcloud):
00064 intensities = []
00065 labels = []
00066 pts3d = []
00067 for point in ROS_pointcloud.points:
00068 pts3d += [[point.x, point.y, point.z]]
00069 pts3d = np.array(pts3d).T
00070
00071
00072 for value in ROS_pointcloud.channels[0].values:
00073 intensities += [value]
00074 intensities = np.array(intensities)
00075
00076 for value in ROS_pointcloud.channels[1].values:
00077 labels += [value]
00078 labels = np.array(labels)
00079
00080 return pts3d, intensities, labels
00081
00082
00083 def convert_cvimage_to_ROS(image):
00084 imgTmp = cv.cvCloneImage(image)
00085 imNP = ut.cv2np(imgTmp,format='BGR')
00086 ROS_image = np.reshape(imNP,(1,-1))[0]
00087
00088 return ROS_image
00089
00090 def convert_ROS_image_to_cvimage(ROS_image, width, height):
00091 ROS_image = np.array(ROS_image, dtype='uint8')
00092 imNP = np.reshape(ROS_image,(height,width, 3))
00093 cvimage = ut.np2cv(imNP)
00094
00095 return cvimage
00096
00097
00098
00099 def convert_polygon_to_ROS(polygon):
00100 ROS_polygon = geometry_msgs.msg.Polygon()
00101 ROS_polygon.points = []
00102 for point in polygon.get_points():
00103 ROS_polygon.points += [geometry_msgs.msg.Point32(point[0], point[1], 0.)]
00104 return ROS_polygon
00105
00106
00107 def convert_ROS_polygon_to_polygon(ROS_polygon):
00108 polygon = label_object()
00109 for point in ROS_polygon.points:
00110 polygon.add_point((point.x,point.y))
00111 return polygon