Merge pull request #5111 from arilowen/sensor-refactoring

Sensor refactoring

.travis.yml

@@ -70,9 +70,11 @@ matrix:
       script:
         - cmake .. -DBUILD_PYTHON_BINDINGS=true -DBUILD_NODEJS_BINDINGS=true -DPYBIND11_PYTHON_VERSION=2.7
         - cmake --build . --config $LRS_BUILD_CONFIG -- -j4
-        - cd ../wrappers/nodejs/test
-        - wget http://realsense-hw-public.s3.amazonaws.com/rs-tests/nodejs_record.rec
-        - mocha test-functional.js --playback nodejs_record.rec
+        - cd ../wrappers/nodejs/
+        - npm install
+        - cd test
+        - wget http://realsense-hw-public.s3.amazonaws.com/rs-tests/nodejs_record_tests.rec
+        - mocha test-functional.js --playback nodejs_record_tests.rec
 
     - name: "Mac - cpp"
       os: osx
@@ -115,7 +117,7 @@ before_install:
   - if [[ "$RS_CPP_TEST" == "true" ]]; then
       export LRS_LOG_LEVEL="DEBUG";
       url_records_path="http://realsense-hw-public.s3.amazonaws.com/rs-tests/lrs_2.8.3/";
-      records_name="records.txt";
+      records_name="records_test.txt";
       url_records_list=$url_records_path$records_name;
       wget $url_records_path$records_name;
       tests_list=`cat $records_name`;

include/librealsense2/h/rs_frame.h

@@ -255,6 +255,19 @@ int rs2_is_frame_extendable_to(const rs2_frame* frame, rs2_extension extension_t
 rs2_frame* rs2_allocate_synthetic_video_frame(rs2_source* source, const rs2_stream_profile* new_stream, rs2_frame* original,
     int new_bpp, int new_width, int new_height, int new_stride, rs2_extension frame_type, rs2_error** error);
 
+/**
+* Allocate new motion frame using a frame-source provided form a processing block
+* \param[in] source      Frame pool to allocate the frame from
+* \param[in] new_stream  New stream profile to assign to newly created frame
+* \param[in] original    A reference frame that can be used to fill in auxilary information like format, width, height, bpp, stride (if applicable)
+* \param[in] frame_type  New value for frame type for the allocated frame
+* \param[out] error      If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+* \return                reference to a newly allocated frame, must be released with release_frame
+*                        memory for the frame is likely to be re-used from previous frame, but in lack of available frames in the pool will be allocated from the free store
+*/
+rs2_frame* rs2_allocate_synthetic_motion_frame(rs2_source* source, const rs2_stream_profile* new_stream, rs2_frame* original,
+    rs2_extension frame_type, rs2_error** error);
+
 /**
 * Allocate new points frame using a frame-source provided from a processing block
 * \param[in] source      Frame pool to allocate the frame from

include/librealsense2/h/rs_sensor.h

@@ -80,6 +80,11 @@ typedef enum rs2_format
     RS2_FORMAT_Y10BPACK        , /**< 16-bit per-pixel grayscale image unpacked from 10 bits per pixel packed ([8:8:8:8:2222]) grey-scale image. The data is unpacked to LSB and padded with 6 zero bits */
     RS2_FORMAT_DISTANCE        , /**< 32-bit float-point depth distance value.  */
     RS2_FORMAT_MJPEG           , /**< Bitstream encoding for video in which an image of each frame is encoded as JPEG-DIB   */
+    RS2_FORMAT_Y8I             , /**< 8-bit per pixel interleaved. 8-bit left, 8-bit right.  */
+    RS2_FORMAT_Y12I            , /**< 12-bit per pixel interleaved. 12-bit left, 12-bit right. Each pixel is stored in a 24-bit word in little-endian order. */
+    RS2_FORMAT_INZI            , /**< multi-planar Depth 16bit + IR 10bit.  */
+    RS2_FORMAT_INVI            , /**< 8-bit IR stream.  */
+    RS2_FORMAT_W10             , /**< Grey-scale image as a bit-packed array. 4 pixel data stream taking 5 bytes */
     RS2_FORMAT_COUNT             /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
 } rs2_format;
 const char* rs2_format_to_string(rs2_format format);

include/librealsense2/hpp/rs_processing.hpp

@@ -26,7 +26,7 @@ namespace rs2
         * \param[in] new_bpp     Frame bit per pixel to create.
         * \param[in] new_width   Frame width to create.
         * \param[in] new_height  Frame height to create.
-        * \param[in] new_stride  Frame stride to crate.
+        * \param[in] new_stride  Frame stride to create.
         * \param[in] frame_type  Which frame type are going to create.
         * \return   The allocated frame
         */
@@ -45,6 +45,25 @@ namespace rs2
             return result;
         }
 
+        /**
+        * Allocate a new motion frame with given params
+        *
+        * \param[in] profile     Stream profile going to allocate.
+        * \param[in] original    Original frame.
+        * \param[in] frame_type  Which frame type are going to create.
+        * \return   The allocated frame
+        */
+        frame allocate_motion_frame(const stream_profile& profile,
+            const frame& original,
+            rs2_extension frame_type = RS2_EXTENSION_MOTION_FRAME) const
+        {
+            rs2_error* e = nullptr;
+            auto result = rs2_allocate_synthetic_motion_frame(_source, profile.get(),
+                original.get(), frame_type, &e);
+            error::handle(e);
+            return result;
+        }
+
         frame allocate_points(const stream_profile& profile,
             const frame& original) const
         {

src/algo.cpp

@@ -63,8 +63,8 @@ auto_exposure_mechanism::auto_exposure_mechanism(option& gain_option, option& ex
             if (!_keep_alive)
                 return;
 
-            frame_and_callback frame_callback;
-            auto frame_sts = _data_queue.dequeue(&frame_callback, RS2_DEFAULT_TIMEOUT);
+            frame_holder f_holder;
+            auto frame_sts = _data_queue.dequeue(&f_holder, RS2_DEFAULT_TIMEOUT);
 
             lk.unlock();
 
@@ -75,7 +75,7 @@ auto_exposure_mechanism::auto_exposure_mechanism(option& gain_option, option& ex
             }
             try
             {
-                auto frame = std::move(frame_callback.f_holder);
+                auto frame = std::move(f_holder);
 
                 double values[2] = {};
 
@@ -147,7 +147,7 @@ void auto_exposure_mechanism::update_auto_exposure_roi(const region_of_interest&
     _auto_exposure_algo.update_roi(roi);
 }
 
-void auto_exposure_mechanism::add_frame(frame_holder frame, callback_invocation_holder callback)
+void auto_exposure_mechanism::add_frame(frame_holder frame)
 {
 
     if (!_keep_alive || (_skip_frames && (_frames_counter++) != _skip_frames))
@@ -159,7 +159,7 @@ void auto_exposure_mechanism::add_frame(frame_holder frame, callback_invocation_
 
     {
         std::lock_guard<std::mutex> lk(_queue_mtx);
-        _data_queue.enqueue({std::move(frame), std::move(callback)});
+        _data_queue.enqueue(std::move(frame));
     }
     _cv.notify_one();
 }

src/algo.h

@@ -102,16 +102,11 @@ namespace librealsense
         std::recursive_mutex state_mutex;
     };
 
-    struct frame_and_callback{
-        frame_holder f_holder;
-        callback_invocation_holder callback;
-    };
-
     class auto_exposure_mechanism {
     public:
         auto_exposure_mechanism(option& gain_option, option& exposure_option, const auto_exposure_state& auto_exposure_state);
         ~auto_exposure_mechanism();
-        void add_frame(frame_holder frame, callback_invocation_holder callback);
+        void add_frame(frame_holder frame);
         void update_auto_exposure_state(const auto_exposure_state& auto_exposure_state);
         void update_auto_exposure_roi(const region_of_interest& roi);
 
@@ -129,16 +124,14 @@ namespace librealsense
         };
 
     private:
-        bool try_pop_front_data(frame_and_callback* data);
-
         static const int                          queue_size = 2;
         option&                                   _gain_option;
         option&                                   _exposure_option;
         auto_exposure_algorithm                   _auto_exposure_algo;
         std::shared_ptr<std::thread>              _exposure_thread;
         std::condition_variable                   _cv;
         std::atomic<bool>                         _keep_alive;
-        single_consumer_queue<frame_and_callback> _data_queue;
+        single_consumer_queue<frame_holder>       _data_queue;
         std::mutex                                _queue_mtx;
         std::atomic<unsigned>                     _frames_counter;
         std::atomic<unsigned>                     _skip_frames;

src/context.cpp

@@ -93,6 +93,15 @@ bool contains(const std::shared_ptr<librealsense::device_info>& first,
 
 namespace librealsense
 {
+    std::map<uint32_t, rs2_format> platform_color_fourcc_to_rs2_format = {
+        {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV},
+        {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}
+    };
+    std::map<uint32_t, rs2_stream> platform_color_fourcc_to_rs2_stream = {
+        {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR},
+        {rs_fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}
+    };
+
     context::context(backend_type type,
                      const char* filename,
                      const char* section,
@@ -210,14 +219,12 @@ namespace librealsense
         std::vector<platform::uvc_device_info> _uvcs;
     };
 
-    class platform_camera_sensor : public uvc_sensor
+    class platform_camera_sensor : public synthetic_sensor
     {
     public:
-        platform_camera_sensor(const std::shared_ptr<context>&,
-            device* owner,
-            std::shared_ptr<platform::uvc_device> uvc_device,
-            std::unique_ptr<frame_timestamp_reader> timestamp_reader)
-            : uvc_sensor("RGB Camera", uvc_device, move(timestamp_reader), owner),
+        platform_camera_sensor(device* owner,
+            std::shared_ptr<uvc_sensor> uvc_sensor)
+            : synthetic_sensor("RGB Camera", uvc_sensor, owner),
               _default_stream(new stream(RS2_STREAM_COLOR))
         {
         }
@@ -226,14 +233,15 @@ namespace librealsense
         {
             auto lock = environment::get_instance().get_extrinsics_graph().lock();
 
-            auto results = uvc_sensor::init_stream_profiles();
+            auto results = synthetic_sensor::init_stream_profiles();
 
-            for (auto p : results)
+            for (auto&& p : results)
             {
                 // Register stream types
                 assign_stream(_default_stream, p);
                 environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_default_stream, *p);
             }
+            add_source_profiles_missing_data();
 
             return results;
         }
@@ -255,9 +263,11 @@ namespace librealsense
             std::vector<std::shared_ptr<platform::uvc_device>> devs;
             for (auto&& info : uvc_infos)
                 devs.push_back(ctx->get_backend().create_uvc_device(info));
-            auto color_ep = std::make_shared<platform_camera_sensor>(ctx, this,
-                                                                     std::make_shared<platform::multi_pins_uvc_device>(devs),
-                                                                     std::unique_ptr<ds5_timestamp_reader>(new ds5_timestamp_reader(environment::get_instance().get_time_service())));
+            auto raw_color_ep = std::make_shared<uvc_sensor>("Raw RGB Camera",
+                std::make_shared<platform::multi_pins_uvc_device>(devs),
+                std::unique_ptr<ds5_timestamp_reader>(new ds5_timestamp_reader(environment::get_instance().get_time_service())),
+                this);
+            auto color_ep = std::make_shared<platform_camera_sensor>(this, raw_color_ep);
             add_sensor(color_ep);
 
             register_info(RS2_CAMERA_INFO_NAME, "Platform Camera");
@@ -268,20 +278,30 @@ namespace librealsense
             register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, uvc_infos.front().device_path);
             register_info(RS2_CAMERA_INFO_PRODUCT_ID, pid_str);
 
-            color_ep->register_pixel_format(pf_yuy2);
-            color_ep->register_pixel_format(pf_yuyv);
-
-            color_ep->try_register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
-            color_ep->try_register_pu(RS2_OPTION_BRIGHTNESS);
-            color_ep->try_register_pu(RS2_OPTION_CONTRAST);
-            color_ep->try_register_pu(RS2_OPTION_EXPOSURE);
-            color_ep->try_register_pu(RS2_OPTION_GAMMA);
-            color_ep->try_register_pu(RS2_OPTION_HUE);
-            color_ep->try_register_pu(RS2_OPTION_SATURATION);
-            color_ep->try_register_pu(RS2_OPTION_SHARPNESS);
-            color_ep->try_register_pu(RS2_OPTION_WHITE_BALANCE);
-            color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_EXPOSURE);
-            color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
+            color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_RGB8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_RGBA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_RGBA8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_BGR8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_BGR8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_BGRA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_BGRA8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_Y16, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_Y16); });
+            color_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_YUYV, RS2_STREAM_COLOR));
+
+            color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_RGB8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_RGBA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_RGBA8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_BGR8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_BGR8); });
+            color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_BGRA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_BGRA8); });
+            color_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_UYVY, RS2_STREAM_COLOR));
+
+            raw_color_ep->try_register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
+            raw_color_ep->try_register_pu(RS2_OPTION_BRIGHTNESS);
+            raw_color_ep->try_register_pu(RS2_OPTION_CONTRAST);
+            raw_color_ep->try_register_pu(RS2_OPTION_EXPOSURE);
+            raw_color_ep->try_register_pu(RS2_OPTION_GAMMA);
+            raw_color_ep->try_register_pu(RS2_OPTION_HUE);
+            raw_color_ep->try_register_pu(RS2_OPTION_SATURATION);
+            raw_color_ep->try_register_pu(RS2_OPTION_SHARPNESS);
+            raw_color_ep->try_register_pu(RS2_OPTION_WHITE_BALANCE);
+            raw_color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_EXPOSURE);
+            raw_color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
         }
 
         virtual rs2_intrinsics get_intrinsics(unsigned int, const stream_profile&) const

src/core/advanced_mode.h

@@ -108,7 +108,7 @@ namespace librealsense
     {
     public:
         explicit ds5_advanced_mode_base(std::shared_ptr<hw_monitor> hwm,
-                                        uvc_sensor& depth_sensor);
+            synthetic_sensor& depth_sensor);
 
         void create_snapshot(std::shared_ptr<ds5_advanced_mode_interface>& snapshot) const override {};
         void enable_recording(std::function<void(const ds5_advanced_mode_interface&)> recording_function) override {};
@@ -156,10 +156,10 @@ namespace librealsense
         static const uint16_t HW_MONITOR_BUFFER_SIZE = 1024;
 
     private:
-        void set_exposure(uvc_sensor& sensor, const exposure_control& val);
-        void set_auto_exposure(uvc_sensor& sensor, const auto_exposure_control& val);
-        void get_exposure(uvc_sensor& sensor, exposure_control* ptr) const;
-        void get_auto_exposure(uvc_sensor& sensor, auto_exposure_control* ptr) const;
+        void set_exposure(synthetic_sensor& sensor, const exposure_control& val);
+        void set_auto_exposure(synthetic_sensor& sensor, const auto_exposure_control& val);
+        void get_exposure(synthetic_sensor& sensor, exposure_control* ptr) const;
+        void get_auto_exposure(synthetic_sensor& sensor, auto_exposure_control* ptr) const;
 
         void get_laser_power(laser_power_control* ptr) const;
         void get_laser_state(laser_state_control* ptr) const;
@@ -201,10 +201,10 @@ namespace librealsense
         void set_color_auto_white_balance(const auto_white_balance_control& val);
         void set_color_power_line_frequency(const power_line_frequency_control& val);
 
-        bool supports_option(const uvc_sensor& sensor, rs2_option opt) const;
+        bool supports_option(const synthetic_sensor& sensor, rs2_option opt) const;
 
         std::shared_ptr<hw_monitor> _hw_monitor;
-        uvc_sensor& _depth_sensor;
+        synthetic_sensor& _depth_sensor;
         lazy<ds5_color_sensor*> _color_sensor;
         lazy<bool> _enabled;
         std::shared_ptr<advanced_mode_preset_option> _preset_opt;
@@ -258,7 +258,7 @@ namespace librealsense
     class advanced_mode_preset_option : public option_base
     {
     public:
-        advanced_mode_preset_option(ds5_advanced_mode_base& advanced, uvc_sensor& ep,
+        advanced_mode_preset_option(ds5_advanced_mode_base& advanced, synthetic_sensor& ep,
                                     const option_range& opt_range);
 
         static rs2_rs400_visual_preset to_preset(float x);
@@ -273,7 +273,7 @@ namespace librealsense
         firmware_version get_firmware_version(const uvc_sensor& sensor) const;
 
         std::mutex _mtx;
-        uvc_sensor& _ep;
+        synthetic_sensor& _ep;
         ds5_advanced_mode_base& _advanced;
         rs2_rs400_visual_preset _last_preset;
     };

src/core/processing.h

@@ -29,6 +29,10 @@ namespace librealsense
                                                       int new_stride = 0,
                                                       rs2_extension frame_type = RS2_EXTENSION_VIDEO_FRAME) = 0;
 
+        virtual frame_interface* allocate_motion_frame(std::shared_ptr<stream_profile_interface> stream,
+                                                       frame_interface* original,
+                                                       rs2_extension frame_type = RS2_EXTENSION_MOTION_FRAME) = 0;
+
         virtual frame_interface* allocate_composite_frame(std::vector<frame_holder> frames) = 0;
 
         virtual frame_interface* allocate_points(std::shared_ptr<stream_profile_interface> stream, 

src/core/streaming.h

@@ -85,7 +85,6 @@ namespace librealsense
         virtual bool supports_frame_metadata(const rs2_frame_metadata_value& frame_metadata) const = 0;
         virtual int get_frame_data_size() const = 0;
         virtual const byte* get_frame_data() const = 0;
-        //TODO: add virtual uint64_t get_frame_data_size() const = 0;
         virtual rs2_time_t get_frame_timestamp() const = 0;
         virtual rs2_timestamp_domain get_frame_timestamp_domain() const = 0;
         virtual void set_timestamp(double new_ts) = 0;
@@ -165,6 +164,14 @@ namespace librealsense
     using stream_profiles = std::vector<std::shared_ptr<stream_profile_interface>>;
     using processing_blocks = std::vector<std::shared_ptr<processing_block_interface>>;
 
+    inline std::ostream& operator << (std::ostream& os, const stream_profiles& profiles)
+    {
+        for (auto&& p : profiles)
+        {
+            os << rs2_format_to_string(p->get_format()) << " " << rs2_stream_to_string(p->get_stream_type()) << ", ";
+        }
+        return os;
+    }
 
     class recommended_proccesing_blocks_interface
     {