00001
00002
00003 import rospy
00004 from sensor_msgs.msg import PointCloud2
00005 from sensor_msgs.point_cloud2 import create_cloud_xyz32
00006 from std_msgs.msg import Header
00007 from jsk_recognition_msgs.msg import (PolygonArray, ModelCoefficientsArray,
00008 BoundingBox, BoundingBoxArray)
00009 from pcl_msgs.msg import ModelCoefficients
00010 from geometry_msgs.msg import PolygonStamped, Point32
00011 from std_srvs.srv import Empty
00012 import numpy as np
00013 from math import pi, cos, sin
00014
00015 def generatePoints0():
00016 plane_points = []
00017 object_points = []
00018 dx = 0.02
00019 for x in np.arange(-1, 1, dx):
00020 for y in np.arange(-1, 1, dx):
00021 plane_points.append([x, y, 0.0])
00022 for x in np.arange(-0.1, 0.1, dx):
00023 for y in np.arange(-0.1, 0.1, dx):
00024 object_points.append([x, y, 0.2])
00025 return plane_points + object_points
00026
00027 def generatePoints1():
00028 plane_points = []
00029 object_points = []
00030 dx = 0.02
00031 for x in np.arange(-1, 1, dx):
00032 for y in np.arange(-1, 1, dx):
00033 plane_points.append([x, y, 0.0])
00034 for x in np.arange(-0.3, 0.2, dx):
00035 for y in np.arange(-0.1, 0.1, dx):
00036 object_points.append([x, y, 0.2])
00037 return plane_points + object_points
00038
00039 def generatePoints2():
00040 plane_points = []
00041 object_points = []
00042 dx = 0.02
00043 for x in np.arange(-1, 1, dx):
00044 for y in np.arange(-1, 1, dx):
00045 plane_points.append([x, y, 0.0])
00046 for x in np.arange(-0.1, 0.1, dx):
00047 for y in np.arange(-0.1, 0.1, dx):
00048 object_points.append([x, y, 0.2])
00049 for x in np.arange(-0.4, -0.2, dx):
00050 for y in np.arange(-0.1, 0.1, dx):
00051 object_points.append([x, y, 0.2])
00052
00053 return plane_points + object_points
00054
00055 def generatePoints3():
00056 plane_points = []
00057 object_points = []
00058 dx = 0.02
00059 for x in np.arange(-1, 1, dx):
00060 for y in np.arange(-1, 1, dx):
00061 plane_points.append([x, y, 0.0])
00062 for x in np.arange(-0.1, 0.1, dx):
00063 for y in np.arange(-0.1, 0.1, dx):
00064 object_points.append([x, y, 0.2])
00065 for z in np.arange(0.0, 0.2, dx):
00066 for y in np.arange(-0.1, 0.1, dx):
00067 object_points.append([0.1, y, z])
00068 object_points.append([-0.1, y, z])
00069 for z in np.arange(0.0, 0.2, dx):
00070 for x in np.arange(-0.1, 0.1, dx):
00071 object_points.append([x, 0.1, z])
00072 object_points.append([x, -0.1, z])
00073
00074 return plane_points + object_points
00075
00076 def generatePointsDoor():
00077 plane_points = []
00078 object_points = []
00079 dx = 0.02
00080 for y in np.arange(-0.5, 0.5, dx):
00081 for z in np.arange(0, 2, dx):
00082 plane_points.append([0.0, y, z])
00083 for y in np.arange(0.2, 0.4, dx):
00084 for z in np.arange(0.8, 0.9, dx):
00085 object_points.append([-0.1, y, z])
00086 return plane_points + object_points
00087
00088 def generatePoints(model_index):
00089 if model_index == 0:
00090 return generatePoints0()
00091 elif model_index == 1:
00092 return generatePoints1()
00093 elif model_index == 2:
00094 return generatePoints2()
00095 elif model_index == 3:
00096 return generatePoints3()
00097 elif model_index == 4:
00098 return generatePointsDoor()
00099
00100 def generatePolygons(header, model_index):
00101 polygon = PolygonArray()
00102 polygon.header = header
00103 polygon.polygons = [PolygonStamped()]
00104 polygon.polygons[0].header = header;
00105 coef = ModelCoefficientsArray()
00106 coef.coefficients = [ModelCoefficients()]
00107 coef.header = header
00108 if model_index in [0, 1, 2, 3]:
00109
00110 polygon.polygons[0].polygon.points = [Point32(x=1.0, y=1.0),
00111 Point32(x=-1.0, y=1.0),
00112 Point32(x=-1.0, y=-1.0),
00113 Point32(x=1.0, y=-1.0)]
00114 coef.coefficients[0].header = header
00115 coef.coefficients[0].values = [0, 0, 1, 0]
00116 elif model_index == 4:
00117 polygon.polygons[0].polygon.points = [Point32(x=0.0, y=-0.5, z=0.0),
00118 Point32(x=0.0, y=-0.5, z=2.0),
00119 Point32(x=0.0, y=0.5, z=2.0),
00120 Point32(x=0.0, y=0.5, z=0.0)]
00121
00122 coef.coefficients[0].header = header
00123 coef.coefficients[0].values = [-1, 0, 0, 0]
00124 return (polygon, coef)
00125
00126
00127 def candidateBoxes(header, model_index):
00128 box_array = BoundingBoxArray()
00129 box_array.header.stamp = header.stamp
00130 box_array.header.frame_id = "odom"
00131 dx = 0.1
00132 for y in np.arange(0.0, 0.6, dx):
00133 for z in np.arange(0.7, 1.0, dx):
00134 for x in np.arange(0.0, -0.5, -dx):
00135 box = BoundingBox()
00136 box.header = box_array.header
00137 box.pose.orientation.w = 1.0
00138 box.pose.position.x = x
00139 box.pose.position.y = y
00140 box.pose.position.z = z
00141 box.dimensions.x = 0.1
00142 box.dimensions.y = 0.1
00143 box.dimensions.z = 0.1
00144 box_array.boxes.append(box)
00145 return box_array
00146
00147 if __name__ == "__main__":
00148 rospy.init_node("sample_simulate_tabletop_cloud")
00149 pub = rospy.Publisher("~output", PointCloud2)
00150 pub_polygon = rospy.Publisher("~output/polygon", PolygonArray)
00151 pub_coef = rospy.Publisher("~output/coef", ModelCoefficientsArray)
00152 pub_boxes = rospy.Publisher("~output/candidate_boxes", BoundingBoxArray)
00153 r = rospy.Rate(10)
00154 counter = 0
00155 model_index = 4
00156 reset = False
00157 while not rospy.is_shutdown():
00158 points = generatePoints(model_index)
00159 header = Header()
00160 header.frame_id = "odom"
00161 header.stamp = rospy.Time.now()
00162 msg = create_cloud_xyz32(header, points)
00163 pub.publish(msg)
00164 (polygon, coef) = generatePolygons(header, model_index)
00165 pub_polygon.publish(polygon)
00166 pub_coef.publish(coef)
00167 counter = counter + 1
00168 pub_boxes.publish(candidateBoxes(header, model_index))
00169 if reset and counter > 1.0 / r.sleep_dur.to_sec() * 10:
00170 reset = rospy.ServiceProxy("/plane_supported_cuboid_estimator/reset", Empty)
00171 counter = 0
00172 model_index = model_index + 1
00173 if model_index >= 5:
00174 model_index = 0
00175 reset()
00176 r.sleep()