예제 #1
0
std::vector<uint8_t> librealsense::command_transfer_over_xu::send_receive(const std::vector<uint8_t>& data, int, bool require_response)
{
    return _uvc.invoke_powered([this, &data, require_response]
        (platform::uvc_device& dev)
        {
            std::vector<uint8_t> result;
            std::lock_guard<platform::uvc_device> lock(dev);

            if (data.size() > HW_MONITOR_BUFFER_SIZE)
            {
                LOG_ERROR("XU command size is invalid");
                throw invalid_value_exception(to_string() << "Requested XU command size " <<
                    std::dec << data.size() << " exceeds permitted limit " << HW_MONITOR_BUFFER_SIZE);
            }

            std::vector<uint8_t> transmit_buf(HW_MONITOR_BUFFER_SIZE, 0);
            std::copy(data.begin(), data.end(), transmit_buf.begin());

            if (!dev.set_xu(_xu, _ctrl, transmit_buf.data(), static_cast<int>(transmit_buf.size())))
                throw invalid_value_exception(to_string() << "set_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno));

            if (require_response)
            {
                result.resize(HW_MONITOR_BUFFER_SIZE);
                if (!dev.get_xu(_xu, _ctrl, result.data(), static_cast<int>(result.size())))
                    throw invalid_value_exception(to_string() << "get_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno));

                // Returned data size located in the last 4 bytes
                auto data_size = *(reinterpret_cast<uint32_t*>(result.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER;
                result.resize(data_size);
            }
            return result;
        });
}
    disparity_transform::disparity_transform(bool transform_to_disparity):
        generic_processing_block(transform_to_disparity ? "Depth to Disparity" : "Disparity to Depth"),
        _transform_to_disparity(transform_to_disparity),
        _update_target(false),
        _stereoscopic_depth(false),
        _focal_lenght_mm(0.f),
        _stereo_baseline_meter(0.f),
        _depth_units(0.f),
        _d2d_convert_factor(0.f),
        _width(0), _height(0), _bpp(0)
    {
        auto transform_opt = std::make_shared<ptr_option<bool>>(
            false,true,true,true,
            &_transform_to_disparity,
            "Stereoscopic Transformation Mode");
        transform_opt->set_description(false, "Disparity to Depth");
        transform_opt->set_description(true, "Depth to Disparity");
        transform_opt->on_set([this, transform_opt](float val)
        {
            std::lock_guard<std::mutex> lock(_mutex);
            if (!transform_opt->is_valid(val))
                throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range.");

            on_set_mode(static_cast<bool>(!!int(val)));
        });

        unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE);

        on_set_mode(_transform_to_disparity);
    }
    hole_filling_filter::hole_filling_filter() :
        _width(0), _height(0), _stride(0), _bpp(0),
        _extension_type(RS2_EXTENSION_DEPTH_FRAME),
        _current_frm_size_pixels(0),
        _hole_filling_mode(hole_fill_def)
    {
        _stream_filter.stream = RS2_STREAM_DEPTH;
        _stream_filter.format = RS2_FORMAT_Z16;

        auto hole_filling_mode = std::make_shared<ptr_option<uint8_t>>(
            hole_fill_min,
            hole_fill_max,
            hole_fill_step,
            hole_fill_def,
            &_hole_filling_mode, "Hole Filling mode");

        hole_filling_mode->set_description(hf_fill_from_left, "Fill from Left");
        hole_filling_mode->set_description(hf_farest_from_around, "Farest from around");
        hole_filling_mode->set_description(hf_nearest_from_around, "Nearest from around");

        hole_filling_mode->on_set([this, hole_filling_mode](float val)
        {
            std::lock_guard<std::mutex> lock(_mutex);

            if (!hole_filling_mode->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported mode for hole filling selected: value " << val << " is out of range.");

            _hole_filling_mode = static_cast<uint8_t>(val);
        });

        register_option(RS2_OPTION_HOLES_FILL, hole_filling_mode);

    }
예제 #4
0
void playback_sensor::handle_frame(frame_holder frame, bool is_real_time)
{
    if(frame == nullptr)
    {
        throw invalid_value_exception("null frame passed to handle_frame");
    }
    if(m_is_started)
    {
        frame->get_owner()->set_sensor(shared_from_this());
        auto type = frame->get_stream()->get_stream_type();
        auto index = static_cast<uint32_t>(frame->get_stream()->get_stream_index());
        frame->set_stream(m_streams[std::make_pair(type, index)]);
        frame->set_sensor(shared_from_this());
        auto stream_id = frame.frame->get_stream()->get_unique_id();
        //TODO: Ziv, remove usage of shared_ptr when frame_holder is cpoyable
        auto pf = std::make_shared<frame_holder>(std::move(frame));
        m_dispatchers.at(stream_id)->invoke([this, pf](dispatcher::cancellable_timer t)
        {
            frame_interface* pframe = nullptr;
            std::swap((*pf).frame, pframe);
            m_user_callback->on_frame((rs2_frame*)pframe);
        });
        if(is_real_time)
        {
            m_dispatchers.at(stream_id)->flush();
        }
    }
}
예제 #5
0
void playback_sensor::register_sensor_options(const device_serializer::sensor_snapshot& sensor_snapshot)
{
    auto options_snapshot = sensor_snapshot.get_sensor_extensions_snapshots().find(RS2_EXTENSION_OPTIONS);
    if (options_snapshot == nullptr)
    {
        LOG_WARNING("Recorded file does not contain sensor options");
        return;
    }
    auto options_api = As<options_interface>(options_snapshot);
    if (options_api == nullptr)
    {
        throw invalid_value_exception("Failed to get options interface from sensor snapshots");
    }

    for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
    {
        auto option_id = static_cast<rs2_option>(i);
        try
        {
            if (options_api->supports_option(option_id))
            {
                auto&& option = options_api->get_option(option_id);
                float value = option.query();
                register_option(option_id, std::make_shared<const_value_option>(option.get_description(), option.query()));
                LOG_DEBUG("Registered " << rs2_option_to_string(option_id) << " for sensor " << m_sensor_id << " with value: " << option.query());
            }
        }
        catch (std::exception& e)
        {
            LOG_WARNING("Failed to register option " << option_id << ". Exception: " << e.what());
        }
    }
}
예제 #6
0
    context::context(backend_type type,
                     const char* filename,
                     const char* section,
                     rs2_recording_mode mode)
        : _devices_changed_callback(nullptr, [](rs2_devices_changed_callback*){})
    {
        LOG_DEBUG("Librealsense " << std::string(std::begin(rs2_api_version),std::end(rs2_api_version)));

        switch(type)
        {
        case backend_type::standard:
            _backend = platform::create_backend();
            break;
        case backend_type::record:
            _backend = std::make_shared<platform::record_backend>(platform::create_backend(), filename, section, mode);
            break;
        case backend_type::playback:
            _backend = std::make_shared<platform::playback_backend>(filename, section);

            break;
        default: throw invalid_value_exception(to_string() << "Undefined backend type " << static_cast<int>(type));
        }

       environment::get_instance().set_time_service(_backend->create_time_service());

       _device_watcher = _backend->create_device_watcher();
    }
예제 #7
0
void playback_sensor::register_sensor_infos(const device_serializer::sensor_snapshot& sensor_snapshot)
{
    auto info_snapshot = sensor_snapshot.get_sensor_extensions_snapshots().find(RS2_EXTENSION_INFO);
    if (info_snapshot == nullptr)
    {
        LOG_WARNING("Recorded file does not contain sensor information");
        return;
    }
    auto info_api = As<info_interface>(info_snapshot);
    if (info_api == nullptr)
    {
        throw invalid_value_exception("Failed to get info interface from sensor snapshots");
    }

    for (int i = 0; i < RS2_CAMERA_INFO_COUNT; ++i)
    {
        rs2_camera_info info = static_cast<rs2_camera_info>(i);
        if (info_api->supports_info(info))
        {
            const std::string& str = info_api->get_info(info);
            register_info(info, str);
            LOG_DEBUG("Registered " << info << " for sensor " << m_sensor_id << " with value: " << str);
        }
    }
}
예제 #8
0
    pointcloud::pointcloud()
        :stream_filter_processing_block("Pointcloud")
    {
        _occlusion_filter = std::make_shared<occlusion_filter>();

        auto occlusion_invalidation = std::make_shared<ptr_option<uint8_t>>(
            occlusion_none,
            occlusion_max - 1, 1,
            occlusion_none,
            (uint8_t*)&_occlusion_filter->_occlusion_filter,
            "Occlusion removal");
        occlusion_invalidation->on_set([this, occlusion_invalidation](float val)
        {
            if (!occlusion_invalidation->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported occlusion filtering requiested " << val << " is out of range.");

            _occlusion_filter->set_mode(static_cast<uint8_t>(val));

        });

        occlusion_invalidation->set_description(0.f, "Off");
        occlusion_invalidation->set_description(1.f, "Heuristic");
        occlusion_invalidation->set_description(2.f, "Exhaustive");
        register_option(RS2_OPTION_FILTER_MAGNITUDE, occlusion_invalidation);
    }
예제 #9
0
 platform::uvc_device_info get_mi(const std::vector<platform::uvc_device_info>& devices, uint32_t mi)
 {
     for (auto&& info : devices)
     {
         if (info.mi == mi)
             return info;
     }
     throw invalid_value_exception("Interface not found!");
 }
예제 #10
0
    void check_vector_for_valid_entries(VectorType const & vec, std::string message = "Location not specified")
    {
      for (std::size_t i=0; i<vec.size(); ++i)
      {
        if ( viennashe::util::is_NaN(vec[i]) )
        {
          std::stringstream ss;
          ss << message << ": Found NaN in vector at index " << i << std::endl;
          throw invalid_value_exception(ss.str());
        }

        if ( viennashe::util::is_Inf(vec[i]) )
        {
          std::stringstream ss;
          ss << message << ": Found infinity in vector at index " << i << std::endl;
          throw invalid_value_exception(ss.str());
        }
      }
    }
예제 #11
0
void librealsense::uvc_pu_option::set(float value)
{
    _ep.invoke_powered(
        [this, value](platform::uvc_device& dev)
        {
            if (!dev.set_pu(_id, static_cast<int32_t>(value)))
                throw invalid_value_exception(to_string() << "set_pu(id=" << std::to_string(_id) << ") failed!" << " Last Error: " << strerror(errno));
            _record(*this);
        });
}
예제 #12
0
bool option_base::is_valid(float value) const
{
    if (!std::isnormal(_opt_range.step))
        throw invalid_value_exception(to_string() << "is_valid(...) failed! step is not properly defined. (" << _opt_range.step << ")");

    if ((value < _opt_range.min) || (value > _opt_range.max))
        return false;

    auto n = (value - _opt_range.min) / _opt_range.step;
    return (fabs(fmod(n, 1)) < std::numeric_limits<float>::min());
}
예제 #13
0
float librealsense::uvc_pu_option::query() const
{
    return static_cast<float>(_ep.invoke_powered(
        [this](platform::uvc_device& dev)
        {
            int32_t value = 0;
            if (!dev.get_pu(_id, value))
                throw invalid_value_exception(to_string() << "get_pu(id=" << std::to_string(_id) << ") failed!" << " Last Error: " << strerror(errno));

            return static_cast<float>(value);
        }));
}
예제 #14
0
    void hw_monitor::execute_usb_command(uint8_t *out, size_t outSize, uint32_t & op, uint8_t * in, size_t & inSize) const
    {
        std::vector<uint8_t> out_vec(out, out + outSize);
        auto res = _locked_transfer->send_receive(out_vec);

        // read
        if (in && inSize)
        {
            if (res.size() < static_cast<int>(sizeof(uint32_t)))
                throw invalid_value_exception("Incomplete bulk usb transfer!");

            if (res.size() > IVCAM_MONITOR_MAX_BUFFER_SIZE)
                throw invalid_value_exception("Out buffer is greater than max buffer size!");

            op = *reinterpret_cast<uint32_t *>(res.data());
            if (res.size() > static_cast<int>(inSize))
                throw invalid_value_exception("bulk transfer failed - user buffer too small");

            inSize = res.size();
            librealsense::copy(in, res.data(), inSize);
        }
    }
예제 #15
0
    void hw_monitor::update_cmd_details(hwmon_cmd_details& details, size_t receivedCmdLen, unsigned char* outputBuffer)
    {
        details.receivedCommandDataLength = receivedCmdLen;

        if (details.oneDirection) return;

        if (details.receivedCommandDataLength < 4)
            throw invalid_value_exception("received incomplete response to usb command");

        details.receivedCommandDataLength -= 4;
        librealsense::copy(details.receivedOpcode.data(), outputBuffer, 4);

        if (details.receivedCommandDataLength > 0)
            librealsense::copy(details.receivedCommandData.data(), outputBuffer + 4, details.receivedCommandDataLength);
    }
예제 #16
0
void librealsense::polling_errors_disable::set(float value)
{
    if (value < 0)
        throw invalid_value_exception("Invalid polling errors disable request " + std::to_string(value));

    if (value == 0)
    {
        _polling_error_handler->stop();
        _value = 0;
    }
    else
    {
        _polling_error_handler->start();
        _value = 1;
    }
    _recording_function(*this);
}
예제 #17
0
    std::vector<uint8_t> hw_monitor::send(command cmd) const
    {
        hwmon_cmd newCommand(cmd);
        auto opCodeXmit = static_cast<uint32_t>(newCommand.cmd);

        hwmon_cmd_details details;
        details.oneDirection = newCommand.oneDirection;
        details.timeOut = newCommand.timeOut;

        fill_usb_buffer(opCodeXmit,
            newCommand.param1,
            newCommand.param2,
            newCommand.param3,
            newCommand.param4,
            newCommand.data,
            newCommand.sizeOfSendCommandData,
            details.sendCommandData.data(),
            details.sizeOfSendCommandData);

        send_hw_monitor_command(details);

        // Error/exit conditions
        if (newCommand.oneDirection)
            return std::vector<uint8_t>();

        librealsense::copy(newCommand.receivedOpcode, details.receivedOpcode.data(), 4);
        librealsense::copy(newCommand.receivedCommandData, details.receivedCommandData.data(), details.receivedCommandDataLength);
        newCommand.receivedCommandDataLength = details.receivedCommandDataLength;

        // endian?
        auto opCodeAsUint32 = pack(details.receivedOpcode[3], details.receivedOpcode[2],
                                   details.receivedOpcode[1], details.receivedOpcode[0]);
        if (opCodeAsUint32 != opCodeXmit)
        {
            throw invalid_value_exception(to_string() << "OpCodes do not match! Sent "
                << opCodeXmit << " but received " << static_cast<int>(opCodeAsUint32) << "!");
        }

        return std::vector<uint8_t>(newCommand.receivedCommandData,
            newCommand.receivedCommandData + newCommand.receivedCommandDataLength);
    }
예제 #18
0
void librealsense::float_option::set(float value)
{
    if (!is_valid(value))
        throw invalid_value_exception(to_string() << "set(...) failed! " << value << " is not a valid value");
    _value = value;
}
예제 #19
0
    disparity_transform::disparity_transform(bool transform_to_disparity):
        _transform_to_disparity(transform_to_disparity),
        _update_target(false),
        _stereoscopic_depth(false),
        _focal_lenght_mm(0.f),
        _stereo_baseline(0.f),
        _depth_units(0.f),
        _d2d_convert_factor(0.f),
        _width(0), _height(0), _bpp(0)
    {
        auto transform_opt = std::make_shared<ptr_option<bool>>(
            false,true,true,true,
            &_transform_to_disparity,
            "Stereoscopic Transformation Mode");
        transform_opt->set_description(false, "Disparity to Depth");
        transform_opt->set_description(true, "Depth to Disparity");
        transform_opt->on_set([this, transform_opt](float val)
        {
            std::lock_guard<std::mutex> lock(_mutex);
            if (!transform_opt->is_valid(val))
                throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range.");

            on_set_mode(static_cast<bool>(!!int(val)));
        });

        unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE);

        auto on_frame = [this](rs2::frame f, const rs2::frame_source& source)
        {
            std::lock_guard<std::mutex> lock(_mutex);

            rs2::frame out = f;
            rs2::frame tgt, depth_data;

            bool composite = f.is<rs2::frameset>();

            tgt = depth_data = (composite) ? f.as<rs2::frameset>().first_or_default(RS2_STREAM_DEPTH) : f;

            // Verify that the input depth format is aligned with the block's configuration
            if (depth_data &&  (f.is<rs2::disparity_frame>() != _transform_to_disparity))
            {
                update_transformation_profile(depth_data);

                if (_stereoscopic_depth && (tgt = prepare_target_frame(depth_data, source)))
                {
                    auto src = depth_data.as<rs2::video_frame>();

                    if (_transform_to_disparity)
                        convert<uint16_t, float>(src.get_data(), const_cast<void*>(tgt.get_data()));
                    else
                        convert<float, uint16_t>(src.get_data(), const_cast<void*>(tgt.get_data()));
                }
            }

            out = composite ? source.allocate_composite_frame({ tgt }) : tgt;

            source.frame_ready(out);
        };

        auto callback = new rs2::frame_processor_callback<decltype(on_frame)>(on_frame);
        processing_block::set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(callback));

        on_set_mode(_transform_to_disparity);
    }