*i%dZddlZddlmZddlmZmZmZmZm Z m Z ddl m Z hdZ erddlmZded eeeffd ZGd d ZdS) aCamera configuration management. Maps semantic camera names to hardware serial numbers, camera types, and roles. **Format**:: { "scene_1": {"serial": 37038161, "type": "zed", "role": "scene"}, "scene_2": {"serial": 55667788, "type": "zed", "role": "scene"}, "left_wrist": {"serial": "123456789012", "type": "realsense", "role": "left_wrist"}, "right_wrist": {"serial": 14932342, "type": "zed", "role": "right_wrist"} } Roles ----- - ``"scene"`` : fixed overhead / scene camera (multiple allowed) - ``"left_wrist"`` : wrist camera on the left arm (at most one) - ``"right_wrist"`` : wrist camera on the right arm (at most one) **Legacy format** (ZED only, still fully supported):: { "scene_camera": 37038161, "left_wrist_camera": 16522755 } Integer values are assumed to be ZED cameras with no role assigned. N)Path) TYPE_CHECKINGAnyDictListOptionalUnion) CAMERA_CONFIG>scene left_wrist right_wrist)Cameraentryreturnct|trd|fSt|tr2|dd}||dfSt d|)zEReturn (camera_type, serial) from a config entry (old or new format).zedtypeserialzInvalid camera config entry: ) isinstanceintdictgetlower ValueError)rcam_types 2/home/robot-lab/raiden_cmu/raiden/camera_config.py _parse_entryr*su%e|%)99VU++1133x(( >U>> ? ??c eZdZdZefdefdZddZddZdede e e effd Z dede efd Z d e e efde efd Zdede efd Zdede efdZdedeefdZdeeeffdZdeefdZ dded e e efdede eddf dZdedefdZdeddfdZdeeeffdZedee effdZdeeeffdZdS) CameraConfigzEManages camera configuration mapping semantic names to hardware info. config_filect||_|jjdd|jr[t |j5}t j||_dddn #1swxYwY| dSi|_| dS)NT)parentsexist_ok) rr!parentmkdirexistsopenjsonloadcameras_warn_invalid_roles_save)selfr!fs r__init__zCameraConfig.__init__7s ,, %%dT%BBB   " " $ $ d&'' <1/3y||  < < < < < < < < < < < < < < <  $ $ & & & & &DL JJLLLLLs#B  B B rNct|jd5}tj|j|dddddS#1swxYwYdS)Nw)indent)r(r!r)dumpr+)r.r/s rr-zCameraConfig._saveGs $"C ( ( 1A IdlAa 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1sAAAc |jD]b\}}t|trHd|vrD|d}|tvr3t d|d|jd|dttd cdS)NroleCamera 'z' in z has invalid role ''. Valid roles: .)r+itemsrr _VALID_ROLESrr!sorted)r.namerr7s rr,z CameraConfig._warn_invalid_rolesKs<--//  KD%%&& 6U??V}|++$J4JJd.>JJ JJ282F2FJJJ   rr>cf|j|}|dSt|\}}|S)zLReturn the serial number for a camera name (int for ZED, str for RealSense).Nr+rr)r.r>r_rs rget_serial_by_namezCameraConfig.get_serial_by_nameYs7   && =4 '' 6 rcf|j|}|dSt|\}}|S)zGReturn 'zed' or 'realsense' for the named camera, or None if not found.Nr@)r.r>rrrAs rget_camera_typezCameraConfig.get_camera_typeas7   && =4"5)) !rrc|jD];\}}t|\}}t|t|kr|cSrrA entry_serials rget_name_by_serialzCameraConfig.get_name_by_serialis`<--//  KD%*511OA|<  CKK// 0trc|j|}t|tr|dSdS)zMReturn the role for a camera ('scene', 'left_wrist', 'right_wrist'), or None.r7N)r+rrr)r.r>rs rget_rolezCameraConfig.get_roleps@   && eT " " %99V$$ $trr7c|jD]7\}}t|tr|d|kr|cS8dS)zReturn the name of the unique camera with the given role, or None. Use for 'left_wrist' and 'right_wrist' (at most one each). For 'scene' cameras use get_cameras_by_role(). r7N)r+r;rrr)r.r7r>rs rget_camera_by_rolezCameraConfig.get_camera_by_rolews\  <--//  KD%%&& 599V+<+<+D+D trcNfd|jDS)zPReturn names of all cameras with the given role (e.g. multiple 'scene' cameras).cvg|]5\}}t|tr|dk3|6S)r7)rrr).0r>rr7s r z4CameraConfig.get_cameras_by_role..sR   e%&& ,199V+<+<+D+D +D+D+Dr)r+r;)r.r7s `rget_cameras_by_rolez CameraConfig.get_cameras_by_roles<    #|1133    rc4|jS)z%Return a copy of the raw config dict.)r+copyr.s r list_cameraszCameraConfig.list_camerass|  """rcNt|jSrF)listr+keysrUs rlist_camera_nameszCameraConfig.list_camera_namessDL%%''(((rr camera_typec |4|tvr+td|d|dttd||d}|||d<||j|<|dS)a@Add or update a camera entry. Args: name: Unique camera name (key in the config). serial: Hardware serial number (int for ZED, str for RealSense). camera_type: 'zed' or 'realsense'. role: 'scene', 'left_wrist', or 'right_wrist'. Optional but recommended. NzInvalid role '' for camera 'r9r:)rrr7)r<rr=r+r-)r.r>rr[r7rs r add_camerazCameraConfig.add_cameras  L 8 8888T88 &| 4 4888 ,2; G G   E&M" T rcT||jvr|j|=|dSdS)NTF)r+r-)r.r>s r remove_camerazCameraConfig.remove_cameras0 4<   T" JJLLL4urrc|j|}|td|d|jdt |\}}t |t r|ddnd}|dkr!dd lm}||t|| S|d kr!dd l m }||t|| Std |d|d)uCreate and return a Camera instance for the named camera. Does NOT call open() – the caller is responsible for that. Raises: ValueError: if the name is not in the config or the type is unknown. Nr8z' not found in config ()fpsrr) ZedCamera)rc realsense)RealSenseCamerazUnknown camera type 'r]z&'. Supported types: 'zed', 'realsense') r+rrr!rrrraiden.cameras.zedrerraiden.cameras.realsensergrG)r.r>rrrrcrergs r create_camerazCameraConfig.create_cameras,   && =K4KK8HKKK (..&+5eT+B+BJ599UB''' u   4 4 4 4 4 49T3v;;C888 8 { " " @ @ @ @ @ @"?4V#>>> > 2H 2 2D 2 2 2   rcddlm}d|jD}i}|jD]7\}}t |\}}|dkrt||v||<2d||<8|S)z;Check which configured ZED cameras are currently connected.rNch|] }|j S) serial_number)rPcams r z9CameraConfig.validate_against_hardware..sVVV3S.VVVrrT)pyzed.slslrget_device_listr+r;rr)r.rrconnected_serialsresultr>rrrs rvalidate_against_hardwarez&CameraConfig.validate_against_hardwaresVV"):S:S:U:UVVV<--// $ $KD%+E22 Hf5  "6{{.??t  $t  rcddlm}i}|jD]:}t |j|j|j|jj kd||j <;|S)z7Detect all connected ZED cameras and return their info.rN)modelid available) rqrrrrsrG camera_modelry camera_state CAMERA_STATE AVAILABLErn)rrdetectedcam_infos rdetect_cameraszCameraConfig.detect_camerassy  1133  HX233k%2bo6OO00HX+ , , rc |}|siSt|}i}gd}t|D]3\}}|t |kr||nd|df\}}|d|d||<4|S)z7Suggest a default config based on detected ZED cameras.)) scene_camerar )right_wrist_camerar )left_wrist_camerar camera_r r)rrr7)rrXrY enumeratelen) r.rserials suggestedrolesirkeyr7s rcreate_default_configz"CameraConfig.create_default_configs&&(( Ix}}''$&     #7++ M MIAv$%E NNa18PIC(.tLLIcNNr)rN)rN) __name__ __module__ __qualname____doc__r rGr0r-r,rr rrBrDrIrKrMrrRrrrVrXrZr^boolr`rjrv staticmethodrrrmrrr r 4sOO*7  C     1111sxc3h/HCHSMsCxXc]SXc] s x}      S     #d38n####)49))))!" c3h  sm   4#$ # (    H 4T ?     DdO   \ tCH~rr )rr)pathlibrtypingrrrrrr raiden._configr r<raiden.cameras.basertuplerGrr rmrrrs: BBBBBBBBBBBBBBBB((((((555 +******@@c3h@@@@VVVVVVVVVVr