コード例 #1
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)
コード例 #2
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)
コード例 #3
0
ファイル: get_plane.py プロジェクト: Utaro-M/rm_debris_vision
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")
コード例 #4
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)
コード例 #5
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)
コード例 #6
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)
コード例 #7
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)
コード例 #8
0
 def publishPolygons(self, event=None):
     pmsg = PolygonArray()
     pmsg.header.stamp = rospy.Time.now()
     cmsg = ModelCoefficientsArray()
     cmsg.header.stamp = pmsg.header.stamp
     for i in range(10):
         pmsg.polygons.append(PolygonStamped())
         pmsg.polygons[i].header.stamp = pmsg.header.stamp
         pmsg.polygons[i].header.frame_id = str(i)
         cmsg.coefficients.append(ModelCoefficients())
         cmsg.coefficients[i].header.stamp = cmsg.header.stamp
         cmsg.coefficients[i].header.frame_id = str(i)
     pmsg.likelihood = [1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0]
     self.pub_polygon.publish(pmsg)
     self.pub_coefficients.publish(cmsg)
コード例 #9
0
def imu_cb(imu):
    ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z
    # rospy.loginfo("%f %f %f" % (ax, ay, az))
    if az > 0.1:
        dx, dy = [-az, 0, ax], [0, -az, ay]
    elif ay > 0.1:
        dx, dy = [-ay, ax, 0], [0, az, -ay]
    else:
        dx, dy = [ay, -ax, 0], [az, 0, -az]
    PStamped = PolygonStamped()
    for x, y in [[-10, -10], [-10, 10], [10, 10], [10, -10]]:
        PStamped.polygon.points.append(
            Point32(x * dx[0] + y * dy[0], x * dx[1] + y * dy[1],
                    x * dx[2] + y * dy[2]))

    PStamped.header = imu.header
    PArrayPub.publish(PolygonArray(imu.header, [PStamped]))
    normal_array = np.array([ax, ay, az, 0])
    normal_array = normal_array / np.linalg.norm(x)
    MStamped = ModelCoefficients(imu.header, normal_array)
    MArrayPub.publish(ModelCoefficientsArray(imu.header, [MStamped]))