Skip to content

Commit

Permalink
PR #12819 from Eran: DDS ROI support; enums in adapter
Browse files Browse the repository at this point in the history
  • Loading branch information
maloel authored Apr 10, 2024
2 parents 3c910ca + fd26216 commit c17054a
Show file tree
Hide file tree
Showing 15 changed files with 499 additions and 136 deletions.
100 changes: 49 additions & 51 deletions common/stream-model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -281,69 +281,67 @@ namespace rs2
// x,y remain the same, only update the width,height with new mouse position relative to starting mouse position
roi_display_rect.w = mouse.cursor.x - roi_display_rect.x;
roi_display_rect.h = mouse.cursor.y - roi_display_rect.y;
}
// Case 3: We are in middle of dragging (capturing) and mouse was released
if (!mouse.mouse_down[0] && capturing_roi && stream_rect.contains(mouse.cursor))
{
// Update width,height one last time
roi_display_rect.w = mouse.cursor.x - roi_display_rect.x;
roi_display_rect.h = mouse.cursor.y - roi_display_rect.y;
capturing_roi = false; // Mark that we are no longer dragging

if (roi_display_rect) // If the rect is not empty?
// Case 3: We are in middle of dragging (capturing) and mouse was released
if( ! mouse.mouse_down[0] )
{
// Convert from local (pixel) coordinate system to device coordinate system
auto r = roi_display_rect;
r = r.normalize(stream_rect).unnormalize(_normalized_zoom.unnormalize(get_original_stream_bounds()));
dev->roi_rect = r; // Store new rect in device coordinates into the subdevice object

// Send it to firmware:
// Step 1: get rid of negative width / height
region_of_interest roi{};
roi.min_x = static_cast<int>(std::min(r.x, r.x + r.w));
roi.max_x = static_cast<int>(std::max(r.x, r.x + r.w));
roi.min_y = static_cast<int>(std::min(r.y, r.y + r.h));
roi.max_y = static_cast<int>(std::max(r.y, r.y + r.h));

try
capturing_roi = false; // Mark that we are no longer dragging

if (roi_display_rect) // If the rect is not empty?
{
// Step 2: send it to firmware
if (sensor->is<roi_sensor>())
// Convert from local (pixel) coordinate system to device coordinate system
auto r = roi_display_rect;
r = r.normalize(stream_rect).unnormalize(_normalized_zoom.unnormalize(get_original_stream_bounds()));
dev->roi_rect = r; // Store new rect in device coordinates into the subdevice object

// Send it to firmware:
// Step 1: get rid of negative width / height
region_of_interest roi{};
roi.min_x = static_cast<int>(std::min(r.x, r.x + r.w));
roi.max_x = static_cast<int>(std::max(r.x, r.x + r.w));
roi.min_y = static_cast<int>(std::min(r.y, r.y + r.h));
roi.max_y = static_cast<int>(std::max(r.y, r.y + r.h));

try
{
// Step 2: send it to firmware
if (sensor->is<roi_sensor>())
{
sensor->as<roi_sensor>().set_region_of_interest(roi);
}
}
catch (const error& e)
{
sensor->as<roi_sensor>().set_region_of_interest(roi);
error_message = error_to_string(e);
}
}
catch (const error& e)
else // If the rect is empty
{
error_message = error_to_string(e);
}
}
else // If the rect is empty
{
try
{
// To reset ROI, just set ROI to the entire frame
auto x_margin = (int)size.x / 8;
auto y_margin = (int)size.y / 8;
try
{
// To reset ROI, just set ROI to the entire frame
auto x_margin = (int)size.x / 8;
auto y_margin = (int)size.y / 8;

// Default ROI behavior is center 3/4 of the screen:
if (sensor->is<roi_sensor>())
{
sensor->as<roi_sensor>().set_region_of_interest({ x_margin, y_margin,
(int)size.x - x_margin - 1,
(int)size.y - y_margin - 1 });
}

// Default ROI behavior is center 3/4 of the screen:
if (sensor->is<roi_sensor>())
roi_display_rect = { 0, 0, 0, 0 };
dev->roi_rect = { 0, 0, 0, 0 };
}
catch (const error& e)
{
sensor->as<roi_sensor>().set_region_of_interest({ x_margin, y_margin,
(int)size.x - x_margin - 1,
(int)size.y - y_margin - 1 });
error_message = error_to_string(e);
}

roi_display_rect = { 0, 0, 0, 0 };
dev->roi_rect = { 0, 0, 0, 0 };
}
catch (const error& e)
{
error_message = error_to_string(e);
}
}

dev->roi_checked = false;
dev->roi_checked = false;
}
}
// If we left stream bounds while capturing, stop capturing
if (capturing_roi && !stream_rect.contains(mouse.cursor))
Expand Down
1 change: 1 addition & 0 deletions common/viewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -791,6 +791,7 @@ namespace rs2
_hidden_options.emplace(RS2_OPTION_FRAMES_QUEUE_SIZE);
_hidden_options.emplace(RS2_OPTION_SENSOR_MODE);
_hidden_options.emplace(RS2_OPTION_NOISE_ESTIMATION);
_hidden_options.emplace(RS2_OPTION_REGION_OF_INTEREST);
}

void viewer_model::update_configuration()
Expand Down
20 changes: 18 additions & 2 deletions include/librealsense2/h/rs_option.h
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,8 @@ extern "C" {
RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, /**< Select depth sensor auto exposure mode see rs2_depth_auto_exposure_mode for values */
RS2_OPTION_OHM_TEMPERATURE, /**< Temperature of the Optical Head Sensor */
RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */
RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level, see rs2_gyro_sensitivity for values */
RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level, see rs2_gyro_sensitivity for values */
RS2_OPTION_REGION_OF_INTEREST,/**< The rectangular area used from the streaming profile */
RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
} rs2_option;

Expand All @@ -149,6 +150,7 @@ extern "C" {
RS2_OPTION_TYPE_FLOAT,
RS2_OPTION_TYPE_STRING,
RS2_OPTION_TYPE_BOOLEAN,
RS2_OPTION_TYPE_RECT,

RS2_OPTION_TYPE_COUNT

Expand All @@ -160,18 +162,32 @@ extern "C" {
*/
const char * rs2_option_type_to_string( rs2_option_type type );

/**
* A rectangle expressed in 64 bits, used with rs2_option_value::as_rect.
* Same semantics as rs2_set_region_of_interest.
*/
typedef struct rs2_option_rect
{
int16_t x1, y1;
int16_t x2, y2;
} rs2_option_rect;

/** \brief The value of an option, in a known option type.
*/
typedef struct rs2_option_value
{
rs2_option id;
int is_valid; /**< 0 if no value available; 1 otherwise */
rs2_option_type type;
union {
#pragma pack(push,1)
union
{
char const * as_string; /**< valid only while rs2_option_value is alive! */
float as_float;
int64_t as_integer; /**< including boolean value */
rs2_option_rect as_rect;
};
#pragma pack(pop)
} rs2_option_value;

/** \brief For SR300 devices: provides optimized settings (presets) for specific types of usage. */
Expand Down
63 changes: 62 additions & 1 deletion src/dds/rs-dds-option.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.

#include "rs-dds-option.h"
#include <src/core/enum-helpers.h>

#include <realdds/dds-option.h>
#include <rsutils/json.h>
Expand Down Expand Up @@ -44,6 +45,8 @@ static rs2_option_type rs_type_from_dds_option( std::shared_ptr< realdds::dds_op
return RS2_OPTION_TYPE_BOOLEAN;
if( std::dynamic_pointer_cast< realdds::dds_integer_option >( dds_opt ) )
return RS2_OPTION_TYPE_INTEGER;
if( std::dynamic_pointer_cast< realdds::dds_rect_option >( dds_opt ) )
return RS2_OPTION_TYPE_RECT;
throw not_implemented_exception( "unknown DDS option type" );
}

Expand All @@ -65,7 +68,65 @@ void rs_dds_option::set( float value )
if( ! _set_opt_cb )
throw std::runtime_error( "Set option callback is not set for option " + _dds_opt->get_name() );

_set_opt_cb( value );
// This is the legacy API for setting option values - it accepts a float. It's not always called via the rs2_...
// APIs, but can be called from within librealsense, so we have to convert to the proper type:
// (usually the range checks are only done at the level of rs2_...)

auto validate_range = []( float const value, option_range const & range )
{
if( range.min != range.max && range.step )
{
if( value < range.min )
{
throw librealsense::invalid_value_exception(
rsutils::string::from() << "value (" << value << ") less than minimum (" << range.min << ")" );
}
if( value > range.max )
{
throw librealsense::invalid_value_exception(
rsutils::string::from() << "value (" << value << ") greater than maximum (" << range.max << ")" );
}
}
};

json j_value;
switch( _rs_type )
{
case RS2_OPTION_TYPE_FLOAT:
validate_range( value, get_range() );
j_value = value;
break;

case RS2_OPTION_TYPE_INTEGER:
validate_range( value, get_range() );
if( (int) value != value )
throw invalid_value_exception( rsutils::string::from() << "not an integer: " << value );
j_value = int( value );
break;

case RS2_OPTION_TYPE_BOOLEAN:
if( value == 0.f )
j_value = false;
else if( value == 1.f )
j_value = true;
else
throw invalid_value_exception( rsutils::string::from() << "not a boolean: " << value );
break;

case RS2_OPTION_TYPE_STRING:
// We can convert "enum" options to a float value
if( auto desc = get_value_description( value ) )
j_value = desc;
else
throw not_implemented_exception( "use rs2_set_option_value to set string values" );
break;

default:
throw not_implemented_exception( rsutils::string::from()
<< "use rs2_set_option_value to set " << get_string( _rs_type ) << " value" );
}

_set_opt_cb( j_value );
}


Expand Down
53 changes: 53 additions & 0 deletions src/dds/rs-dds-sensor-proxy.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <src/core/options-registry.h>
#include <src/core/frame-callback.h>
#include <src/core/roi.h>
#include <src/stream.h>

#include <src/proc/color-formats-converter.h>
Expand Down Expand Up @@ -44,6 +45,22 @@ dds_sensor_proxy::dds_sensor_proxy( std::string const & sensor_name,
}


bool dds_sensor_proxy::extend_to( rs2_extension extension_type, void ** ptr )
{
if( extension_type == RS2_EXTENSION_ROI )
{
// We do not extend roi_sensor_interface, as our support is enabled only if there's an option with the specific
// type! Instead, we expose through extend_to() only if such an option is found. See add_option().
if( _roi_support )
{
*ptr = _roi_support.get();
return true;
}
}
return super::extend_to( extension_type, ptr );
}


void dds_sensor_proxy::add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream )
{
auto & s = _streams[sidx];
Expand Down Expand Up @@ -529,6 +546,32 @@ void dds_sensor_proxy::stop()
}


class dds_option_roi_method : public region_of_interest_method
{
std::shared_ptr< rs_dds_option > _rs_option;

public:
dds_option_roi_method( std::shared_ptr< rs_dds_option > const & rs_option )
: _rs_option( rs_option )
{
}

void set( const region_of_interest & roi ) override
{
_rs_option->set_value( json::array( { roi.min_x, roi.min_y, roi.max_x, roi.max_y } ) );
}

region_of_interest get() const override
{
auto j = _rs_option->get_value();
if( ! j.is_array() )
throw std::runtime_error( "no ROI available" );
region_of_interest roi{ j[0], j[1], j[2], j[3] };
return roi;
}
};


void dds_sensor_proxy::add_option( std::shared_ptr< realdds::dds_option > option )
{
bool const ok_if_there = true;
Expand Down Expand Up @@ -564,6 +607,16 @@ void dds_sensor_proxy::add_option( std::shared_ptr< realdds::dds_option > option
} );
register_option( option_id, opt );
_options_watcher.register_option( option_id, opt );

if( std::dynamic_pointer_cast< realdds::dds_rect_option >( option ) && option->get_name() == "Region of Interest" )
{
if( _roi_support )
throw std::runtime_error( "more than one ROI option in stream" );

auto roi = std::make_shared< roi_sensor_base >();
roi->set_roi_method( std::make_shared< dds_option_roi_method >( opt ) );
_roi_support = roi;
}
}


Expand Down
5 changes: 5 additions & 0 deletions src/dds/rs-dds-sensor-proxy.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ namespace librealsense {


class dds_device_proxy;
class roi_sensor_interface;


class dds_sensor_proxy : public software_sensor
Expand All @@ -46,6 +47,8 @@ class dds_sensor_proxy : public software_sensor
typedef realdds::dds_metadata_syncer syncer_type;
static void frame_releaser( syncer_type::frame_type * f ) { static_cast< frame * >( f )->release(); }

std::shared_ptr< roi_sensor_interface > _roi_support;

protected:
struct streaming_impl
{
Expand All @@ -64,6 +67,8 @@ class dds_sensor_proxy : public software_sensor
software_device * owner,
std::shared_ptr< realdds::dds_device > const & dev );

bool extend_to( rs2_extension, void ** ptr ) override; // extendable_interface

const std::string & get_name() const { return _name; }

void add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream );
Expand Down
Loading

0 comments on commit c17054a

Please sign in to comment.