Skip to content

Commit

Permalink
add options-changed notifications to dds-adapter
Browse files Browse the repository at this point in the history
  • Loading branch information
@maloel
maloel committed Feb 8, 2024
1 parent eee76cd commit 8da22c2
Show file tree
Hide file tree
Showing 2 changed files with 111 additions and 29 deletions.
9 changes: 6 additions & 3 deletions third-party/realdds/include/realdds/dds-device-server.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
#pragma once

#include <realdds/dds-option.h>
Expand Down Expand Up @@ -101,6 +101,11 @@ class dds_device_server
typedef std::function< bool( std::string const &, rsutils::json const &, rsutils::json & ) > control_callback;
void on_control( control_callback callback ) { _control_callback = std::move( callback ); }

// Locate an option based on stream name (empty for device option) and option name
std::shared_ptr< dds_option > find_option( std::string const & option_name, std::string const & stream_name ) const;
// Same as find_options, except throws if not found
std::shared_ptr< dds_option > get_option( std::string const & option_name, std::string const & stream_name ) const;

private:
struct control_sample;

Expand All @@ -109,8 +114,6 @@ class dds_device_server
void on_query_option( control_sample const &, rsutils::json & reply );
void on_query_options( control_sample const &, rsutils::json & reply );

std::shared_ptr< dds_option > find_option( const std::string & option_name, const std::string & stream_name ) const;
std::shared_ptr< dds_option > get_option( const std::string & option_name, const std::string & stream_name ) const;
float query_option( std::shared_ptr< dds_option > const & ) const;

std::shared_ptr< dds_publisher > _publisher;
Expand Down
131 changes: 105 additions & 26 deletions tools/dds/dds-adapter/lrs-device-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ using rsutils::string::hexarray;
#include <realdds/topics/imu-msg.h>
#include <realdds/topics/ros2/ros2vector3.h>
#include <realdds/topics/flexible-msg.h>
#include <realdds/topics/dds-topic-names.h>
#include <realdds/dds-device-server.h>
#include <realdds/dds-stream-server.h>

Expand Down Expand Up @@ -110,6 +111,21 @@ static std::string stream_name_from_rs2( const rs2::stream_profile & profile )
}


static std::string stream_name_from_rs2( rs2::sensor const & sensor )
{
static std::map< std::string, std::string > sensor_stream_name{
{ "Stereo Module", rs2_stream_to_string( RS2_STREAM_DEPTH ) },
{ "RGB Camera", rs2_stream_to_string( RS2_STREAM_COLOR ) },
{ "Motion Module", "Motion" },
};

auto it = sensor_stream_name.find( sensor.get_info( RS2_CAMERA_INFO_NAME ) );
if( it == sensor_stream_name.end() )
return {};
return it->second;
}


std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controller::get_supported_streams()
{
std::map< std::string, realdds::dds_stream_profiles > stream_name_to_profiles;
Expand Down Expand Up @@ -613,6 +629,70 @@ lrs_device_controller::lrs_device_controller( rs2::device dev, std::shared_ptr<

// Initialize the DDS device server with the supported streams
_dds_device_server->init( supported_streams, options, extrinsics );

for( auto & name_sensor : _rs_sensors )
{
auto & sensor = name_sensor.second;
sensor.on_options_changed(
[this, weak_sensor = std::weak_ptr< rs2_sensor >( sensor.get() )]( rs2::options_list const & options )
{
if( auto strong_sensor = weak_sensor.lock() )
{
rs2::sensor sensor( strong_sensor );
json option_values = json::object();
for( auto changed_option : options )
{
std::string const option_name = sensor.get_option_name( changed_option->id );
std::string const stream_name = stream_name_from_rs2( sensor );
if( stream_name.empty() )
{
LOG_ERROR( "Unknown option '" << option_name
<< "' stream: " << sensor.get_info( RS2_CAMERA_INFO_NAME ) );
continue;
}
auto dds_option = _dds_device_server->find_option( option_name, stream_name );
if( ! dds_option )
{
LOG_ERROR( "Missing option '" << option_name
<< "' stream: " << sensor.get_info( RS2_CAMERA_INFO_NAME ) );
continue;
}
json value;
switch( changed_option->type )
{
case RS2_OPTION_TYPE_FLOAT:
value = changed_option->as_float;
break;
case RS2_OPTION_TYPE_STRING:
value = changed_option->as_string;
break;
case RS2_OPTION_TYPE_NUMBER:
value = changed_option->as_number_signed;
break;
case RS2_OPTION_TYPE_COUNT:
// No value available
value = rsutils::null_json;
break;
default:
LOG_ERROR( "Unknown option '" << option_name << "' type: "
<< rs2_option_type_to_string( changed_option->type ) );
continue;
}
dds_option->set_value( std::move( value ) );
option_values[stream_name][option_name] = dds_option->get_value();
}
if( option_values.size() )
{
json j = json::object( {
{ realdds::topics::notification::key::id, realdds::topics::reply::query_options::id },
{ realdds::topics::reply::query_options::key::option_values, std::move( option_values ) },
} );
LOG_DEBUG( "[" << _dds_device_server->debug_name() << "] options changed: " << j.dump( 4 ) );
_dds_device_server->publish_notification( std::move( j ) );
}
}
} );
}
}


Expand All @@ -630,10 +710,10 @@ bool lrs_device_controller::on_open_streams( json const & control, json & reply
// out, a sensor is reset back to its default state using implicit stream selection.
// (For example, the 'Stereo Module' sensor controls Depth, IR1, IR2: but turning on all 3 has performance
// implications and may not be desirable. So you can open only Depth and IR1/2 will stay inactive...)
if( control.nested( "reset" ).default_value( true ) )
if( control.nested( topics::control::open_streams::key::reset ).default_value( true ) )
_bridge.reset();

auto const & msg_profiles = control["stream-profiles"];
auto const & msg_profiles = control[topics::control::open_streams::key::stream_profiles];
for( auto const & name2profile : msg_profiles.items() )
{
std::string const & stream_name = name2profile.key();
Expand All @@ -653,7 +733,7 @@ bool lrs_device_controller::on_open_streams( json const & control, json & reply
}

// We're here so all the profiles were acceptable; lock them in -- with no implicit profiles!
if( control.nested( "commit" ).default_value( true ) )
if( control.nested( topics::control::open_streams::key::commit ).default_value( true ) )
_bridge.commit();

// We don't touch the reply - it's already filled in for us
Expand All @@ -667,12 +747,12 @@ void lrs_device_controller::publish_frame_metadata( const rs2::frame & f, realdd
return;

json md_header = json::object( {
{ "frame-number", f.get_frame_number() }, // communicated; up to client to pick up
{ "timestamp", timestamp.to_ns() }, // syncer key: needs to match the image timestamp, bit-for-bit!
{ "timestamp-domain", f.get_frame_timestamp_domain() } // needed if we're dealing with different domains!
{ topics::metadata::header::key::frame_number, f.get_frame_number() }, // communicated; up to client to pick up
{ topics::metadata::header::key::timestamp, timestamp.to_ns() }, // syncer key: needs to match the image timestamp, bit-for-bit!
{ topics::metadata::header::key::timestamp_domain, f.get_frame_timestamp_domain() } // needed if we're dealing with different domains!
} );
if( f.is< rs2::depth_frame >() )
md_header["depth-units"] = f.as< rs2::depth_frame >().get_units();
md_header[topics::metadata::header::key::depth_units] = f.as< rs2::depth_frame >().get_units();

json metadata = json::object();
for( size_t i = 0; i < static_cast< size_t >( RS2_FRAME_METADATA_COUNT ); ++i )
Expand All @@ -683,9 +763,9 @@ void lrs_device_controller::publish_frame_metadata( const rs2::frame & f, realdd
}

json md_msg = json::object( {
{ "stream-name", stream_name_from_rs2( f.get_profile() ) },
{ "header", std::move( md_header ) },
{ "metadata", std::move( metadata ) },
{ topics::metadata::key::stream_name, stream_name_from_rs2( f.get_profile() ) },
{ topics::metadata::key::header, std::move( md_header ) },
{ topics::metadata::key::metadata, std::move( metadata ) },
} );
_dds_device_server->publish_metadata( std::move( md_msg ) );
}
Expand Down Expand Up @@ -840,12 +920,11 @@ size_t lrs_device_controller::get_index_of_profile( const realdds::dds_stream_pr

bool lrs_device_controller::on_control( std::string const & id, json const & control, json & reply )
{
static std::map< std::string, bool ( lrs_device_controller::* )( json const &, json & ) > const
control_handlers{
{ "hw-reset", &lrs_device_controller::on_hardware_reset },
{ "open-streams", &lrs_device_controller::on_open_streams },
{ "hwm", &lrs_device_controller::on_hwm },
};
static std::map< std::string, bool ( lrs_device_controller::* )( json const &, json & ) > const control_handlers{
{ realdds::topics::control::hw_reset::id, &lrs_device_controller::on_hardware_reset },
{ realdds::topics::control::open_streams::id, &lrs_device_controller::on_open_streams },
{ realdds::topics::control::hwm::id, &lrs_device_controller::on_hwm },
};
auto it = control_handlers.find( id );
if( it == control_handlers.end() )
return false;
Expand All @@ -870,35 +949,35 @@ bool lrs_device_controller::on_hwm( json const & control, json & reply )
rsutils::string::hexarray data;

uint32_t opcode;
if( control.nested( "opcode" ).get_ex( opcode ) )
if( control.nested( topics::control::hwm::key::opcode ).get_ex( opcode ) )
{
// In the presence of 'opcode', we're asked to build the command using optional parameters
uint32_t param1 = 0, param2 = 0, param3 = 0, param4 = 0;
control.nested( "param1" ).get_ex( param1 );
control.nested( "param2" ).get_ex( param2 );
control.nested( "param3" ).get_ex( param3 );
control.nested( "param4" ).get_ex( param4 );
control.nested( topics::control::hwm::key::param1 ).get_ex( param1 );
control.nested( topics::control::hwm::key::param2 ).get_ex( param2 );
control.nested( topics::control::hwm::key::param3 ).get_ex( param3 );
control.nested( topics::control::hwm::key::param4 ).get_ex( param4 );

control.nested( "data" ).get_ex( data ); // optional
control.nested( topics::control::hwm::key::data ).get_ex( data ); // optional

// Build the HWM command
data = dp.build_command( opcode, param1, param2, param3, param4, data.get_bytes() );

// And, if told to not actually run it, we return the HWM command
if( control.nested( "build-command" ).default_value( false ) )
if( control.nested( topics::control::hwm::key::build_command ).default_value( false ) )
{
reply["data"] = data;
reply[topics::reply::hwm::key::data] = data;
return true;
}
}
else
{
if( ! control.nested( "data" ).get_ex( data ) )
if( ! control.nested( topics::control::hwm::key::data ).get_ex( data ) )
throw std::runtime_error( "no 'data' in HWM control" );
}

data = dp.send_and_receive_raw_data( data.get_bytes() );
reply["data"] = data;
reply[topics::reply::hwm::key::data] = data;
return true;
}

Expand Down

0 comments on commit 8da22c2

Please sign in to comment.