*ih6dZddlZddlmZddlmZmZddlZddl Z ddl m Z m Z de jddfd ZGd d e ZdS) au Intel RealSense camera implementation (e.g. D405). Recording mode opens by serial number and records to .bag via the SDK recorder. Playback mode opens a .bag file and reads color + depth frames; use ``RealSenseCamera.from_bag()`` to create a playback instance. .. warning:: **For dynamic tasks, prefer ZED cameras over RealSense.** RealSense cameras have known synchronization limitations that make them less suitable for capturing fast robot motion. See *Synchronization issues* below for details. Intrinsics note --------------- RealSense cameras store per-device calibration on-board. get_intrinsics() reads the color stream's Brown-Conrady coefficients directly from the SDK, which is the same model OpenCV uses, so the values can be passed straight to cv2 functions without any conversion. Synchronization issues ---------------------- RealSense cameras (especially the D405) have several properties that make multi-camera synchronization harder than with ZED cameras: **Timestamp reliability** The ``global_time_enabled`` option is supposed to stamp frames with the host wall-clock time, but support is inconsistent across D4xx firmware versions. When it is not supported, the SDK reports hardware-relative timestamps (milliseconds since device boot) that must be converted to wall-clock time. ``_measure_clock_offset()`` compensates for this by measuring the offset between the first frame timestamp and ``time.time_ns()`` at recording start, but this is only an approximation. **Frame extraction drain-loop bug** The RealSense SDK pre-buffers frames during bag playback when ``set_real_time(False)`` is set. A naive "drain the queue to get the latest frame" loop (appropriate for live preview) will consume every other buffered frame, halving the apparent FPS to ~15 fps even when the bag was recorded at 30 fps. ``grab()`` therefore skips the drain loop in playback mode (``_is_playback = True``). **Recommendation** Use ZED cameras for scene and wrist cameras in dynamic manipulation tasks. ZED timestamps (``sl.TIME_REFERENCE.IMAGE``) are wall-clock Unix nanoseconds, consistent across cameras and with the robot controller clock, making multi-camera alignment straightforward. RealSense cameras are best suited for static or slow-motion scenes where their close-range depth quality is the primary requirement. N)Path)OptionalTuple)Camera CameraFramedevicereturnc|D]K}|tjjr%|tjjdLdS)z7Enable global timestamp on all sensors that support it.rN) query_sensorssupportsrsoptionglobal_time_enabled set_option)r sensors 6/home/robot-lab/raiden_cmu/raiden/cameras/realsense.py_enable_global_timer<sa&&((@@ ??298 9 9 @   bi;Q ? ? ?@@c^eZdZdZd\ZZd\ZZddedede fdZ e defd Z e defd Z e defd ZddZddZddZdeddfdZddZddZdefdZde fdZdefdZdeejejee e fffdZedededdfdZ de!fdZ"dS) RealSenseCamerau/Intel RealSense D4xx camera – records to .bag)ii camera_name serial_numberfpsc||_||_||_d|_d|_d|_d|_d|_|j|_ |j |_ d|_ dS)NMbP?) _name_serial_fps _pipeline_config_profile _depth_scale_latest_frames_COLOR_W_color_w_COLOR_H_color_h_clock_offset_ns)selfrrrs r__init__zRealSenseCamera.__init__Is^  $  04,0 7; #("!] !] 04rr c|jSN)rr+s rnamezRealSenseCamera.name_s zrc*t|jSr.)strrr/s rrzRealSenseCamera.serial_numbercs4<   rcdS)Nbagr/s rrecording_extensionz#RealSenseCamera.recording_extensiongsurcfg rs.configNc,|j||_||_|jt jj}| |_ | |_ dS)z>Start the pipeline and record the negotiated color resolution.N) r!startr#r" get_streamrstreamcoloras_video_stream_profilewidthr'heightr))r+r7 color_streams r_start_pipelinezRealSenseCamera._start_pipelineosy,,S11  }// IO  ! ! # # %**,, $++-- rc tj|_tj}||j|tjj|j |j tj j |j |tjj|j|jtj j|j ||t'd|jd|jd|jd|j d |j}t3||}||_dS)N [z] opened: BGR8 × @ r)rpipeliner!config enable_devicer enable_streamr<r=r&r(formatbgr8r depth_DEPTH_W_DEPTH_Hz16rBprintrr'r)r# get_devicerfirst_depth_sensorget_depth_scaler$)r+r7r depth_sensors ropenzRealSenseCamera.openysYikk $,'''  IOT]DM29>49     IOT]DM29=$)    S!!!  F$* F FM F F%)] F F7;y F F F   ))++F###0022 (88::rcf|jr)|jd|_d|_dSdSr.)r!stopr#r/s rclosezRealSenseCamera.closes< > ! N   ! ! !!DN DMMM ! !rpathc |jr|jtj}||j|tjj|j |j tj j |j |tjj|j|jtj j|j |t'|||t+|jt1d|jd|jd|jd|j d |dS)z,Restart pipeline with bag recorder attached.rDz] recording: BGR8 rErFrN)r!rXrrHrIrrJr<r=r&r(rKrLr rMrNrOrPenable_record_to_filer2rBrr#rRrQrr'r)_measure_clock_offset)r+rZr7s rstart_recordingzRealSenseCamera.start_recordingsl > " N   ! ! !ikk $,'''  IOT]DM29>49     IOT]DM29=$)    !!#d)),,, S!!!DM4466777  F$* F FM F F%)] F F7;y F F F    ""$$$$$rc  |jd}tj}|}|r0t |dz}||z |_dSdS#t$r d|_YdSwxYw)abGrab the first available frame and compute RS-to-wall-clock offset. ``_clock_offset_ns`` is the additive correction such that: wall_ns = int(frame.get_timestamp() * 1_000_000) + _clock_offset_ns This works whether or not global_time_enabled is supported by the device: if it is supported the stored timestamps are already wall-clock and the offset will be ~0; if not, the offset captures the difference between the RealSense hardware clock and the system clock at the moment recording begins, which is stable enough over a 10-60 s episode. i timeout_ms@BN) r!wait_for_framestimetime_nsget_color_frameint get_timestampr* Exception)r+frameswall_ns color_framers_nss rr]z%RealSenseCamera._measure_clock_offsets )^33t3DDFlnnG 0022K 8K5577)CDD(/%%%% 8 8 ) ) )$(D ! ! ! ! )sA2A88B  B cP|jr|jtj}||j|tjj|j |j tj j |j |tjj|j|jtj j|j ||_|j||_t+|jdS)z;Restart pipeline without recorder to finalize the bag file.N)r!rXrrHrIrrJr<r=r'r)rKrLr rMrNrOrPr"r:r#rrR)r+r7s rstop_recordingzRealSenseCamera.stop_recordings > " N   ! ! !ikk $,'''  IOT]DM29>49     IOT]DM29=$)    ,,S11 DM446677777rc |jd}t|dds% |jd\}}|sn|}$||_dS#t $rYdSwxYw)Nir` _is_playbackFTr)r!rcgetattrtry_wait_for_framesr% RuntimeError)r+rjoknewers rgrabzRealSenseCamera.grabs ^33s3CCF477 # # $ B Ba B P PIB"F # #)D 4   55 sAA A*)A*c|j?|j}|r$t|dzSddl}|jS)a(Return the capture timestamp of the most recently grabbed frame. With global_time_enabled the RealSense SDK stamps frames with system wall-clock time, so this is on the same clock as time.time_ns() and can be used directly to align robot data with camera frames. Nrbr)r%rfrgrhrdre)r+framerds rget_current_timestamp_nsz(RealSenseCamera.get_current_timestamp_nss_   *'7799E >5..009<=== t|~~rc.|jtd|j}t|dd}|||}|}|}t j|}t j|}||j z t j }t| dz}t|||S)Nz'No frames available. Call grab() first._alignrb)r=rM timestamp_ns)r%rtrrprocessrfget_depth_framenp asanyarrayget_datar$astypefloat32rgrhr) r+rjalignrl depth_framer= depth_rawrMr}s r get_framezRealSenseCamera.get_frames   &HII I$h--  ]]6**F,,.. ,,..  k224455M+"6"6"8"899 T..66rzBB;4466BCC e,OOOOrc|jtd|jtjj}|}tj |j d|j gd|j |j ggdgtj}tj |jddtj}|j|jf}|||fS)aReturn on-device calibrated intrinsics for the color stream. RealSense stores per-unit calibration on-board. The distortion model is Brown-Conrady (identical to OpenCV's), so coefficients are directly usable with cv2 functions. Nz&Camera is not open. Call open() first.)rrg?)dtype)r#rtr;rr<r=r>get_intrinsicsrarrayfxppxfyppyfloat64coeffsr?r@)r+rAintr camera_matrix dist_coeffs image_sizes rrzRealSenseCamera.get_intrinsics s = GHH H}// IO  ! ! # # **,,gsDH %TWdh'? Q*   ht{2A2bjAAA j$+. k:55rbag_pathc||}||_d|_d|_d|_d|_t j|_t j |_ t j |j t|d|j |j |_|j}|d |j}||_n#t($rYnwxYwt jt jj|_d|_|S)z/Open a .bag file for frame-by-frame extraction.rrNF)repeat_playbackT)__new__rrr r$r%rrGr!rHr"enable_device_from_filer2r:r#rR as_playback set_real_timerSrTrirr<r=r|rq)clsrrcamplaybackdss rfrom_bagzRealSenseCamera.from_bag%sQkk#   !  ikk  )) KX *   }**3;77 <**,,88::u%%% ((**==??B!1133C      D Xbio..  sAE EEc|jsiS|jtjj}|}|jd|j|j |j |j |j |j |j|jd|jd|jd|jd|jddS)z6Return camera intrinsics and metadata as a plain dict. RealSenserr)rmodelrr?r@rrcxcyk1k2p1p2k3)r#r;rr<r=r>rrr r?r@rrrrr)r+rArs rget_camera_infozRealSenseCamera.get_camera_infoFs} I}// IO  ! ! # # **,,!\ 9Zk''((+a.+a.+a.+a.+a.   r)r)r7r8r N)r N)#__name__ __module__ __qualname____doc__r&r(rNrOr2rgr,propertyr0rr6rBrVrYrr^r]roboolrwrzrrrrndarrayr classmethodrdictrr5rrrrCs299!Hh!Hh44C44#4444,cX!s!!!X!SX....;;;;(!!!! %D%T%%%%,)))),8888&d& #    P;PPPP*6bj"*eCHo&M N666683$;L[@       rr)rrdpathlibrtypingrrnumpyr pyrealsense2rbaserrr rrr5rrrs//b """"""""%%%%%%%%@ @d@@@@Z Z Z Z Z fZ Z Z Z Z r