def generatePolygons(header, model_index):
    polygon = PolygonArray()
    polygon.header = header
    polygon.polygons = [PolygonStamped()]
    polygon.polygons[0].header = header;
    coef = ModelCoefficientsArray()
    coef.coefficients = [ModelCoefficients()]
    coef.header = header
    if model_index in [0, 1, 2, 3]:
        # Rectangle
        polygon.polygons[0].polygon.points = [Point32(x=1.0, y=1.0),
                                              Point32(x=-1.0, y=1.0),
                                              Point32(x=-1.0, y=-1.0),
                                              Point32(x=1.0, y=-1.0)]
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [0, 0, 1, 0]
    elif model_index == 4:
        polygon.polygons[0].polygon.points = [Point32(x=0.0, y=-0.5, z=0.0),
                                              Point32(x=0.0, y=-0.5, z=2.0),
                                              Point32(x=0.0, y=0.5, z=2.0),
                                              Point32(x=0.0, y=0.5, z=0.0)]
        # polygon.polygons[0].polygon.points.reverse()
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [-1, 0, 0, 0]
    return (polygon, coef)
    def dynamic_custom_polygon_demo(self, zones_list):

        #rospy.init_node("polygon_array_dynamic")
        # /polygon_array_dynamic/output
        pub = rospy.Publisher("/polygon_array_dynamic/output",
                              PolygonArray,
                              queue_size=1)
        r = rospy.Rate(5)
        count = 0
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = []
        msg.labels = []
        msg.likelihood = []
        for i in zones_list:
            #print(i)
            msg.polygons.append(self.DynamicPolygon(header, i))
            msg.labels.append(count)
            msg.likelihood.append(np.random.ranf())
            count += 1

        rospy.loginfo("Publishing box")
        pub.publish(msg)
        r.sleep()
示例#3
0
def generatePolygons(header, model_index):
    polygon = PolygonArray()
    polygon.header = header
    polygon.polygons = [PolygonStamped()]
    polygon.polygons[0].header = header
    coef = ModelCoefficientsArray()
    coef.coefficients = [ModelCoefficients()]
    coef.header = header
    if model_index in [0, 1, 2, 3]:
        # Rectangle
        polygon.polygons[0].polygon.points = [
            Point32(x=1.0, y=1.0),
            Point32(x=-1.0, y=1.0),
            Point32(x=-1.0, y=-1.0),
            Point32(x=1.0, y=-1.0)
        ]
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [0, 0, 1, 0]
    elif model_index == 4:
        polygon.polygons[0].polygon.points = [
            Point32(x=0.0, y=-0.5, z=0.0),
            Point32(x=0.0, y=-0.5, z=2.0),
            Point32(x=0.0, y=0.5, z=2.0),
            Point32(x=0.0, y=0.5, z=0.0)
        ]
        # polygon.polygons[0].polygon.points.reverse()
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [-1, 0, 0, 0]
    return (polygon, coef)
示例#4
0
def sample_demo():
    """
    Draws a star, circle, rectangle and square 
    """
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [
            SquarePolygon(header),
            RectanglePolygon(header),
            CirclePolygon(header),
            StarPolygon(header)
        ]
        # rospy.loginfo("number of point to enter :"+ str(msg.polygons))
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf()
        ]
        pub.publish(msg)
        r.sleep()
示例#5
0
def sub_bb(msg):
    rospy.loginfo("sub_bb cb")
    bb = msg.boxes[0]
    x1 = bb.pose.position.x - bb.dimensions.x
    x2 = bb.pose.position.x + bb.dimensions.x
    y1 = bb.pose.position.y - bb.dimensions.y
    y2 = bb.pose.position.y + bb.dimensions.y
    z1 = bb.pose.position.z - bb.dimensions.z
    z2 = bb.pose.position.z + bb.dimensions.z
    ps = PolygonStamped()
    if face == "xy":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y2, z1))
        ps.polygon.points.append(Point32(x2, y2, z1))
        ps.polygon.points.append(Point32(x2, y1, z1))
    elif face == "yz":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x1, y2, z2))
        ps.polygon.points.append(Point32(x1, y2, z1))
    elif face == "zx":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z1))
    else:
        rospy.loginfo("Face Error!")
    ps.header = msg.header
    pa = PolygonArray()
    pa.header = msg.header
    pa.polygons = [ps]
    rospy.loginfo('pa:%s', pa)
    pub.publish(pa)
示例#6
0
def timer_cb(msg1, msg2, msg3):
    global polygons
    # print(msg1.header)
    # info_msg_polygon=msg2
    info_msg_polygon = PolygonArray()
    # info_msg_coefficient=msg3
    info_msg_coefficient = ModelCoefficientsArray()
    planes_indices = msg1.cluster_indices
    # print('len(planes_indices)', len(planes_indices))
    ind = 0
    longest_ind = 0
    longest_length = 0
    if (len(planes_indices) >= 1):
        for ind, plane in enumerate(planes_indices):
            length = len(plane.indices)
            if (length > longest_length):
                longest_ind = ind
                longest_length = length
                ind += 1
        # print(longest_ind)
        # # print (msg2.polygons)
        # print (len(msg2.polygons))
        # print (len(planes_indices))
        info_msg_polygon.polygons = msg2.polygons
        info_msg_polygon.header = msg2.header
        info_msg_coefficient.coefficients = [msg3.coefficients[longest_ind]]
        info_msg_coefficient.header = msg3.header
        pub_info_polygon.publish(info_msg_polygon)
        pub_info_coefficient.publish(info_msg_coefficient)
        # print('len(msg2.polygons)', len(msg2.polygons))
        polygons = msg2.polygons[longest_ind]
    if polygons is not None:
        pub_info_polygonstamped.publish(polygons)
    else:
        print("finding planes")
示例#7
0
def callback(msg):
    output_msg = PolygonArray()
    output_msg.header = msg.header
    output_msg.polygons = msg.polygons
    output_msg.labels = msg.labels
    output_msg.likelihood = msg.likelihood

    # fill empty
    output_msg.header.stamp = rospy.Time(0)
    for i in range(len(output_msg.polygons)):
        output_msg.polygons[i].header = output_msg.header

    pub.publish(output_msg)
示例#8
0
def dynamic_custom_polygon_demo():

    rospy.init_node("polygon_array_dynamic")
    # /polygon_array_dynamic/output
    pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
    r = rospy.Rate(5)
    #while not rospy.is_shutdown():
    msg = PolygonArray()
    header = Header()
    header.frame_id = "world"
    header.stamp = rospy.Time.now()
    msg.header = header
    msg.polygons = [DynamicPolygon(header)]
    msg.labels = [0]
    msg.likelihood = [np.random.ranf()]
    pub.publish(msg)
    r.sleep()
 def callback(self, msg, msg_coef):
     if len(msg.polygons) > 0:
         #self._pub.publish(msg.histograms[0])
         max_index = max(xrange(len(msg.polygons)), key=lambda i: msg.likelihood[i])
         res = PolygonArray()
         res.header = msg.header
         res.polygons = [msg.polygons[max_index]]
         res.likelihood = [msg.likelihood[max_index]]
         if msg.likelihood[max_index] < self._min_likelihood:
             rospy.loginfo("Ignore result because of too small likelihood: {0} < {1}".format(
                 msg.likelihood[max_index],
                 self._min_likelihood))
             return
         self._pub.publish(res)
         res_coef = ModelCoefficientsArray()
         res_coef.header = msg.header
         res_coef.coefficients = [msg_coef.coefficients[max_index]]
         self._pub_coef.publish(res_coef)
示例#10
0
 def callback(self, msg, msg_coef):
     if len(msg.polygons) > 0:
         #self._pub.publish(msg.histograms[0])
         max_index = max(xrange(len(msg.polygons)),
                         key=lambda i: msg.likelihood[i])
         res = PolygonArray()
         res.header = msg.header
         res.polygons = [msg.polygons[max_index]]
         res.likelihood = [msg.likelihood[max_index]]
         if msg.likelihood[max_index] < self._min_likelihood:
             rospy.loginfo(
                 "Ignore result because of too small likelihood: {0} < {1}".
                 format(msg.likelihood[max_index], self._min_likelihood))
             return
         self._pub.publish(res)
         res_coef = ModelCoefficientsArray()
         res_coef.header = msg.header
         res_coef.coefficients = [msg_coef.coefficients[max_index]]
         self._pub_coef.publish(res_coef)
示例#11
0
def callback(polygon_msg, coeff_msg):
    print "callback"
    # odom->ground
    max_val = -1.0;
    max_index = None
    for i in range(len(polygon_msg.polygons)):
        val = polygon_msg.likelihood
        if max_val < val:
            max_val = val
            max_index = i
    if max_index != None:
        out_poly_msg = PolygonArray()
        out_poly_msg.header = polygon_msg.header
        out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
        pub_poly.publish(out_poly_msg)
        out_coeff_msg = ModelCoefficientsArray()
        out_coeff_msg.header = coeff_msg.header
        out_coeff_msg.coefficients= [coeff_msg.coefficients[max_index]]
        pub_coeff.publish(out_coeff_msg)
def callback(polygon_msg, coeff_msg):
    print "callback"
    # odom->ground
    max_val = -1.0
    max_index = None
    for i in range(len(polygon_msg.polygons)):
        val = polygon_msg.likelihood
        if max_val < val:
            max_val = val
            max_index = i
    if max_index != None:
        out_poly_msg = PolygonArray()
        out_poly_msg.header = polygon_msg.header
        out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
        pub_poly.publish(out_poly_msg)
        out_coeff_msg = ModelCoefficientsArray()
        out_coeff_msg.header = coeff_msg.header
        out_coeff_msg.coefficients = [coeff_msg.coefficients[max_index]]
        pub_coeff.publish(out_coeff_msg)
    p.header = header
    p.polygon.points = [Point32(x= .0000, y= 1.0000 + 3.0),
                        Point32(x= .2245, y= .3090 + 3.0),
                        Point32(x= .9511, y= .3090 + 3.0),
                        Point32(x= .3633, y= -.1180 + 3.0),
                        Point32(x= .5878, y= -.8090 + 3.0),
                        Point32(x= .0000, y= -.3820 + 3.0),
                        Point32(x= -.5878, y= -.8090 + 3.0),
                        Point32(x= -.3633, y= -.1180 + 3.0),
                        Point32(x= -.9511, y= .3090 + 3.0),
                        Point32(x= -.2245, y= .3090 + 3.0)]
    return p
    
if __name__ == "__main__":
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray)
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [SquarePolygon(header),
                        RectanglePolygon(header),
                        CirclePolygon(header),
                        StarPolygon(header)]
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [np.random.ranf(), np.random.ranf(), np.random.ranf(), np.random.ranf()]
        pub.publish(msg)
示例#14
0
if __name__ == "__main__":
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray)
    r = rospy.Rate(10)

    br = tf.TransformBroadcaster()

    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [
            SquarePolygon(header),
            RectanglePolygon(header),
            CirclePolygon(header),
            StarPolygon(header)
        ]
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf()
        ]
        pub.publish(msg)

        br.sendTransform((0, 0, 0),
                         tf.transformations.quaternion_from_euler(0, 0, 0),
                         rospy.Time.now(), 'map', 'world')
def timer_cb(msg1):
    plarray = PolygonArray()
    plarray.header = msg1.header
    plarray.polygons = [msg1]
    pub_info_polygonstamped.publish(plarray)