sample_simulate_tabletop_cloud.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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         # Rectangle
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         # polygon.polygons[0].polygon.points.reverse()
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, queue_size=1)
00150     pub_polygon = rospy.Publisher(
00151         "~output/polygon", PolygonArray, queue_size=1)
00152     pub_coef = rospy.Publisher(
00153         "~output/coef", ModelCoefficientsArray, queue_size=1)
00154     pub_boxes = rospy.Publisher(
00155         "~output/candidate_boxes", BoundingBoxArray, queue_size=1)
00156     r = rospy.Rate(10)
00157     counter = 0
00158     model_index = 4
00159     reset = False
00160     while not rospy.is_shutdown():
00161         points = generatePoints(model_index)
00162         header = Header()
00163         header.frame_id = "odom"
00164         header.stamp = rospy.Time.now()
00165         msg = create_cloud_xyz32(header, points)
00166         pub.publish(msg)
00167         (polygon, coef) = generatePolygons(header, model_index)
00168         pub_polygon.publish(polygon)
00169         pub_coef.publish(coef)
00170         counter = counter + 1
00171         pub_boxes.publish(candidateBoxes(header, model_index))
00172         if reset and counter > 1.0 / r.sleep_dur.to_sec() * 10:
00173             reset = rospy.ServiceProxy("/plane_supported_cuboid_estimator/reset", Empty)
00174             counter = 0
00175             model_index = model_index + 1
00176             if model_index >= 5:
00177                 model_index = 0
00178             reset()
00179         r.sleep()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:45