00001
00002
00003
00004
00005 import numpy as np
00006 import rospy
00007 from sensor_msgs.msg import PointCloud2, PointField
00008 from geometry_msgs.msg import PolygonStamped
00009 from geometry_msgs.msg import Point32
00010 from jsk_recognition_msgs.msg import PolygonArray
00011 from sensor_msgs.point_cloud2 import create_cloud
00012 from std_msgs.msg import Header
00013
00014
00015 def create_cloud_xyzrgb(header, xyzrgb):
00016 fields = [
00017 PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
00018 PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
00019 PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
00020 PointField(name='rgba', offset=16, datatype=PointField.UINT32, count=1),
00021 ]
00022 return create_cloud(header, fields, xyzrgb)
00023
00024
00025 class TestPrimitiveClassifier(object):
00026 def __init__(self):
00027 self.pub_plane = rospy.Publisher("~output/plane", PolygonArray, queue_size=10)
00028 self.pub_cloud = rospy.Publisher("~output/cloud", PointCloud2, queue_size=10)
00029 self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb)
00030
00031 def generate_plane(self, header,
00032 xmin=-0.0, xmax=1.0,
00033 ymin=-1.0, ymax=1.0):
00034 msg = PolygonArray()
00035 msg.header = header
00036 p = PolygonStamped()
00037 p.header = header
00038 p.polygon.points = [Point32(x=xmin, y=ymin),
00039 Point32(x=xmax, y=ymin),
00040 Point32(x=xmax, y=ymax),
00041 Point32(x=xmin, y=ymax)]
00042 msg.polygons.append(p)
00043 return msg
00044
00045 def generate_box_cloud(self, d=0.01, noise=0.0015,
00046 xmin=0.2, xmax=0.4,
00047 ymin=-0.05, ymax=0.05,
00048 zmin=0.01, zmax=0.1):
00049 pts = []
00050
00051 for x in np.arange(xmin, xmax, d):
00052 for y in np.arange(ymin, ymax, d):
00053 pts.append([x, y, zmax] + np.random.normal(0.0, noise, 3))
00054
00055 for z in np.arange(zmin, zmax, d):
00056 for y in np.arange(ymin, ymax, d):
00057 pts.append([xmin, y, z] + np.random.normal(0.0, noise, 3))
00058 return pts
00059
00060 def generate_circle_cloud(self, d=0.01, noise=0.0015,
00061 radius=0.05,
00062 xpos=0.2, ypos=0.2,
00063 rmin=np.pi/2., rmax=np.pi*3./2,
00064 zmin=0.01, zmax=0.15):
00065 pts = []
00066
00067 for z in np.arange(zmin, zmax, d):
00068 for rad in np.arange(rmin, rmax, d*2.):
00069 pts.append([xpos + radius * np.cos(rad),
00070 ypos + radius * np.sin(rad),
00071 z] + np.random.normal(0.0, noise, 3))
00072
00073 for r in np.arange(0, radius, d):
00074 for rad in np.arange(0, np.pi * 2., d*2.):
00075 pts.append([xpos + r * np.cos(rad),
00076 ypos + r * np.sin(rad),
00077 zmax] + np.random.normal(0.0, noise, 3))
00078
00079 return pts
00080
00081 def generate_plane_cloud(self, xmin, xmax, ymin, ymax):
00082 d = 0.02
00083 plane = []
00084 for x in np.arange(xmin, xmax, d):
00085 for y in np.arange(ymin, ymax, d):
00086 plane.append([x, y, 1.0])
00087 return plane
00088
00089 def timer_cb(self, args=None):
00090 header = Header(frame_id="camera_rgb_optical_frame", stamp=rospy.Time.now())
00091
00092 plane = self.generate_plane(header)
00093 self.pub_plane.publish(plane)
00094
00095 pts = [pt.tolist() + [0xff0000ff] for pt in self.generate_box_cloud()]
00096 pts += [pt.tolist() + [0xffffffff] for pt in self.generate_circle_cloud()]
00097 cloud = create_cloud_xyzrgb(header, pts)
00098 self.pub_cloud.publish(cloud)
00099
00100
00101 if __name__ == '__main__':
00102 rospy.init_node("test_primitive_classifier")
00103 c = TestPrimitiveClassifier()
00104 rospy.spin()