simulate_primitive_shape_on_plane.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Copyright: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
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()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50