Example #1
0
def problem4(data, outDir, outName, targetValue):
    resultOutLoc = os.path.join(outDir, '4', outName + '.txt')
    if os.path.exists(resultOutLoc):
        return
    if not os.path.exists(os.path.dirname(resultOutLoc)):
        os.makedirs(os.path.dirname(resultOutLoc))
    
    p = phi(data[TRAIN])
    tVect = tListToTVector(data[TRAIN_LABELS])
    eignValues = eig(dot(p.transpose(), p))[0]
    
    convergenceDelta = .1
    lastAlpha = 0.0
    lastBeta = 0.0
    alpha = 1.0
    beta = 1.0
    while abs(lastAlpha - alpha) > convergenceDelta or abs(lastBeta - beta) > convergenceDelta:
        lastAlpha = alpha
        lastBeta = beta
        m = mN(lastAlpha, lastBeta, p, tVect)
        g = gamma(lastAlpha, lastBeta, eignValues)
        alpha = Float64(alphaF(g, m))
        beta = Float64(betaF(g, m, data))
    testMSE = MSE(data[TEST], mN(alpha, beta, p, tVect), data[TEST_LABELS])
    
    outFile = open(resultOutLoc, 'w')
    outFile.write('\t'.join([str(alpha), str(beta), str(testMSE)]))
    outFile.close()
Example #2
0
    def _compare_output_betas(self, output_path):
        print "Comparing output..."
        if len(self._errors[output_path]) == 0:
            beta_x_twiss = metaclass.twiss(
                os.path.join(output_path, "getbetax.out"))
            beta_y_twiss = metaclass.twiss(
                os.path.join(output_path, "getbetay.out"))

            for index in range(len(beta_x_twiss.NAME)):
                rel_error = abs(
                    (beta_x_twiss.BETX[index] - beta_x_twiss.BETXMDL[index]) /
                    beta_x_twiss.BETXMDL[index])
                self.assertTrue(
                    rel_error < MAX_BETA_REL_ERR,
                    "Relative error too big found in: " +
                    beta_x_twiss.NAME[index] + " (" + str(rel_error) + ")")
            for index in range(len(beta_y_twiss.NAME)):
                rel_error = abs(
                    (beta_y_twiss.BETY[index] - beta_y_twiss.BETYMDL[index]) /
                    beta_y_twiss.BETYMDL[index])
                self.assertTrue(
                    rel_error < MAX_BETA_REL_ERR,
                    "Relative error too big found in: " +
                    beta_y_twiss.NAME[index] + " (" + str(rel_error) + ")")
        else:
            print "Manual errors in the madx job: skipping betas check"
Example #3
0
 def _compare_match_errors(self):
     for sbs_match_output_path in self._sbs_match_output_paths:
         print "Checking path", sbs_match_output_path, "..."
         beam1_path = sbs_match_output_path.replace("match", "beam1")
         beam2_path = sbs_match_output_path.replace("match", "beam2")
         errors_beam1 = self._errors[beam1_path]
         errors_beam2 = self._errors[beam2_path]
         for ip in IP_SEGMENTS_TO_RUN:
             changeparameters_path = os.path.join(
                 sbs_match_output_path,
                 "changeparametersIP" + str(ip) + ".madx")
             with open(changeparameters_path, "r") as correction_lines:
                 for correction_line in correction_lines:
                     correction_split = correction_line.replace(
                         ";", "").replace("d", "").replace("\n",
                                                           "").split("=")
                     variable, value = correction_split[0].strip(), float(
                         correction_split[1])
                     if variable in errors_beam1:
                         difference = abs(value - errors_beam1[variable])
                         self.assertTrue(
                             difference < MAX_CORRECTION_DEVIATION,
                             "Wrong correction for variable: " + variable)
                         print "Error for variable", variable, "corrected."
                     elif variable in errors_beam2:
                         difference = abs(value - errors_beam2[variable])
                         self.assertTrue(
                             difference < MAX_CORRECTION_DEVIATION,
                             "Wrong correction for variable: " + variable)
                         print "Error for variable", variable, "corrected."
                     else:
                         self.assertTrue(
                             value < MAX_CORRECTION_DEVIATION,
                             "Wrong correction for variable: " + variable)
     print "Done\n"
Example #4
0
 def _compare_match_errors(self):
     for sbs_match_output_path in self._sbs_match_output_paths:
         print "Checking path", sbs_match_output_path, "..."
         beam1_path = sbs_match_output_path.replace("match", "beam1")
         beam2_path = sbs_match_output_path.replace("match", "beam2")
         errors_beam1 = self._errors[beam1_path]
         errors_beam2 = self._errors[beam2_path]
         for ip in IP_SEGMENTS_TO_RUN:
             changeparameters_path = os.path.join(sbs_match_output_path, "changeparametersIP" + str(ip) + ".madx")
             with open(changeparameters_path, "r") as correction_lines:
                 for correction_line in correction_lines:
                     correction_split = (
                         correction_line.replace(";", "").replace("d", "").replace("\n", "").split("=")
                     )
                     variable, value = correction_split[0].strip(), float(correction_split[1])
                     if variable in errors_beam1:
                         difference = abs(value - errors_beam1[variable])
                         self.assertTrue(
                             difference < MAX_CORRECTION_DEVIATION, "Wrong correction for variable: " + variable
                         )
                         print "Error for variable", variable, "corrected."
                     elif variable in errors_beam2:
                         difference = abs(value - errors_beam2[variable])
                         self.assertTrue(
                             difference < MAX_CORRECTION_DEVIATION, "Wrong correction for variable: " + variable
                         )
                         print "Error for variable", variable, "corrected."
                     else:
                         self.assertTrue(
                             value < MAX_CORRECTION_DEVIATION, "Wrong correction for variable: " + variable
                         )
     print "Done\n"
Example #5
0
def top_genre_comp(test_df, seed_df, g1, g2):
    '''Compare the top genres in two diff genre dfs'''
            
    sum_edge_weight = 1

    for genre in seed_df.genre[g1: g2]:                
        sum_edge_weight = 0
            
        if any(test_df.genre == genre):
                                
            seed_wf = seed_df.weight[seed_df.genre == genre]
            seed_wf = seed_wf.values[0]
                    
            test_wf = test_df.weight[test_df.genre ==  genre] 
            test_wf = test_wf.values[0]
            
            diff = seed_wf/test_wf
            m1 = abs(1 - diff)
            
            if m1 == 0:
                m1 = 1
            else:
                pass
                                    
        else:   
            m1 = 1
                        
        sum_edge_weight += m1

    this_edge_weight = 1/sum_edge_weight    
    return this_edge_weight
Example #6
0
    def cb_ReadCompass(self, req):
        '''
        making api call to the middleware which resides outside ROS
        output parameters: 
         - theta
        '''
        self._readCompass()

        resp = CompassResponse()

        if self._CYAN_is_leftSide:
            resp.theta.data = abs(
                mod(360.0 - self._angle + self._homegoalAngle, 360.0))
        else:
            resp.theta.data = abs(
                mod(360.0 - self._angle + self._homegoalAngle + 180.0, 360.0))
        return resp
    def call_back(self, flag):
        veto = True
        if (flag and self.humanPublish.get_num_connections() > 0) or \
                (flag and self.humanInFront.get_num_connections() > 0) or veto:
            res = self.laserResoloution
            ii1 = self.leg_mid_point_inclass
            ii9 = self.torso_mid_point_inclass
            range1 = self.leg_mid_range_inclass
            range9 = self.torso_mid_range_inclass

            rad4 = ((ii1 * res * pi) / 180) - pi  # "-pi" is a TF
            rad5 = ((ii9 * res * pi) / 180) - pi  # "-pi" is a TF

            xleg = range1 * cos(rad4)
            yleg = range1 * sin(rad4)

            xtor = range9 * cos(rad5)
            ytor = range9 * sin(rad5)

            if ii1 != 0 and ii9 != 0 and range1 != 0 and range9 != 0:
                # print "x,y leg", xleg, yleg, "x,y torso", xtor, ytor

                if (abs(xleg - xtor) < 0.4) and (abs(yleg - ytor) < 0.4):
                    self.count = self.count + 1
                    print(self.count, "Human Detected")
                    self.humanPublish.publish(True)  # for scenario
                    if yleg < 1.5 and xleg < 1:
                        print "there is human in front robina"
                        self.humanInFront.publish(True)

                    i = 0
                    positions = []
                    while i < 1:  # :D
                        positions.append([])
                        positions[0].append(xtor)
                        positions[0].append(ytor)

                        i = i + 1

                    self.visualize(positions)

        self.t1 = True
        self.t2 = True
        self.t3 = True
        self.t4 = True
Example #8
0
def betaF(g, mN, data):
    summation = 0.0
    N = len(data[TRAIN_LABELS])
    for n in range(N):
        summation += (squeeze(data[TRAIN_LABELS][n] - dot(mN.transpose(), data[TRAIN][n])) ** 2)
    if abs(N-g) < .001:
        g += .001
    oneOverBeta = summation / (N-g)
    b = 1.0 / oneOverBeta
    return b
Example #9
0
    def _compare_output_betas(self, output_path):
        print "Comparing output..."
        if len(self._errors[output_path]) == 0:
            beta_x_twiss = metaclass.twiss(os.path.join(output_path, "getbetax.out"))
            beta_y_twiss = metaclass.twiss(os.path.join(output_path, "getbetay.out"))

            for index in range(len(beta_x_twiss.NAME)):
                rel_error = abs((beta_x_twiss.BETX[index] - beta_x_twiss.BETXMDL[index]) / beta_x_twiss.BETXMDL[index])
                self.assertTrue(
                    rel_error < MAX_BETA_REL_ERR,
                    "Relative error too big found in: " + beta_x_twiss.NAME[index] + " (" + str(rel_error) + ")",
                )
            for index in range(len(beta_y_twiss.NAME)):
                rel_error = abs((beta_y_twiss.BETY[index] - beta_y_twiss.BETYMDL[index]) / beta_y_twiss.BETYMDL[index])
                self.assertTrue(
                    rel_error < MAX_BETA_REL_ERR,
                    "Relative error too big found in: " + beta_y_twiss.NAME[index] + " (" + str(rel_error) + ")",
                )
        else:
            print "Manual errors in the madx job: skipping betas check"
Example #10
0
def problem3(data, outDir, outName, targetValue):
    resultOutLoc = os.path.join(outDir, '3', outName + '.txt')
    if os.path.exists(resultOutLoc):
        return
    if not os.path.exists(os.path.dirname(resultOutLoc)):
        os.makedirs(os.path.dirname(resultOutLoc))
    avgMSEList = []
    numFolds = 10
    trainDataSize = len(data[TRAIN])
    foldSize = len(data[TRAIN]) / numFolds
    for l in range(151):
        lMSEList = []
        for foldNum in range(numFolds):
            trainFoldIndexes = range(foldSize * foldNum) + range(foldSize * (foldNum + 1), trainDataSize)
            testFoldIndexes = range(foldSize * foldNum, foldSize * (foldNum + 1))
            trainFoldData = selectSample(data[TRAIN], trainFoldIndexes)
            trainFoldLabels = selectSample(data[TRAIN_LABELS], trainFoldIndexes)
            testFoldData = selectSample(data[TRAIN], testFoldIndexes)
            testFoldLabels = selectSample(data[TRAIN_LABELS], testFoldIndexes)
            w = doubleU(phi(trainFoldData), l, tListToTVector(trainFoldLabels))
            mse = MSE(testFoldData, w, testFoldLabels)
            lMSEList.append(mse)
            
        #average the results, and add it to the list
        avgMSEList.append(float(sum(lMSEList)) / numFolds)
    #find the best avg MSE
    bestAvgMSE = avgMSEList[0]
    bestL = 0
    for lVal, avgMSE in enumerate(avgMSEList):
        if abs(avgMSE-targetValue) < abs(bestAvgMSE - targetValue):
            bestL = lVal
            bestAvgMSE = avgMSE
            
    outFile = open(resultOutLoc, 'w')
    outFile.write(str(bestL) + '\t' + str(bestAvgMSE))
    outFile.close()
    # delta_th = vth
    delta_th = ddif / (2 * base)

    th0 = th
    th += delta_th

    th_sin = math.sin(th)
    th_cos = math.cos(th)

    vx = (vsum / 2.0) * th_cos
    vy = (vsum / 2.0) * th_sin

    delta_x = vx * varT
    delta_y = vy * varT

    if abs(vdata[0] - vdata[1]) > 5 and vdif != 0.0:
        t1 = (base * vsum) / (2.0 * vdif)
        delta_x = t1 * (th_sin - math.sin(th0))
        delta_y = t1 * (th_cos - math.cos(th0))

    x += delta_x
    y += delta_y

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
                                   "base_link", "odom")

    # next, we'll publish the odometry message over ROS
            idx_keep_cond.append(idx)
    lst_AlleleIDX = idx_keep_cond
    if len(lst_AlleleIDX) == 0:
        sys.exit("Error. None of the desired haplotype(s) to keep is present.")

# loop through each allele list and extract the allele count to obtain the alt allele frequency
dct_af = reducedAF(dct_allele, lst_AlleleIDX)
# change AF to DAF in dct_af and count number of positions (required for Watterson); use only sites with high confidence AA inference!
daf_ctr = 0
for key in dct_info.keys():
    '''# only remove nonvariable sites when keep flag not specified.(w/ -k we want to keep all sites and define presence absence outside of this script)
    if dct_af[key] == 0 or dct_af[key] == 1 and keep_haps == None:    
        del dct_info[key]
    else:'''
    if dct_info[key][4] == dct_info[key][5]:
        dct_af[key] = abs((dct_af[key] - 1))
        daf_ctr = daf_ctr + 1
    elif dct_info[key][3] == dct_info[key][5]:
        daf_ctr = daf_ctr + 1
    else:
        del dct_info[key]

# watterson can only be calculated with number of haps > 1 (when -k used maybe only one hap needed...than we do not need all that)
# following Clark/Hartl Principles of PopGen, pg.176 eq.:4.23
if len(lst_AlleleIDX) > 1:
    theta = watterson(len(lst_AlleleIDX), daf_ctr)
    # get haplotype IDs which were kept after pos filtering
    hap_names = [hapIDs[i] for i in lst_AlleleIDX]
    # print header with watterson and info
    out.write("## ThetaWatterson:\t" + str(theta) + "\n")
    out.write("## N_Haplotypes:\t" + str(len(lst_AlleleIDX)) + "\n")
Example #13
0
    def down_laser_cb(self, laser):
        self.laserResolution = 360.0 / len(laser.ranges)
        # clustersPoints = self.cluster(laser)  # TODO
        # good_markers, good_indices = self.cluster_filtering(clustersPoints, laser)  # TODO
        # angle = self.curve_detection(good_markers, laser, clustersPoints)  # TODO
        # curv_indexes = self.curve_filtering(angle, clustersPoints, good_markers)  # TODO

        veto = True
        if self.legPublisher.get_num_connections(
        ) > 0 or self.crowdPub2.get_num_connections() or veto:
            print("Start ...")
            i = 0
            localEndPoint = len(laser.ranges) - 1
            clustersPoints = []
            clustersPoints.append([])
            appendFlag = False
            remote_points_flag = True

            while i < localEndPoint:  # clustering
                rad = (self.laserResolution * pi) / 180
                b = laser.ranges[i]
                c = laser.ranges[i + 1]
                Distance = sqrt((pow(b, 2) + pow(c, 2)) -
                                (2 * b * c * cos(rad)))

                if laser.ranges[i] > 2 and remote_points_flag:
                    '''Increase threshold between two points in remote points.'''
                    self.threshold += 0.035
                    remote_points_flag = False
                elif laser.ranges[i] < 2 and not remote_points_flag:
                    '''Revert to default.'''
                    self.threshold -= 0.035
                    remote_points_flag = True

                if (Distance < self.threshold) and \
                        (laser.ranges[i] < self.outOfRange) and (laser.ranges[i] != 0):
                    '''Cluster gather.'''
                    clustersPoints[len(clustersPoints) - 1].append(i)
                    appendFlag = True

                elif (Distance > self.threshold) and appendFlag and \
                        (laser.ranges[i] < self.outOfRange) and (laser.ranges[i] != 0):
                    '''Cluster cutter.'''
                    clustersPoints.append([])
                    appendFlag = False

                i = i + 1

            del clustersPoints[len(clustersPoints) -
                               1]  # yeduneye akhari xalie hamishe

            i = 0
            while i < len(clustersPoints) - 1:  # better clustering
                a = (clustersPoints[i + 1][0]) - (
                    clustersPoints[i][len(clustersPoints[i]) - 1])

                b = laser.ranges[clustersPoints[i][len(clustersPoints[i]) - 1]]
                c = laser.ranges[clustersPoints[i + 1][0]]

                localDegreeOfCluster = a * self.laserResolution
                rad2 = (localDegreeOfCluster * pi) / 180

                clusterDistance = (sqrt((pow(b, 2) + pow(c, 2)) -
                                        (2 * b * c * cos(rad2))))

                if clusterDistance < self.threshold2:
                    clustersPoints[i] = clustersPoints[i] + clustersPoints[i +
                                                                           1]
                    del clustersPoints[i + 1]
                    i = i - 1  # baraye inke 2bare haman index ra check konad

                i = i + 1

            clustersDistance = []
            indexes = []
            markIndexes = []
            i = 0
            while i < len(clustersPoints):  # clusters Distance
                a = len(clustersPoints[i]) - 1
                b = laser.ranges[clustersPoints[i][len(clustersPoints[i]) - 1]]
                c = laser.ranges[clustersPoints[i][0]]
                localDegreeOfCluster = a * self.laserResolution
                rad3 = (localDegreeOfCluster * pi) / 180
                clustersDistance.append(
                    sqrt((pow(b, 2) + pow(c, 2)) - (2 * b * c * cos(rad3))))
                if (len(clustersPoints[i]) % 2) == 0:
                    middle = (len(clustersPoints[i]) / 2) - 1

                elif (len(clustersPoints[i]) % 2) == 1:
                    middle = (len(clustersPoints[i]) / 2)

                indexes.append(clustersPoints[i][middle])
                markIndexes.append(i)  # indexe clusterha
                i += 1

            i = 0
            goodClusters = []
            goodIndexes = []
            goodMarkers = []
            while i < len(clustersDistance):  # good distance choose
                if clustersDistance[i] > self.minLenght and clustersDistance[
                        i] < self.maxLenght:
                    goodClusters.append(clustersDistance[i])  # faselehaye xub
                    goodIndexes.append(
                        indexes[i])  # index haye mianie xube laser.
                    goodMarkers.append(
                        markIndexes[i])  # indexe clusterhaye xub

                i = i + 1

            i = 0
            dist1 = []
            dist2 = []
            dist3 = []
            angle = []
            while i < len(goodMarkers):  # curve detection
                gm = goodMarkers[i]
                a = laser.ranges[clustersPoints[gm][0]]
                z = laser.ranges[clustersPoints[gm][len(clustersPoints[gm]) -
                                                    1]]
                n1 = len(clustersPoints[gm]) - 1
                localDegreeOfCluster = n1 * self.laserResolution
                rad6 = (localDegreeOfCluster * pi) / 180
                Distance1 = (sqrt((pow(a, 2) + pow(z, 2)) -
                                  (2 * a * z * cos(rad6))))
                dist3.append(Distance1)
                dist1.append([])
                dist2.append([])
                angle.append([])

                j = 0
                while j < len(
                        clustersPoints[gm]
                ) - 2:  # baraye inke avvali va akhari dar bala hesab shode and.
                    n2 = ((clustersPoints[gm][j + 1]) -
                          (clustersPoints[gm][0]))
                    n3 = ((clustersPoints[gm][len(clustersPoints[gm]) - 1]) -
                          (clustersPoints[gm][j + 1]))
                    b = laser.ranges[clustersPoints[gm][j + 1]]
                    localDegreeOfCluster2 = n2 * self.laserResolution
                    localDegreeOfCluster3 = n3 * self.laserResolution
                    rad7 = (localDegreeOfCluster2 * pi) / 180
                    rad8 = (localDegreeOfCluster3 * pi) / 180
                    Distance2 = (sqrt((pow(a, 2) + pow(b, 2)) -
                                      (2 * a * b * cos(rad7))))
                    Distance3 = (sqrt((pow(z, 2) + pow(b, 2)) -
                                      (2 * z * b * cos(rad8))))
                    dist1[i].append(Distance2)
                    dist2[i].append(Distance3)
                    angl = numpy.arccos(
                        (pow(dist1[i][j], 2) + pow(dist2[i][j], 2) -
                         pow(dist3[i], 2)) / (2 * dist1[i][j] * dist2[i][j]))
                    angl = (angl * (180 / pi))  # radian to degree
                    angle[i].append(angl)

                    j = j + 1

                i = i + 1

            ##############filter kardane clusterha ba zavie monaseb##################
            i = 0
            curv = []
            cmp2 = 0
            cmp3 = 1
            curv_indexes = []
            while i < len(angle):
                j = 0
                while j < len(angle[i]) - 1:
                    cmp3 = cmp3 + 1

                    if (abs(angle[i][j] - angle[i][j + 1]) < 50) and \
                            (abs(angle[i][j] - self.threshold_angle) < 50):
                        print('matchhhhhhhhhhhhh')
                        cmp2 = cmp2 + 1  # increase match counter

                    if ((len(angle) - cmp3) == 0) and \
                            (abs(angle[i][j + 1] - self.threshold_angle) < 50):  # baraye inke zaviye akhar ham barresi shavad
                        print('mathchhhhhhhhhh22')
                        cmp2 = cmp2 + 1  # increase match counter

                    j = j + 1

                if (len(angle[i]) - cmp2) < ((50.0 * len(angle[i])) / 100):
                    '''If 50% is match, entered.'''
                    curv.append(clustersPoints[goodMarkers[i]])
                    curv_indexes.append(goodMarkers[i])

                i = i + 1

            print("good_Markers", goodMarkers)
            print("curve_indexes", curv_indexes)

            ####Pair Leg Detection####wear, bayad moqayeseE shavad, yany hame ba hame, na motevali
            _Legs = []
            Legs_Indexes = []
            i = 0
            k = -1

            while i < len(curv) - 1:
                end1 = len(clustersPoints[curv_indexes[i]]) - 1
                end2 = len(clustersPoints[curv_indexes[i + 1]]) - 1
                a = ((clustersPoints[curv_indexes[i + 1]][0]) -
                     (clustersPoints[curv_indexes[i]][end1]))
                b = laser.ranges[clustersPoints[curv_indexes[i]][end1]]
                c = laser.ranges[clustersPoints[curv_indexes[i + 1]][0]]
                localDegreeOfCluster = a * self.laserResolution
                rad8 = (localDegreeOfCluster * pi) / 180
                twoLegsDistance = sqrt((pow(b, 2) + pow(c, 2)) -
                                       (2 * b * c * cos(rad8)))
                d = len(clustersPoints[curv_indexes[i]])
                e = laser.ranges[clustersPoints[curv_indexes[i]][0]]
                f = len(clustersPoints[curv_indexes[i + 1]])
                g = laser.ranges[clustersPoints[curv_indexes[i + 1]][end2]]
                localDegreeOfCluster2 = d * self.laserResolution
                localDegreeOfCluster3 = f * self.laserResolution
                rad9 = (localDegreeOfCluster2 * pi) / 180
                rad10 = (localDegreeOfCluster3 * pi) / 180
                diagonal1 = sqrt(
                    pow(e, 2) + pow(b, 2) - (2 * b * e * cos(rad9)))
                diagonal2 = sqrt(
                    pow(g, 2) + pow(c, 2) - (2 * g * c * cos(rad10)))

                if (twoLegsDistance < self.farLenght) and (
                        abs(diagonal1 - diagonal2) < self.threshold_diagonal):
                    k = k + 1
                    _Legs.append(curv[i] + curv[i + 1])
                    Legs_Indexes.append([])
                    Legs_Indexes[k].append(curv_indexes[i])
                    Legs_Indexes[k].append(curv_indexes[i + 1])
                    i = i + 1  # baraye inke agar vared if shod yany inke 1 joft pa kamel shod va Dgar niaz b moqayese paye dovvome aan joft pa ba clustere Dgar nist.

                i = i + 1

            print("leg_indexes", Legs_Indexes)

            #######################Detect pair togetherLegs########################
            i = 0
            goodClustersTogetherLegs = []
            goodIndexesTogetherLegs = []
            goodMarkersTogetherLegs = []

            while i < len(
                    clustersDistance):  # good distance togetherLegs choose
                if clustersDistance[i] > self.togetherMin and \
                        (clustersDistance[i] < self.togetherMax):
                    goodClustersTogetherLegs.append(
                        clustersDistance[i]
                    )  #faselehaye xub dar in zakhire mishavand
                    goodIndexesTogetherLegs.append(
                        indexes[i]
                    )  # index haye mianie xube laser. #pointhaye mianie cluster ha
                    goodMarkersTogetherLegs.append(
                        markIndexes[i])  # indexe clusterhaye xub

                i = i + 1

            print("goodMarkersTogetherLegs", goodMarkersTogetherLegs)

            i = 0
            string = []
            togetherLegs = []
            #####for simple way#####
            angle11 = []
            angle22 = []
            dist1 = []
            dist2 = []
            dist3 = []
            dist4 = []
            dist5 = []
            dist6 = []
            while i < len(goodMarkersTogetherLegs):
                j = 0
                string.append([])
                flag1 = True  # for pass the first time
                flag2 = True
                c1 = 0
                c2 = 0
                while j < len(clustersPoints[goodMarkersTogetherLegs[i]]) - 1:
                    a = laser.ranges[clustersPoints[goodMarkersTogetherLegs[i]]
                                     [j + 1]]
                    b = laser.ranges[clustersPoints[goodMarkersTogetherLegs[i]]
                                     [j]]
                    if abs(a - b) > self.togetherPoint_thresold:
                        if a < b:  # is L
                            c1 = c1 + 1
                            if not flag1:
                                if (
                                        string[i][len(string[i]) - 1] == "R"
                                ) and c1 > 2:  # yany hade aqal 2 ta bayad az in halat bashe.
                                    string[i].append("L")

                            if flag1 and c1 > 2:
                                string[i].append("L")
                                flag1 = False

                        elif b < a:  # is R
                            c2 = c2 + 1
                            if not flag2:
                                if (string[i][len(string[i]) - 1]
                                        == "L") and c2 > 2:
                                    string[i].append("R")

                            if flag2 and c2 > 2:
                                string[i].append("R")
                                flag2 = False

                    j = j + 1

                # word = ["L","R","L","R"]

                if len(string[i]) == 4:  # this mean LRLR number is 4
                    if string[i][0] == "L" and string[i][1] == "R" and \
                            string[i][2] == "L" and string[i][3] == "R":
                        print('Matched')
                        # TODO :: doing stuff.

                ###########################new and simple approach######################
                gmtl = goodMarkersTogetherLegs[i]
                a = laser.ranges[clustersPoints[gmtl][0]]
                b1 = goodIndexesTogetherLegs[
                    i]  # pointe mianie cluster haye ehtemalan togetherlegs
                mid = laser.ranges[b1]
                z = laser.ranges[clustersPoints[gmtl][len(clustersPoints[gmtl])
                                                      - 1]]
                n1 = int((len(clustersPoints[gmtl]) - 1) / 2)
                localDegreeOfCluster = n1 * self.laserResolution
                rad6 = (localDegreeOfCluster * pi) / 180
                Distance1 = (sqrt((pow(a, 2) + pow(mid, 2)) -
                                  (2 * a * mid * cos(rad6))))
                Distance4 = (sqrt((pow(mid, 2) + pow(z, 2)) -
                                  (2 * mid * z * cos(rad6))))
                dist3.append(Distance1)
                dist4.append(Distance4)
                dist1.append([])
                dist2.append([])
                dist5.append([])
                dist6.append([])
                angle11.append([])
                angle22.append([])
                k = 0

                while k < (
                        int(len(clustersPoints[gmtl]) - 1) / 2
                ) - 1:  # baraye inke avvali va akhari va vasaty dar bala hesab shode and.
                    n2 = ((clustersPoints[gmtl][k + 1]) -
                          (clustersPoints[gmtl][0]))
                    n3 = (b1 - (clustersPoints[gmtl][k + 1]))
                    b = laser.ranges[clustersPoints[gmtl][k + 1]]
                    nextMid = laser.ranges[b1 + k + 1]
                    localDegreeOfCluster2 = n2 * self.laserResolution
                    localDegreeOfCluster3 = n3 * self.laserResolution
                    rad7 = (localDegreeOfCluster2 * pi) / 180
                    rad8 = (localDegreeOfCluster3 * pi) / 180
                    Distance2 = (sqrt((pow(a, 2) + pow(b, 2)) -
                                      (2 * a * b * cos(rad7))))
                    Distance3 = (sqrt((pow(mid, 2) + pow(b, 2)) -
                                      (2 * mid * b * cos(rad8))))
                    dist1[i].append(Distance2)
                    dist2[i].append(Distance3)
                    Distance5 = (sqrt((pow(mid, 2) + pow(nextMid, 2)) -
                                      (2 * mid * nextMid * cos(rad7))))
                    Distance6 = (sqrt((pow(z, 2) + pow(nextMid, 2)) -
                                      (2 * z * nextMid * cos(rad8))))
                    dist5[i].append(Distance5)
                    dist6[i].append(Distance6)
                    angl11 = numpy.arccos(
                        (pow(dist1[i][k], 2) + pow(dist2[i][k], 2) -
                         pow(dist3[i], 2)) / (2 * dist1[i][k] * dist2[i][k]))
                    angl11 = (angl11 * (180 / pi))  # radian to degree
                    angl22 = numpy.arccos(
                        (pow(dist5[i][k], 2) + pow(dist6[i][k], 2) -
                         pow(dist4[i], 2)) / (2 * dist5[i][k] * dist6[i][k]))
                    angl22 = (angl22 * (180 / pi))  # radian to degree
                    angle11[i].append(angl11)
                    angle22[i].append(angl22)
                    k = k + 1

                i = i + 1

            ##############filter kardane clusterha ba zavie monasebe 2 ##################
            i = 0
            cmp2 = 0
            cmp3 = 1
            while i < len(angle11):
                j = 0

                while j < len(angle11[i]) - 1:
                    cmp3 = cmp3 + 1
                    #  TODO :: all 50(s) was 25 and 50.0%(s) was 30.0%
                    if (abs(angle11[i][j] - angle11[i][j + 1]) < 40) and \
                            (abs(angle11[i][j] - self.threshold_angle) < 40) and \
                            (abs(angle22[i][j] - angle22[i][j + 1]) < 40) and \
                            (abs(angle22[i][j] - self.threshold_angle) < 40):
                        cmp2 = cmp2 + 1

                    if ((len(angle11) - cmp3) == 0) and \
                            (abs(angle11[i][j + 1] - self.threshold_angle) < 40) and \
                            (abs(angle22[i][j + 1] - self.threshold_angle) < 40):  # baraye inke zaviye akhar ham barresi shavad
                        cmp2 = cmp2 + 1

                    j = j + 1

                if (len(angle11[i]) - cmp2) < ((60.0 * len(angle11[i])) / 100) and \
                        (len(angle22[i]) - cmp2) < ((60.0 * len(angle22[i])) / 100): # yani dar suraT varede if mishavad k ekhtelafe cmp2 ba har goodcluster bayad kamtar az 40% an bashad
                    togetherLegs.append(goodMarkersTogetherLegs[i])

                i = i + 1

            print("togetherlegs", togetherLegs)

            ##################################################################
            cmpIndexes = []
            i = 0

            while i < len(
                    goodMarkers
            ):  # ijade 1 araye baraye pyda kardane un clustri k ruberuye robote.
                cmpIndexes.append(
                    abs(360 - goodIndexes[i]))  #360 shot roobe rooye robot
                i = i + 1

            i = 0
            localMin = 180  # bishtarin chizi k Mkan dare bashe = 360 - 180 = 180
            while i < len(cmpIndexes):  # Ntekhabe clustere ruberuye robot.
                if cmpIndexes[i] < localMin:
                    localMin = cmpIndexes[i]

                i = i + 1

            i = 0
            positions = []
            x1 = []
            y1 = []
            x2 = []
            y2 = []
            # create marker
            while i < len(Legs_Indexes):
                '''crrate marker for first approach.'''
                positions.append([])
                lif = Legs_Indexes[i][0]  # LIF means, is, Legs Indexes First
                lie = Legs_Indexes[i][
                    1]  # chon har shakhs 2 pa darad pas 1 yany paye 2vvome fard
                end = len(clustersPoints[lie]) - 1
                end2 = len(clustersPoints[lif]) - 1
                res = self.laserResolution
                ii1 = clustersPoints[lif][0]
                ii9 = clustersPoints[lie][end]
                rad4 = ((ii1 * res * pi) / 180) - pi  # "-pi" is a TF
                rad5 = ((ii9 * res * pi) / 180) - pi  # "-pi" is a TF

                x1.append(laser.ranges[ii1] * cos(rad4))  # Red
                y1.append(laser.ranges[ii1] * sin(rad4))

                x2.append(laser.ranges[ii9] * cos(rad5))  # Green
                y2.append(laser.ranges[ii9] * sin(rad5))

                positions[i].append(x1[i])
                positions[i].append(y1[i])
                positions[i].append(x2[i])
                positions[i].append(y2[i])
                #############4 human detection##############
                mid = clustersPoints[lif][end2]  # Leg Index Middle
                range2send = Float32(laser.ranges[mid])
                point2send = Float32(mid)
                self.legPublisher.publish(range2send)
                self.legPublisher2.publish(point2send)
                self.rate.sleep()

                i = i + 1

            if Legs_Indexes:
                self.visualize(positions)

            i2 = 0
            positions2 = []
            x12 = []
            y12 = []
            x22 = []
            y22 = []
            while i2 < len(togetherLegs):
                '''create marker for second approach (simpler and more powerful)'''
                positions2.append([])
                end = len(clustersPoints[togetherLegs[i2]]) - 1
                res = self.laserResolution
                ii1 = clustersPoints[togetherLegs[i2]][0]
                ii9 = clustersPoints[togetherLegs[i2]][end]
                rad4 = ((ii1 * res * pi) / 180) - pi  # "-pi" is a TF
                rad5 = ((ii9 * res * pi) / 180) - pi  # "-pi" is a TF

                x12.append(laser.ranges[ii1] * cos(rad4))  # Read
                y12.append(laser.ranges[ii1] * sin(rad4))

                x22.append(laser.ranges[ii9] * cos(rad5))  # green
                y22.append(laser.ranges[ii9] * sin(rad5))

                positions2[i2].append(x12[i2])
                positions2[i2].append(y12[i2])
                positions2[i2].append(x22[i2])
                positions2[i2].append(y22[i2])
                #############4 human detection##############
                mid2 = clustersPoints[togetherLegs[i2]][int(end) /
                                                        2]  # Leg Index Middle
                range2send2 = Float32(laser.ranges[mid2])
                point2send2 = Float32(mid2)
                self.legPublisher.publish(range2send2)
                self.legPublisher2.publish(point2send2)
                self.rate.sleep()

                i2 = i2 + 1

            if togetherLegs:
                self.visualize(positions2)


#############test######3
# i2 = 0
# positions3 = []
# x12 = []
# y12 = []
# x22 = []
# y22 = []
# while i2 < len(goodMarkersTogetherLegs):
#     positions3.append([])
#     end = len(clustersPoints[goodMarkersTogetherLegs[i2]]) - 1
#     res = self.laserResolution
#     ii1 = clustersPoints[goodMarkersTogetherLegs[i2]][0]
#     ii9 = clustersPoints[goodMarkersTogetherLegs[i2]][end]
#     rad4 = ((ii1 * res * pi) / 180) - pi
#     rad5 = ((ii9 * res * pi) / 180) - pi
#
#     x12.append(laser.ranges[ii1] * cos(rad4))  # Read
#     y12.append(laser.ranges[ii1] * sin(rad4))
#
#     x22.append(laser.ranges[ii9] * cos(rad5))  # green
#     y22.append(laser.ranges[ii9] * sin(rad5))
#
#     positions3[i2].append(x12[i2])
#     positions3[i2].append(y12[i2])
#     positions3[i2].append(x22[i2])
#     positions3[i2].append(y22[i2])
#     #############4 human detection##############
#     mid2 = clustersPoints[goodMarkersTogetherLegs[i2]][int(end) / 2]  # Leg Index Middle
#     range2send2 = Float32(laser.ranges[mid2])
#     point2send2 = Float32(mid2)
#     self.legPublisher.publish(range2send2)
#     self.legPublisher2.publish(point2send2)
#     self.rate.sleep()
#
#     i2 = i2 + 1
#
# if goodMarkersTogetherLegs:
#     self.visualize(positions3)
#######################3

#################Legs Counter Call Back####################
            if self.start:
                self.legsCount.append(len(Legs_Indexes) + len(togetherLegs))
                self.tmp_legs.append(Legs_Indexes)
                self.tmp_tog.append(togetherLegs)
                self.tmp_clusters.append(clustersPoints)

                self.counter += 1
                print self.counter

                if self.counter == self.countTry:
                    self.counter = 0
                    i = 0
                    self.tedad = max(self.legsCount)

                    while self.tedad != self.legsCount[
                            i]:  # baraye bdast avardane indexe an clustere ya leg haye tashkhis dade shodeye ziaddd
                        i += 1

                    try:
                        merged_legs = self.merger(self.tmp_legs[i],
                                                  self.tmp_tog[i],
                                                  self.tmp_clusters[i])
                        print "merged_legss", merged_legs

                        if len(merged_legs) > 0:  # for scenario
                            self.legPublisherScenario.publish(True)
                        else:
                            self.legPublisherScenario.publish(False)

                        crowdSize, crowd_legs = self.crowd_detector(
                            merged_legs, self.tmp_clusters[i], laser)
                        print "All-legsCount", max(self.legsCount)
                        print "crowd_legs", crowd_legs
                        if len(crowd_legs) > 0:
                            self.msg.count = crowdSize
                            self.msg.first = self.tmp_clusters[i][crowd_legs[
                                0][0]][0] - 20  #tmp_cluster[][][] (template)
                            # print "inja", self.msg.first
                            self.msg.end = self.tmp_clusters[i][crowd_legs[
                                len(crowd_legs) -
                                1][len(crowd_legs[len(crowd_legs) - 1]) -
                                   1]][len(self.tmp_clusters[i][crowd_legs[
                                       len(crowd_legs) -
                                       1][len(crowd_legs[len(crowd_legs) - 1])
                                          - 1]]) - 1] + 20  #:| :| :|
                            self.crowdPub.publish(self.msg)
                            legsMsg = Legs()  # msg
                            index = 0

                            while index < len(crowd_legs):
                                res = self.laserResolution
                                end = len(crowd_legs[index]) - 1
                                a = crowd_legs[index][0]
                                b = crowd_legs[index][end]
                                i1 = self.tmp_clusters[i][a][0]
                                rad1 = ((i1 * self.laserResolution * pi) /
                                        180) - pi
                                i2 = self.tmp_clusters[i][b][end]
                                rad2 = ((i2 * self.laserResolution * pi) /
                                        180) - pi

                                x1 = laser.ranges[i1] * numpy.cos(rad1)
                                y1 = laser.ranges[i1] * numpy.sin(rad1)

                                x2 = laser.ranges[i2] * numpy.cos(rad2)
                                y2 = laser.ranges[i2] * numpy.sin(rad2)

                                x = (x1 + x2) / 2  # X e vasate paye fard
                                y = (y1 + y2) / 2  # Y e vasate paye fard

                                grade = (x2 - x1) / (y2 - y1
                                                     )  #shibe khatte paye fard
                                grade_vertical = -1 * (
                                    1 / grade
                                )  #shibe makus qarine bar khatte paye fard

                                grade_vertical = 1 * grade_vertical
                                theta = numpy.arctan(
                                    grade_vertical
                                )  # zaviyeye bordare amud nesbat b laser b dast miayad #arctan2 behtar ast

                                legMsg = Leg()  #msg
                                legMsg.pose.position.x = x
                                legMsg.pose.position.y = y
                                legMsg.pose.position.z = 0  #ertefae laser utm paEn 0.36 ast
                                legMsg.pose.orientation.w = 1
                                legMsg.pose.orientation.x = 0
                                legMsg.pose.orientation.y = 0
                                legMsg.pose.orientation.z = theta  #rad_theta # chon bayad hole mhvare z davaran dashte bashad

                                legsMsg.legsCrowd.append(
                                    legMsg
                                )  #yek msg daram k dakhelesh msg e digar list shode ast

                                index += 1

                            legsMsg.first = self.tmp_clusters[i][crowd_legs[0]
                                                                 [0]][0] - 20
                            legsMsg.end = self.tmp_clusters[i][crowd_legs[
                                len(crowd_legs) -
                                1][len(crowd_legs[len(crowd_legs) - 1]) -
                                   1]][len(self.tmp_clusters[i][crowd_legs[
                                       len(crowd_legs) -
                                       1][len(crowd_legs[len(crowd_legs) - 1])
                                          - 1]]) - 1] + 20  #:| :| :|
                            legsMsg.count = crowdSize
                            self.crowdPub2.publish(legsMsg)
                        self.legsCount = []  # inha jayeshan mashkuk ast
                        self.tmp_legs = []
                        self.tmp_tog = []
                        self.tmp_clusters = []

                    except Exception as exc:  # rospy.ServiceException as e:
                        print(exc)
                        print "legsCount", max(self.legsCount)
                        self.legsCount = []
                        self.tmp_legs = []
                        self.tmp_tog = []
                        self.tmp_clusters = []