text: #!/usr/bin/env python
  # -*- coding:utf-8 -*-
  #学習した場所領域のサンプルをrviz上に可視化するプログラム
  #作成者 石伏智
  #作成日 2015年12月
  #サンプリング点プロット→ガウスの概形描画に変更（磯部、2016卒論）
  #編集、更新：谷口彰 更新日：2017/02/10
  #mu 2次元、sig 2×2次元版
  
  """
  実行前に指定されているフォルダが正しいかをチェックする
  file_read.pyも同様に ！
  
  実行方法
  python place_draw.py (parameterフォルダの絶対パス) (表示する場所領域を指定したい場合は数字を入力)
  
  実行例
  python place_draw.py /home/emlab/py-faster-rcnn/work/gibbs_sampling_program
  
  """
  
  import glob
  import re
  import os
  import rospy
  import math
  import sys
  import geometry_msgs.msg as gm
  from geometry_msgs.msg import Point
  import sensor_msgs.msg as sm
  from  visualization_msgs.msg import Marker
  from  visualization_msgs.msg import MarkerArray
  import numpy as np
  import struct
  #import PyKDLs
  sys.path.append("lib/")
  from __init__ import *
  
  """
  def read_result(filename):
    file_dir = os.chdir(filename)
    f = open('SBP.txt')
    line = f.readline() # 1行を文字列として読み込む(改行文字も含まれる)
    place_num = int(line)
    return place_num
  """
  
  def mu_read(filename):
      all_mu=[]
      #fp = open(filename+'mu'+maxparticle+".csv", "r") # check
      #convert = lambda text: int(text) if text.isdigit() else text
      #alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ]
      #file.sort(key=alphanum_key)
      #for f in file:
      K = 0
      for line in open(filename + "/" + trialname + '_Myu_' + str(iteration) + '_' + str(sample) + '.csv', 'r'): #.readlines()
          mu=[] #(x,y,sin,cos)
          
          # readlines()は,ファイルを全て読み込み、1行毎に処理を行う
          #print line
          data=line[:].split(',')
          mu +=[float(data[0])]
          mu +=[float(data[1])]
          mu +=[0]#float(data[2])]
          mu +=[0]#float(data[3])]
          #print position
          all_mu.append(mu)
          K += 1
      return all_mu, K
  
  
  def sigma_read(filename):
      all_sigma=[]
      #file = glob.glob(filename+'/parameter3/sigma/*.txt') # check
      #convert = lambda text: int(text) if text.isdigit() else text
      #alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ]
      #file.sort(key=alphanum_key)
      for line in open(filename + "/" + trialname + '_S_' + str(iteration) + '_' + str(sample) + '.csv', 'r'):
          #sigma=[] #(x,y,sin,cos)
          data=line[:].split(',')
          sigma = [[float(data[0]),float(data[1]),0,0],[float(data[2]),float(data[3]),0,0],[0,0,0,0],[0,0,0,0]]
          # readlines()は,ファイルを全て読み込み、1行毎に処理を行う
          #line=open(f, 'r').readlines()
          #i = 0
          #for l in line:
          #    sigma_l.append(float(data[0]))
          #    sigma_l.append(float(data[1]))
          #    sigma_l.append(float(data[2]))
          #    sigma_l.append(float(data[3]))
          #    
          #sigma.append(sigma_l)
          #    
          all_sigma.append(sigma)
      return all_sigma
  
  """
  def sampling_read(filename, class_num):
      c_all_position=[]
      for c in range(class_num):
          all_position=[] #すべての自己位置データのリスト
          
          file = glob.glob(filename+'/sampling_data3/class'+repr(c)+'/*.txt') # check
          convert = lambda text: int(text) if text.isdigit() else text 
          alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ] 
          file.sort(key=alphanum_key)
          #print file
          for f in file:
              position=[] #(x,y,sin,cos)
              
              line=open(f, 'r').readlines()
              #print line
              data=line[0][:].split(',')
              position +=[float(data[0])]
              position +=[float(data[1])]
              position +=[float(data[2])]
              position +=[float(data[3])]
              #print position
              all_position.append(position)
          c_all_position.append(all_position)
      return c_all_position
  """
  
  # 自作ファイル
  #import file_read as f_r
  #from SBP import read_result
  
  #実験ファイル名trialnameを取得
  trialname = sys.argv[1]
  print trialname
  
  #iterationを要求
  iteration = sys.argv[2] #1
  
  #sampleを要求
  sample = sys.argv[3] #0
  
  ##FullPath of folder
  filename = outputfolder_SIG + trialname #+ "/" 
  print filename, iteration, sample
  outputfile = filename + navigation_folder #outputfolder + trialname + navigation_folder
  #outputname = outputfile + "T"+str(T_horizon)+"N"+str(N_best)+"A"+str(Approx)+"S"+str(init_position_num)+"G"+str(speech_num)
  
  
  maxparticle = 0
  i = 0
  ##datafolder+trialname+"/"+stepにおける最大尤度のパーティクルを読み込み
  #for line in open( filename + 'weights.csv', 'r'):
  #      #itemList = line[:].split(',')
  #      if (i == 0):
  #        maxparticle = int(line)
  #        i +=1
  
  #maxparticle = int(sys.argv[3])
  
  
  
  #filename=sys.argv[1]
  
  #Class_NUM=0#read_result(filename)
  RAD_90=math.radians(90)
  color_all=1   #1 or 0 、(0ならばすべて赤)
  mu_draw =1    #1 or 0 、(0ならば中心値を表示しない)
  sigma_draw=1  #1 or 0, (0ならば分散を表示しない)
  mu_arrow=0    #矢印を可視化する場合
  COLOR=[
  [1,0,0],[0,1,0],[0,0,1],[0.5,0.5,0],[0.5,0,0.5], #4
  [0,0.5,0.5],[0.8,0.1,0.1],[0.1,0.8,0.1],[0.1,0.1,0.8],[0.6,0.2,0.2],#9
  [0.2,0.6,0.2],[0.2,0.2,0.6],[0.4,0.3,0.3],[0.3,0.4,0.3],[0.3,0.3,0.4], #14
  [0.7,0.2,0.1],[0.7,0.1,0.2],[0.2,0.7,0.1],[0.1,0.7,0.2],[0.2,0.1,0.7],#19
  [0.1,0.2,0.7],[0.5,0.2,0.3],[0.5,0.3,0.2],[0.3,0.5,0.2],[0.2,0.5,0.3],#24
  [0.3,0.2,0.5],[0.2,0.3,0.5],[0.7,0.15,0.15],[0.15,0.7,0.15],[0.15,0.15,0.7],#29
  [0.6,0.3,0.1],[0.6,0.1,0.3],[0.1,0.6,0.3],[0.3,0.6,0.1],[0.3,0.1,0.6],#34
  [0.1,0.3,0.6],[0.8,0.2,0],[0.8,0,0.2],[0.2,0.8,0],[0,0.8,0.2],#39
  [0.2,0,0.8],[0,0.2,0.8],[0.7,0.3,0],[0.7,0,0.3],[0.3,0.7,0.0],#44
  [0.3,0,0.7],[0,0.7,0.3],[0,0.3,0.7],[0.25,0.25,0.5],[0.25,0.5,0.25], #49
  [1,0,0],[0,1,0],[0,0,1],[0.5,0.5,0],[0.5,0,0.5], #54
  [0,0.5,0.5],[0.8,0.1,0.1],[0.1,0.8,0.1],[0.1,0.1,0.8],[0.6,0.2,0.2],#59
  [0.2,0.6,0.2],[0.2,0.2,0.6],[0.4,0.3,0.3],[0.3,0.4,0.3],[0.3,0.3,0.4], #64
  [0,7,0.2,0.1],[0.7,0.1,0.2],[0.2,0.7,0.1],[0.1,0.7,0.2],[0.2,0.1,0.7],#69
  [0.1,0.2,0.7],[0.5,0.2,0.3],[0.5,0.3,0.2],[0.3,0.5,0.2],[0.2,0.5,0.3],#74
  [0.3,0.2,0.5],[0.2,0.3,0.5],[0.7,0.15,0.15],[0.15,0.7,0.15],[0.15,0.15,0.7],#79
  [0.6,0.3,0.1],[0.6,0.1,0.3],[0.1,0.6,0.3],[0.3,0.6,0.1],[0.3,0.1,0.6],#84
  [0.1,0.3,0.6],[0.8,0.2,0],[0.8,0,0.2],[0.2,0.8,0],[0,0.8,0.2],#89
  [0.2,0,0.8],[0,0.2,0.8],[0.7,0.3,0],[0.7,0,0.3],[0.3,0.7,0.0],#94
  [0.3,0,0.7],[0,0.7,0.3],[0,0.3,0.7],[0.25,0.25,0.5],[0.25,0.5,0.25] #99
  ]
  
  #特定の番号のガウス分布のみ描画したいとき
  try: 
      Number=None #int(sys.argv[3])
  except IndexError:
      Number=None
  
  # 石伏さんはハイパーパラメータの値をパラメータ.txtに保持しているため、以下の処理をしている
  #env_para=np.genfromtxt(filename+"/パラメータ.txt",dtype= None,delimiter =" ")
  #Class_NUM=int(env_para[4][1])
  
  """
  #=============各場所領域に割り当てられているデータの読みこみ===================
  def class_check():
      Class_list=[]
      for i in range(Class_NUM):
          #f=filename+"/parameter3/class/class"+repr(i)+".txt" # check
          data=[]
          # default(エラー)
          #for line in open(f,'r').readlines():
          #    print str(line) + "\n\n"
          #    data.append(int(line))
          
          #for line in open(f, 'r'):
          #    print "読み込み完了"
          
          #replaceを使えば簡単にできる
          #line1=line.split('[') # 始めの"["を除く
          #line1=line1[1].split(']') # 終わりの"["を除く
          #line2=line1[0]
          #print "\nline2:" + str(line2) + "\n"
          
          # 場所クラスに中身があるときはtry、中身がないときはexceptに移動
          #try:
          #    data = [int(item) for item in line2.split(',')]
          #except ValueError:
          #    data = []
  
          #c=[]
  
          #for item in data:
          #    print item
          #    try:
          #        num=int(item)
          #        c.append(num)
          #    except ValueError:
          #        pass
          Class_list.append(data)
      return Class_list
  """
  
  def place_draw():
      # 場所のクラスの割り当てられていない場合は省く→CRPでは割り当てられていないデータは存在しない
      #class_list=class_check()
      #print class_list
      
      
      pub = rospy.Publisher('draw_space',MarkerArray, queue_size = 10)
      rospy.init_node('draw_spatial_concepts', anonymous=True)
      rate = rospy.Rate(10) # 10hz
      
      #最大尤度のパーティクルのmuとsigを読み込み
      mu_all,Class_NUM = mu_read(filename)
      sigma  = sigma_read(filename)
      #sample = sampling_read(filename, Class_NUM)
      #print "sigma: ",sigma
      
      data_class=[i for i in xrange(Class_NUM)]
      #for n in range(Class_NUM):
      #    #if len(class_list[n])!=0:
      #    data_class.append(n)
      
      marker_array=MarkerArray()
      id=0
      for c in data_class:
          #場所領域の中心値を示す場合
          #===場所領域の範囲の可視化====================
          if sigma_draw==1:
              
              marker =Marker()
              marker.type=Marker.CYLINDER
              
              (eigValues,eigVectors) = np.linalg.eig(sigma[c])
              angle = (math.atan2(eigVectors[1, 0], eigVectors[0, 0]));
              
              marker.scale.x = 2*math.sqrt(eigValues[0]);
              marker.scale.y = 2*math.sqrt(eigValues[1]);
              
              marker.pose.orientation.w = math.cos(angle*0.5);
              marker.pose.orientation.z = math.sin(angle*0.5);
              
              
              marker.scale.z=0.01 # default: 0.05
              marker.color.a=0.3
              marker.header.frame_id='map'
              marker.header.stamp=rospy.get_rostime()
              marker.id=id
              id +=1
              marker.action=Marker.ADD
              marker.pose.position.x=mu_all[c][0]
              marker.pose.position.y=mu_all[c][1]
              marker.color.r = COLOR[c][0] # default: COLOR[c][0] 色のばらつきを広げる
              marker.color.g = COLOR[c][1] # default: COLOR[c][1] 色のばらつきを広げる
              marker.color.b = COLOR[c][2] # default: COLOR[c][2] 色のばらつきを広げる
  
              if Number != None:
                  if Number==c:
                      marker_array.markers.append(marker)
              else:
                      marker_array.markers.append(marker)
          if mu_draw==1:
              mu_marker =Marker()
              
              if mu_arrow==1: #矢印を可視化する場合
                  mu_marker.type=Marker.ARROW
                  orient_cos=mu_all[c][3]
                  orient_sin=mu_all[c][2]
                  if orient_sin>1.0:
                      orient_sin=1.0
                  elif orient_sin<-1.0:
                      orient_sin=-1.0
                  #radian xを導出
                  radian=math.asin(orient_sin)
                  if orient_sin>0 and orient_cos<0:
                      radian=radian+RAD_90
                  elif orient_sin<0 and orient_cos<0:
                      radian=radian-RAD_90
              
                  mu_marker.pose.orientation.z=math.sin(radian/2.0)
                  mu_marker.pose.orientation.w=math.cos(radian/2.0)
                  #<<<<<<<矢印の大きさ変更>>>>>>>>>>>>>>>>>>>>>>>>
                  mu_marker.scale.x=0.5 # default: 0.4
                  mu_marker.scale.y=0.07 # default: 0.1
                  mu_marker.scale.z=0.001 # default: 1.0
                  mu_marker.color.a=1.0
                  
              elif mu_arrow==0:
                  mu_marker.type=Marker.SPHERE
                  mu_marker.scale.x=0.1
                  mu_marker.scale.y=0.1
                  mu_marker.scale.z=0.01 # default: 0.05
                  mu_marker.color.a=1.0
              
              mu_marker.header.frame_id='map'
              mu_marker.header.stamp=rospy.get_rostime()
              mu_marker.id=id
              id +=1  
              mu_marker.action=Marker.ADD
              mu_marker.pose.position.x=mu_all[c][0]
              mu_marker.pose.position.y=mu_all[c][1]
              #print c,mu_marker.pose.position.x,mu_marker.pose.position.y
              
              if color_all==1:
                  mu_marker.color.r = COLOR[c][0] # default: COLOR[c][0]
                  mu_marker.color.g = COLOR[c][1] # default: COLOR[c][1]
                  mu_marker.color.b = COLOR[c][2] # default: COLOR[c][2]
              elif color_all==0:
                  mu_marker.color.r = 1.0
                  mu_marker.color.g = 0
                  mu_marker.color.b = 0
                  
              if Number != None:
                  if Number==c:
                      marker_array.markers.append(mu_marker)
              else:
                      marker_array.markers.append(mu_marker)
  
      print marker_array.markers
      
      while not rospy.is_shutdown():
          
          #pub.publish(marker)
          pub.publish(marker_array)
          rate.sleep()
      
  
  if __name__ == '__main__':
      try:
          place_draw()
      except rospy.ROSInterruptException:
          pass
  