ó
ï›îWc           @   sÚ   d  d l  Z  d  d l Z d  d l Z d  d l Z d  d l Z d e f d „  ƒ  YZ d „  Z	 d „  Z
 d „  Z e d k rÖ e j ƒ  Z e j d d	 d
 ƒe j d d e j d	 d ƒe j ƒ  Z e
 e j e j ƒ n  d S(   iÿÿÿÿNt	   MapConfigc           B   s   e  Z d  „  Z RS(   c         C   s‰   t  | d ƒ  } t j | ƒ } Wd  QX| d } | d |  _ | d |  _ | d |  _ | d } t j | d ƒ |  _ | d |  _	 d  S(	   Nt   rt   objectst   Nt   featureVecLent   haveOrientationt   environmentt   sizet   nDims(
   t   opent   yamlt   loadt   nObjectst   nFeatureDimst   useOrientationt   npt   arrayt   envSizet   envDims(   t   selft   yamlConfFilenamet   yamlConfFilet
   yamlConfigt	   objConfigt	   envConfig(    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyt   __init__   s    

(   t   __name__t
   __module__R   (    (    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyR       s   c         C   sA   t  |  ƒ } x. t t | ƒ ƒ D] } | | j ƒ  | | <q W| S(   N(   t   listt   ranget   lent   item(   t   numpy1dArrayt	   listArrayt   i(    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyt   numpyToNativePython   s    c         C   s'  t  |  ƒ } g  } xÉ t | j ƒ D]¸ } i  } t | j | j ƒ \ } } t | ƒ | t j <t	 j
 j
 | j ƒ } t | ƒ | t j <| j rÍ t | ƒ t	 j k r± t | ƒ }	 n t | ƒ }	 |	 | t j <n  | j | ƒ q" Wi t | j ƒ d 6| d 6}
 t | d ƒ  } t j |
 | ƒ Wd  QXd  S(   NR   t	   landmarkst   w(   R    R   R   t   generatePoseR   R   R#   t   ct
   LM_POS_KEYR   t   randomR   t   LM_FEAT_KEYR   t   typet   ndarrayt   floatt   LM_ORNT_KEYt   appendR	   R
   t   dump(   R   t   mapFilenamet	   mapConfigt
   objectListt   _t   thisObjectDictt
   npPositiont   npOrientationt   featurest   orientationt   mapDictt   mapFile(    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyt   generateMap"   s&    		
c         C   sI   |  t  j j | ƒ } | d k r? d t  j t  j j ƒ  } n  | | f S(   Ni   (   R   R)   t   pi(   R   R   t   positionR9   (    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyR&   =   s    t   __main__R1   t   helps7       The filename to which the yaml map will be written.s   --configt   defaults[       The name of the config file that defines the parameters of the map to be     generated.(   R
   t   argparset   numpyR   t   collectionst	   constantsR'   t   objectR    R#   R<   R&   R   t   ArgumentParsert   parsert   add_argumentt   MAP_CONFIG_FILEt
   parse_argst   argst   configR1   (    (    (    sE   /home/andrew/catkin_ws/src/landmark_localizer/scripts/mapGenerator.pyt   <module>   s    				