You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1364 lines
59 KiB
1364 lines
59 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
|
|
|
|
#include "lrs-device-controller.h"
|
|
|
|
#include <common/metadata-helper.h>
|
|
|
|
#include <realdds/topics/image-msg.h>
|
|
#include <realdds/topics/imu-msg.h>
|
|
#include <realdds/topics/blob-msg.h>
|
|
#include <realdds/topics/dds-topic-names.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>
|
|
#include <realdds/dds-topic-reader-thread.h>
|
|
#include <realdds/dds-participant.h>
|
|
#include <realdds/dds-guid.h>
|
|
|
|
#include <fastdds/dds/subscriber/SampleInfo.hpp>
|
|
|
|
#include <rsutils/number/crc32.h>
|
|
#include <rsutils/easylogging/easyloggingpp.h>
|
|
#include <rsutils/json.h>
|
|
#include <rsutils/string/hexarray.h>
|
|
#include <rsutils/time/timer.h>
|
|
#include <rsutils/string/from.h>
|
|
|
|
#include <algorithm>
|
|
#include <iostream>
|
|
|
|
using rsutils::string::hexarray;
|
|
using rsutils::json;
|
|
using namespace realdds;
|
|
using tools::lrs_device_controller;
|
|
|
|
|
|
#define CREATE_SERVER_IF_NEEDED( X ) \
|
|
if( server ) \
|
|
{ \
|
|
if( strcmp( server->type_string(), #X ) ) \
|
|
{ \
|
|
LOG_ERROR( #X " profile type on a stream '" << stream_name << "' that already has type " \
|
|
<< server->type_string() ); \
|
|
return; \
|
|
} \
|
|
} \
|
|
else \
|
|
{ \
|
|
server = std::make_shared< realdds::dds_##X##_stream_server >( stream_name, sensor_name ); \
|
|
} \
|
|
break
|
|
|
|
|
|
realdds::video_intrinsics to_realdds( const rs2_intrinsics & intr )
|
|
{
|
|
realdds::video_intrinsics ret;
|
|
|
|
ret.width = intr.width;
|
|
ret.height = intr.height;
|
|
ret.principal_point_x = intr.ppx;
|
|
ret.principal_point_y = intr.ppy;
|
|
ret.focal_lenght_x = intr.fx;
|
|
ret.focal_lenght_y = intr.fy;
|
|
ret.distortion_model = intr.model;
|
|
memcpy( ret.distortion_coeffs.data(), intr.coeffs, sizeof( ret.distortion_coeffs ) );
|
|
|
|
return ret;
|
|
}
|
|
|
|
realdds::motion_intrinsics to_realdds( const rs2_motion_device_intrinsic & rs2_intr )
|
|
{
|
|
realdds::motion_intrinsics intr;
|
|
|
|
memcpy( intr.data.data(), rs2_intr.data, sizeof( intr.data ) );
|
|
memcpy( intr.noise_variances.data(), rs2_intr.noise_variances, sizeof( intr.noise_variances ) );
|
|
memcpy( intr.bias_variances.data(), rs2_intr.bias_variances, sizeof( intr.bias_variances ) );
|
|
|
|
return intr;
|
|
}
|
|
|
|
realdds::extrinsics to_realdds( const rs2_extrinsics & rs2_extr )
|
|
{
|
|
realdds::extrinsics extr;
|
|
|
|
memcpy( extr.rotation.data(), rs2_extr.rotation, sizeof( extr.rotation ) );
|
|
memcpy( extr.translation.data(), rs2_extr.translation, sizeof( extr.translation ) );
|
|
|
|
return extr;
|
|
}
|
|
|
|
|
|
static std::string stream_name_from_rs2( const rs2::stream_profile & profile )
|
|
{
|
|
// ROS stream names cannot contain spaces! We use underscores instead:
|
|
std::string stream_name;
|
|
switch( profile.stream_type() )
|
|
{
|
|
case RS2_STREAM_ACCEL:
|
|
case RS2_STREAM_GYRO:
|
|
stream_name = "Motion";
|
|
break;
|
|
default:
|
|
stream_name = rs2_stream_to_string( profile.stream_type() );
|
|
if( profile.stream_index() )
|
|
stream_name += '_' + std::to_string( profile.stream_index() );
|
|
break;
|
|
}
|
|
return stream_name;
|
|
}
|
|
|
|
|
|
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;
|
|
std::map< std::string, size_t > stream_name_to_default_profile;
|
|
std::map< std::string, std::set< realdds::video_intrinsics > > stream_name_to_video_intrinsics;
|
|
|
|
// Iterate over all profiles of all sensors and build appropriate dds_stream_servers
|
|
for( auto & sensor : _rs_dev.query_sensors() )
|
|
{
|
|
std::string const sensor_name = sensor.get_info( RS2_CAMERA_INFO_NAME );
|
|
// We keep a copy of the sensors throughout the run time:
|
|
// Otherwise problems could arise like opening streams and they would close at on_open_streams scope end.
|
|
_rs_sensors[sensor_name] = sensor;
|
|
|
|
auto stream_profiles = sensor.get_stream_profiles();
|
|
std::for_each( stream_profiles.begin(), stream_profiles.end(), [&]( const rs2::stream_profile & sp )
|
|
{
|
|
std::string stream_name = stream_name_from_rs2( sp );
|
|
|
|
//Create a realdds::dds_stream_server object for each unique profile type+index
|
|
auto & server = _stream_name_to_server[stream_name];
|
|
switch( sp.stream_type() )
|
|
{
|
|
case RS2_STREAM_DEPTH: CREATE_SERVER_IF_NEEDED( depth );
|
|
case RS2_STREAM_INFRARED: CREATE_SERVER_IF_NEEDED( ir );
|
|
case RS2_STREAM_COLOR: CREATE_SERVER_IF_NEEDED( color );
|
|
case RS2_STREAM_CONFIDENCE: CREATE_SERVER_IF_NEEDED( confidence );
|
|
case RS2_STREAM_ACCEL:
|
|
case RS2_STREAM_GYRO:
|
|
CREATE_SERVER_IF_NEEDED( motion );
|
|
default:
|
|
LOG_ERROR( "unsupported stream type " << sp.stream_type() );
|
|
return;
|
|
}
|
|
|
|
// Create appropriate realdds::profile for each sensor profile and map to a stream
|
|
auto & profiles = stream_name_to_profiles[stream_name];
|
|
std::shared_ptr< realdds::dds_stream_profile > profile;
|
|
bool insert_profile = true;
|
|
if( auto const vsp = rs2::video_stream_profile( sp ) )
|
|
{
|
|
profile = std::make_shared< realdds::dds_video_stream_profile >(
|
|
static_cast< int16_t >( vsp.fps() ),
|
|
realdds::dds_video_encoding::from_rs2( vsp.format() ),
|
|
static_cast< uint16_t >( vsp.width() ),
|
|
static_cast< int16_t >( vsp.height() ) );
|
|
try
|
|
{
|
|
auto intr = to_realdds( vsp.get_intrinsics() );
|
|
stream_name_to_video_intrinsics[stream_name].insert( intr );
|
|
}
|
|
catch( ... ) {} //Some profiles don't have intrinsics
|
|
}
|
|
else if( auto const msp = rs2::motion_stream_profile( sp ) )
|
|
{
|
|
profile = std::make_shared< realdds::dds_motion_stream_profile >(
|
|
static_cast< int16_t >( msp.fps() ) );
|
|
|
|
auto motion_server = std::dynamic_pointer_cast< dds_motion_stream_server >( server );
|
|
if( RS2_STREAM_ACCEL == msp.stream_type() )
|
|
{
|
|
insert_profile = false; // We report only Gyro profiles
|
|
motion_server->set_accel_intrinsics( to_realdds( msp.get_motion_intrinsics() ) );
|
|
}
|
|
else
|
|
motion_server->set_gyro_intrinsics( to_realdds( msp.get_motion_intrinsics() ) );
|
|
}
|
|
else
|
|
{
|
|
LOG_ERROR( "unknown profile type of uid " << sp.unique_id() );
|
|
return;
|
|
}
|
|
if( insert_profile )
|
|
{
|
|
if( sp.is_default() )
|
|
stream_name_to_default_profile[stream_name] = profiles.size();
|
|
profiles.push_back( profile );
|
|
LOG_DEBUG( stream_name << ": " << profile->to_string() );
|
|
}
|
|
} );
|
|
}
|
|
|
|
override_default_profiles( stream_name_to_profiles, stream_name_to_default_profile );
|
|
|
|
// Iterate over the mapped streams and initialize
|
|
std::vector< std::shared_ptr< realdds::dds_stream_server > > servers;
|
|
std::set< std::string > sensors_handled;
|
|
for( auto & it : stream_name_to_profiles )
|
|
{
|
|
auto const & stream_name = it.first;
|
|
|
|
size_t default_profile_index = 0;
|
|
auto default_profile_it = stream_name_to_default_profile.find( stream_name );
|
|
if( default_profile_it != stream_name_to_default_profile.end() )
|
|
default_profile_index = default_profile_it->second;
|
|
else
|
|
LOG_ERROR( "no default profile found; using first available in " << stream_name );
|
|
|
|
auto const & profiles = it.second;
|
|
if( profiles.empty() )
|
|
{
|
|
LOG_ERROR( "ignoring stream '" << stream_name << "' with no profiles" );
|
|
continue;
|
|
}
|
|
auto server = _stream_name_to_server[stream_name];
|
|
if( ! server )
|
|
{
|
|
LOG_ERROR( "ignoring stream '" << stream_name << "' with no server" );
|
|
continue;
|
|
}
|
|
|
|
if( auto video_server = std::dynamic_pointer_cast< dds_video_stream_server >( server ) )
|
|
{
|
|
video_server->set_intrinsics( std::move( stream_name_to_video_intrinsics[stream_name] ) );
|
|
// Set stream metadata support (currently if the device supports metadata all streams does)
|
|
// Must be done before calling init_profiles()
|
|
if( _md_enabled )
|
|
server->enable_metadata();
|
|
}
|
|
|
|
server->init_profiles( profiles, default_profile_index );
|
|
|
|
// Get supported options and recommended filters for this stream
|
|
for( auto & sensor : _rs_dev.query_sensors() )
|
|
{
|
|
std::string const sensor_name = sensor.get_info( RS2_CAMERA_INFO_NAME );
|
|
if( server->sensor_name().compare( sensor_name ) != 0 )
|
|
continue;
|
|
|
|
// Multiple streams map to the same sensor so we may go over the same sensor multiple times - we
|
|
// only need to do this once per sensor!
|
|
if( sensors_handled.emplace( sensor_name ).second )
|
|
{
|
|
realdds::dds_options stream_options;
|
|
auto supported_options = sensor.get_supported_options();
|
|
for( auto option_id : supported_options )
|
|
{
|
|
// Certain options are automatically added by librealsense and shouldn't actually be shared
|
|
if( option_id == RS2_OPTION_FRAMES_QUEUE_SIZE )
|
|
continue; // Added automatically for every sensor_base
|
|
|
|
std::string option_name = sensor.get_option_name( option_id );
|
|
try
|
|
{
|
|
json j = json::array();
|
|
json props = json::array();
|
|
j += option_name;
|
|
json option_value; // null - no value
|
|
try
|
|
{
|
|
option_value = sensor.get_option( option_id );
|
|
}
|
|
catch( ... )
|
|
{
|
|
// Some options can be queried only if certain conditions exist skip them for now
|
|
props += "optional";
|
|
}
|
|
j += option_value;
|
|
{
|
|
// Even read-only options have ranges in librealsense
|
|
auto const range = sensor.get_option_range( option_id );
|
|
j += range.min;
|
|
j += range.max;
|
|
j += range.step;
|
|
j += range.def;
|
|
}
|
|
j += sensor.get_option_description( option_id );
|
|
if( sensor.is_option_read_only( option_id ) )
|
|
props += "read-only";
|
|
if( ! props.empty() )
|
|
j += props;
|
|
auto dds_opt = realdds::dds_option::from_json( j );
|
|
LOG_DEBUG( "... option -> " << j << " -> " << dds_opt->to_json() );
|
|
stream_options.push_back( dds_opt ); // TODO - filter options relevant for stream type
|
|
}
|
|
catch( ... )
|
|
{
|
|
LOG_ERROR( "Cannot query details of option " << option_id );
|
|
// Some options can be queried only if certain conditions exist skip them for now
|
|
}
|
|
}
|
|
|
|
auto recommended_filters = sensor.get_recommended_filters();
|
|
std::vector< std::string > filter_names;
|
|
for( auto const & filter : recommended_filters )
|
|
filter_names.push_back( filter.get_info( RS2_CAMERA_INFO_NAME ) );
|
|
|
|
server->init_options( stream_options );
|
|
server->set_recommended_filters( std::move( filter_names ) );
|
|
}
|
|
}
|
|
|
|
servers.push_back( server );
|
|
}
|
|
|
|
return servers;
|
|
}
|
|
|
|
#undef CREATE_SERVER_IF_NEEDED
|
|
|
|
extrinsics_map get_extrinsics_map( const rs2::device & dev )
|
|
{
|
|
extrinsics_map ret;
|
|
std::map< std::string, rs2::stream_profile > stream_name_to_rs2_stream_profile;
|
|
|
|
// Iterate over profiles of all sensors and split to streams
|
|
for( auto & sensor : dev.query_sensors() )
|
|
{
|
|
auto stream_profiles = sensor.get_stream_profiles();
|
|
std::for_each( stream_profiles.begin(),
|
|
stream_profiles.end(),
|
|
[&]( const rs2::stream_profile & sp )
|
|
{
|
|
if( RS2_STREAM_ACCEL == sp.stream_type() )
|
|
return; // Ignore the accelerometer; we want the gyro extrinsics!
|
|
std::string stream_name = stream_name_from_rs2( sp );
|
|
auto & rs2_stream_profile = stream_name_to_rs2_stream_profile[stream_name];
|
|
if( ! rs2_stream_profile )
|
|
rs2_stream_profile = sp; // Any profile of this stream will do, take the first
|
|
} );
|
|
}
|
|
|
|
// For each stream, get extrinsics to all other streams
|
|
for( auto & from : stream_name_to_rs2_stream_profile )
|
|
{
|
|
auto const & from_stream_name = from.first;
|
|
for( auto & to : stream_name_to_rs2_stream_profile )
|
|
{
|
|
auto & to_stream_name = to.first;
|
|
if( from_stream_name != to_stream_name )
|
|
{
|
|
// Get rs2::stream_profile objects for get_extrinsics API call
|
|
const rs2::stream_profile & from_profile = from.second;
|
|
const rs2::stream_profile & to_profile = to.second;
|
|
const auto & extrinsics = from_profile.get_extrinsics_to( to_profile );
|
|
ret[std::make_pair( from_stream_name, to_stream_name )] =
|
|
std::make_shared< realdds::extrinsics >( to_realdds( extrinsics ) );
|
|
LOG_DEBUG( "have extrinsics from " << from_stream_name << " to " << to_stream_name );
|
|
}
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
std::shared_ptr< dds_stream_profile > create_dds_stream_profile( std::string const & type_string, json const & j )
|
|
{
|
|
if( "motion" == type_string )
|
|
return dds_stream_profile::from_json< dds_motion_stream_profile >( j );
|
|
|
|
static const std::set< std::string > video_types = { "depth", "color", "ir", "confidence" };
|
|
if( video_types.find( type_string ) != video_types.end() )
|
|
return dds_stream_profile::from_json< dds_video_stream_profile >( j );
|
|
|
|
throw std::runtime_error( "unsupported stream type '" + type_string + "'" );
|
|
}
|
|
|
|
|
|
rs2_stream stream_name_to_type( std::string const & type_string )
|
|
{
|
|
static const std::map< std::string, rs2_stream > type_to_rs2 = {
|
|
{ "Depth", RS2_STREAM_DEPTH },
|
|
{ "Color", RS2_STREAM_COLOR },
|
|
{ "Infrared", RS2_STREAM_INFRARED },
|
|
{ "Infrared_1", RS2_STREAM_INFRARED },
|
|
{ "Infrared_2", RS2_STREAM_INFRARED },
|
|
{ "Motion", RS2_STREAM_GYRO }, // We report only gyro profiles
|
|
{ "Gpio", RS2_STREAM_GPIO },
|
|
{ "Confidence", RS2_STREAM_CONFIDENCE },
|
|
};
|
|
auto it = type_to_rs2.find( type_string );
|
|
if( it == type_to_rs2.end() )
|
|
{
|
|
LOG_ERROR( "Unknown stream type '" << type_string << "'" );
|
|
return RS2_STREAM_ANY;
|
|
}
|
|
return it->second;
|
|
}
|
|
|
|
|
|
int stream_name_to_index( std::string const & type_string )
|
|
{
|
|
int index = 0;
|
|
static const std::map< std::string, int > type_to_index = {
|
|
{ "Infrared_1", 1 },
|
|
{ "Infrared_2", 2 },
|
|
};
|
|
auto it = type_to_index.find( type_string );
|
|
if( it != type_to_index.end() )
|
|
{
|
|
index = it->second;
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
rs2_option option_name_to_id( const std::string & name )
|
|
{
|
|
auto option_id = rs2_option_from_string( name.c_str() );
|
|
if( RS2_OPTION_COUNT == option_id )
|
|
throw std::runtime_error( "Option '" + name + "' not found" );
|
|
return option_id;
|
|
}
|
|
|
|
|
|
bool profiles_are_compatible( std::shared_ptr< dds_stream_profile > const & p1,
|
|
std::shared_ptr< dds_stream_profile > const & p2,
|
|
bool any_encoding = false )
|
|
{
|
|
auto vp1 = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( p1 );
|
|
auto vp2 = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( p2 );
|
|
if( ! ! vp1 != ! ! vp2 )
|
|
return false; // types aren't the same
|
|
if( vp1 && vp2 )
|
|
{
|
|
if( vp1->width() != vp2->width() || vp1->height() != vp2->height() )
|
|
return false;
|
|
if( ! any_encoding && vp1->encoding() != vp2->encoding() )
|
|
return false;
|
|
}
|
|
return p1->frequency() == p2->frequency();
|
|
}
|
|
|
|
|
|
rs2::stream_profile get_required_profile( const rs2::sensor & sensor,
|
|
std::vector< rs2::stream_profile > const & sensor_stream_profiles,
|
|
std::string const & stream_name,
|
|
std::shared_ptr< dds_stream_profile > const & profile )
|
|
{
|
|
auto const stream_type = stream_name_to_type( stream_name );
|
|
auto const stream_index = stream_name_to_index( stream_name );
|
|
|
|
auto profile_iter = std::find_if( sensor_stream_profiles.begin(),
|
|
sensor_stream_profiles.end(),
|
|
[&]( rs2::stream_profile const & sp ) {
|
|
auto vp = sp.as< rs2::video_stream_profile >();
|
|
auto dds_vp = std::dynamic_pointer_cast< dds_video_stream_profile >( profile );
|
|
bool video_params_match = ( vp && dds_vp )
|
|
? vp.width() == dds_vp->width()
|
|
&& vp.height() == dds_vp->height()
|
|
&& vp.format() == dds_vp->encoding().to_rs2()
|
|
: true;
|
|
return sp.stream_type() == stream_type
|
|
&& sp.stream_index() == stream_index
|
|
&& sp.fps() == profile->frequency()
|
|
&& video_params_match;
|
|
} );
|
|
if( profile_iter == sensor_stream_profiles.end() )
|
|
{
|
|
throw std::runtime_error( "Could not find required profile" );
|
|
}
|
|
|
|
return *profile_iter;
|
|
}
|
|
|
|
|
|
static std::shared_ptr< realdds::dds_stream_profile >
|
|
find_profile( std::shared_ptr< realdds::dds_stream_server > const & stream,
|
|
std::shared_ptr< realdds::dds_stream_profile > const & profile,
|
|
bool any_encoding = false )
|
|
{
|
|
auto & stream_profiles = stream->profiles();
|
|
auto it = std::find_if( stream_profiles.begin(),
|
|
stream_profiles.end(),
|
|
[&]( std::shared_ptr< realdds::dds_stream_profile > const & sp )
|
|
{
|
|
return profiles_are_compatible( sp, profile, any_encoding );
|
|
} );
|
|
std::shared_ptr< realdds::dds_stream_profile > found_profile;
|
|
if( it != stream_profiles.end() )
|
|
found_profile = *it;
|
|
return found_profile;
|
|
}
|
|
|
|
|
|
std::shared_ptr< realdds::dds_stream_server >
|
|
lrs_device_controller::frame_to_streaming_server( rs2::frame const & f, rs2::stream_profile * p_profile ) const
|
|
{
|
|
rs2::stream_profile profile_;
|
|
if( ! p_profile )
|
|
p_profile = &profile_;
|
|
rs2::stream_profile & profile = *p_profile;
|
|
|
|
profile = f.get_profile();
|
|
auto const stream_name = stream_name_from_rs2( profile );
|
|
auto it = _stream_name_to_server.find( stream_name );
|
|
if( it == _stream_name_to_server.end() )
|
|
return {};
|
|
|
|
auto & server = it->second;
|
|
if( ! _bridge.is_streaming( server ) )
|
|
return {};
|
|
|
|
return server;
|
|
}
|
|
|
|
|
|
lrs_device_controller::lrs_device_controller( rs2::device dev, std::shared_ptr< realdds::dds_device_server > dds_device_server )
|
|
: _rs_dev( dev )
|
|
, _dds_device_server( dds_device_server )
|
|
{
|
|
if( ! _dds_device_server )
|
|
throw std::runtime_error( "Empty dds_device_server" );
|
|
|
|
_dds_device_server->on_set_option( [&]( const std::shared_ptr< realdds::dds_option > & option, json & value ) {
|
|
set_option( option, value );
|
|
} );
|
|
_dds_device_server->on_query_option( [&]( const std::shared_ptr< realdds::dds_option > & option ) -> json {
|
|
return query_option( option );
|
|
} );
|
|
_dds_device_server->on_control(
|
|
[this]( std::string const & id, json const & control, json & reply )
|
|
{ return on_control( id, control, reply ); } );
|
|
|
|
std::vector< std::shared_ptr< realdds::dds_stream_server > > supported_streams;
|
|
extrinsics_map extrinsics;
|
|
realdds::dds_options options;
|
|
|
|
if( is_recovery() )
|
|
{
|
|
_device_sn = _rs_dev.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID );
|
|
LOG_DEBUG( "LRS device manager for recovery device: " << _device_sn << " created" );
|
|
|
|
// Initialize with nothing: no streams, no options, empty extrinsics, etc.
|
|
_md_enabled = false;
|
|
_dds_device_server->init( supported_streams, options, extrinsics );
|
|
return;
|
|
}
|
|
|
|
_device_sn = _rs_dev.get_info( RS2_CAMERA_INFO_SERIAL_NUMBER );
|
|
LOG_DEBUG( "LRS device manager for device: " << _device_sn << " created" );
|
|
|
|
// Some camera models support metadata for frames. can_support_metadata will tell us if this model does.
|
|
// Also, to get the metadata driver support needs to be enabled, requires administrator rights on Windows and Linux.
|
|
// is_enabled will return current state. If one of the conditions is false we cannot get metadata from the device.
|
|
_md_enabled = rs2::metadata_helper::instance().can_support_metadata( _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_LINE ) )
|
|
&& rs2::metadata_helper::instance().is_enabled( _rs_dev.get_info( RS2_CAMERA_INFO_PHYSICAL_PORT ) );
|
|
|
|
// Create a supported streams list for initializing the relevant DDS topics
|
|
supported_streams = get_supported_streams();
|
|
|
|
_bridge.on_start_sensor(
|
|
[this]( std::string const & sensor_name, dds_stream_profiles const & active_profiles )
|
|
{
|
|
auto & sensor = _rs_sensors[sensor_name];
|
|
auto rs2_profiles = get_rs2_profiles( active_profiles );
|
|
sensor.open( rs2_profiles );
|
|
if( sensor.is< rs2::motion_sensor >() )
|
|
{
|
|
struct imu_context
|
|
{
|
|
realdds::topics::imu_msg message;
|
|
// We will be getting gyro and accel together, from different threads:
|
|
// Don't want to change something mid-send
|
|
std::mutex mutex;
|
|
};
|
|
sensor.start(
|
|
[this, imu = std::make_shared< imu_context >()]( rs2::frame f )
|
|
{
|
|
rs2::stream_profile stream_profile;
|
|
auto motion = std::dynamic_pointer_cast< realdds::dds_motion_stream_server >(
|
|
frame_to_streaming_server( f, &stream_profile ) );
|
|
if( ! motion )
|
|
return;
|
|
|
|
auto xyz = reinterpret_cast< float const * >( f.get_data() );
|
|
if( RS2_STREAM_ACCEL == stream_profile.stream_type() )
|
|
{
|
|
std::unique_lock< std::mutex > lock( imu->mutex );
|
|
imu->message.accel_data().x( xyz[0] ); // in m/s^2
|
|
imu->message.accel_data().y( xyz[1] );
|
|
imu->message.accel_data().z( xyz[2] );
|
|
return; // Don't actually publish
|
|
}
|
|
imu->message.gyro_data().x( xyz[0] ); // rad/sec, which is what we need
|
|
imu->message.gyro_data().y( xyz[1] );
|
|
imu->message.gyro_data().z( xyz[2] );
|
|
imu->message.timestamp( // in sec.nsec
|
|
static_cast< long double >( f.get_timestamp() ) / 1e3 );
|
|
std::unique_lock< std::mutex > lock( imu->mutex );
|
|
motion->publish_motion( std::move( imu->message ) );
|
|
|
|
// motion streams have no metadata!
|
|
} );
|
|
}
|
|
else
|
|
{
|
|
sensor.start(
|
|
[this]( rs2::frame f )
|
|
{
|
|
auto video = std::dynamic_pointer_cast< realdds::dds_video_stream_server >(
|
|
frame_to_streaming_server( f ) );
|
|
if( ! video )
|
|
return;
|
|
|
|
dds_time const timestamp // in sec.nsec
|
|
( static_cast< long double >( f.get_timestamp() ) / 1e3 );
|
|
|
|
realdds::topics::image_msg image;
|
|
auto data = static_cast< const uint8_t * >( f.get_data() );
|
|
image.raw_data.assign( data, data + f.get_data_size() );
|
|
image.height = video->get_image_header().height;
|
|
image.width = video->get_image_header().width;
|
|
image.timestamp = timestamp;
|
|
video->publish_image( std::move( image ) );
|
|
|
|
publish_frame_metadata( f, timestamp );
|
|
} );
|
|
}
|
|
std::cout << sensor_name << " sensor started" << std::endl;
|
|
} );
|
|
_bridge.on_stop_sensor(
|
|
[this]( std::string const & sensor_name )
|
|
{
|
|
auto & sensor = _rs_sensors[sensor_name];
|
|
sensor.stop();
|
|
sensor.close();
|
|
std::cout << sensor_name << " sensor stopped" << std::endl;
|
|
} );
|
|
_bridge.on_error(
|
|
[this]( std::string const & error_string )
|
|
{
|
|
json j = json::object( {
|
|
{ "id", "error" },
|
|
{ "error", error_string },
|
|
} );
|
|
_dds_device_server->publish_notification( std::move( j ) );
|
|
} );
|
|
_bridge.init( supported_streams );
|
|
|
|
extrinsics = get_extrinsics_map( dev );
|
|
|
|
// 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;
|
|
if( changed_option->is_valid )
|
|
{
|
|
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_INTEGER:
|
|
value = changed_option->as_integer;
|
|
break;
|
|
case RS2_OPTION_TYPE_BOOLEAN:
|
|
value = (bool)changed_option->as_integer;
|
|
break;
|
|
default:
|
|
LOG_ERROR( "Unknown option '" << option_name << "' type: "
|
|
<< rs2_option_type_to_string( changed_option->type ) );
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No value available
|
|
value = rsutils::null_json;
|
|
}
|
|
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 ) );
|
|
}
|
|
}
|
|
} );
|
|
}
|
|
}
|
|
|
|
|
|
lrs_device_controller::~lrs_device_controller()
|
|
{
|
|
LOG_DEBUG( "LRS device manager for device: " << _device_sn << " deleted" );
|
|
}
|
|
|
|
|
|
bool lrs_device_controller::is_recovery() const
|
|
{
|
|
auto update_device = rs2::update_device( _rs_dev );
|
|
return update_device;
|
|
}
|
|
|
|
|
|
bool lrs_device_controller::on_open_streams( json const & control, json & reply )
|
|
{
|
|
// Note that this function is called "start-streaming" but it's really a response to "open-streams" so does not
|
|
// actually start streaming. It simply sets and locks in which streams should be open when streaming starts.
|
|
// This effectively lets one control _specifically_ which streams should be streamable, and nothing else: if left
|
|
// 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( topics::control::open_streams::key::reset ).default_value( true ) )
|
|
_bridge.reset();
|
|
|
|
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();
|
|
auto name2server = _stream_name_to_server.find( stream_name );
|
|
if( name2server == _stream_name_to_server.end() )
|
|
throw std::runtime_error( "invalid stream name '" + stream_name + "'" );
|
|
auto server = name2server->second;
|
|
|
|
auto requested_profile
|
|
= create_dds_stream_profile( server->type_string(), name2profile.value() );
|
|
auto profile = find_profile( server, requested_profile );
|
|
if( ! profile )
|
|
throw std::runtime_error( "invalid profile " + requested_profile->to_string() + " for stream '"
|
|
+ stream_name + "'" );
|
|
|
|
_bridge.open( profile );
|
|
}
|
|
|
|
// We're here so all the profiles were acceptable; lock them in -- with no implicit profiles!
|
|
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
|
|
return true;
|
|
}
|
|
|
|
|
|
void lrs_device_controller::publish_frame_metadata( const rs2::frame & f, realdds::dds_time const & timestamp )
|
|
{
|
|
if( ! _dds_device_server->has_metadata_readers() )
|
|
return;
|
|
|
|
json md_header = json::object( {
|
|
{ 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[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 )
|
|
{
|
|
rs2_frame_metadata_value val = static_cast< rs2_frame_metadata_value >( i );
|
|
if( f.supports_frame_metadata( val ) )
|
|
metadata[rs2_frame_metadata_to_string( val )] = f.get_frame_metadata( val );
|
|
}
|
|
|
|
json md_msg = json::object( {
|
|
{ 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 ) );
|
|
}
|
|
|
|
|
|
std::vector< rs2::stream_profile >
|
|
lrs_device_controller::get_rs2_profiles( realdds::dds_stream_profiles const & dds_profiles ) const
|
|
{
|
|
std::vector< rs2::stream_profile > rs_profiles;
|
|
for( auto & dds_profile : dds_profiles )
|
|
{
|
|
std::string stream_name = dds_profile->stream()->name();
|
|
std::string sensor_name = dds_profile->stream()->sensor_name();
|
|
|
|
auto it = _rs_sensors.find( sensor_name );
|
|
if( it == _rs_sensors.end() )
|
|
{
|
|
LOG_ERROR( "invalid sensor name '" << sensor_name << "'" );
|
|
continue;
|
|
}
|
|
auto & sensor = it->second;
|
|
auto const sensor_stream_profiles = sensor.get_stream_profiles();
|
|
auto rs2_profile = get_required_profile( sensor, sensor_stream_profiles, stream_name, dds_profile );
|
|
rs_profiles.push_back( rs2_profile );
|
|
|
|
if( rs2_profile.stream_type() == RS2_STREAM_GYRO )
|
|
{
|
|
// When we start the Gyro, we want to start the Accelerometer, too!
|
|
rs2::stream_profile accel_profile;
|
|
assert( accel_profile.fps() == 0 );
|
|
for( auto & sp : sensor_stream_profiles )
|
|
{
|
|
if( sp.stream_type() != RS2_STREAM_ACCEL )
|
|
continue;
|
|
if( sp.format() != rs2_profile.format() )
|
|
continue;
|
|
if( sp.fps() > accel_profile.fps() )
|
|
accel_profile = sp;
|
|
}
|
|
if( accel_profile )
|
|
{
|
|
LOG_DEBUG( "adding accel profile @ " << accel_profile.fps() << " FPS" );
|
|
rs_profiles.push_back( accel_profile );
|
|
}
|
|
}
|
|
}
|
|
return rs_profiles;
|
|
}
|
|
|
|
|
|
void lrs_device_controller::set_option( const std::shared_ptr< realdds::dds_option > & option, float new_value )
|
|
{
|
|
auto stream = option->stream();
|
|
if( ! stream )
|
|
// TODO device option? not implemented yet
|
|
throw std::runtime_error( "device option not implemented" );
|
|
auto it = _stream_name_to_server.find( stream->name() );
|
|
if( it == _stream_name_to_server.end() )
|
|
throw std::runtime_error( "no stream '" + stream->name() + "' in device" );
|
|
auto server = it->second;
|
|
auto & sensor = _rs_sensors[server->sensor_name()];
|
|
sensor.set_option( option_name_to_id( option->get_name() ), new_value );
|
|
}
|
|
|
|
|
|
json lrs_device_controller::query_option( const std::shared_ptr< realdds::dds_option > & option )
|
|
{
|
|
auto stream = option->stream();
|
|
if( ! stream )
|
|
// TODO device option? not implemented yet
|
|
throw std::runtime_error( "device option not implemented" );
|
|
auto it = _stream_name_to_server.find( stream->name() );
|
|
if( it == _stream_name_to_server.end() )
|
|
throw std::runtime_error( "no stream '" + stream->name() + "' in device" );
|
|
auto server = it->second;
|
|
auto & sensor = _rs_sensors[server->sensor_name()];
|
|
try
|
|
{
|
|
return sensor.get_option( option_name_to_id( option->get_name() ) );
|
|
}
|
|
catch( rs2::invalid_value_error const & )
|
|
{
|
|
// It's possible for an option to no longer be supported (temperates do this) because we stopped streaming...
|
|
if( option->is_optional() )
|
|
return rsutils::null_json;
|
|
throw;
|
|
}
|
|
}
|
|
|
|
|
|
void lrs_device_controller::override_default_profiles( const std::map< std::string, realdds::dds_stream_profiles > & stream_name_to_profiles,
|
|
std::map< std::string, size_t > & stream_name_to_default_profile ) const
|
|
{
|
|
// Default resolution for RealSense modules, set according to system SW architect definitions
|
|
std::string const product_line = _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_LINE );
|
|
if( product_line == "D400" )
|
|
{
|
|
static const std::string RS405_PID( "0B5B", 4 );
|
|
static const std::string RS410_PID( "0AD2", 4 );
|
|
static const std::string RS415_PID( "0AD3", 4 );
|
|
|
|
std::string product_id = _rs_dev.get_info( RS2_CAMERA_INFO_PRODUCT_ID );
|
|
|
|
// For best image quality global shutter should use 848x480 resolution, rolling shutter 1280x720
|
|
uint16_t fps = 30;
|
|
uint16_t width = 848;
|
|
uint16_t height = 480;
|
|
if( product_id == RS415_PID || product_id == RS410_PID )
|
|
{
|
|
width = 1280;
|
|
height = 720;
|
|
}
|
|
stream_name_to_default_profile["Depth"] = get_index_of_profile( stream_name_to_profiles.at( "Depth" ),
|
|
realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Z16 ), width, height ) );
|
|
stream_name_to_default_profile["Infrared_1"] = get_index_of_profile( stream_name_to_profiles.at( "Infrared_1" ),
|
|
realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Y8 ), width, height ) );
|
|
stream_name_to_default_profile["Infrared_2"] = get_index_of_profile( stream_name_to_profiles.at( "Infrared_2" ),
|
|
realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_Y8 ), width, height ) );
|
|
|
|
width = 1280;
|
|
height = 720;
|
|
if( product_id == RS405_PID )
|
|
{
|
|
width = 848;
|
|
height = 480;
|
|
}
|
|
stream_name_to_default_profile["Color"] = get_index_of_profile( stream_name_to_profiles.at( "Color" ),
|
|
realdds::dds_video_stream_profile( fps, realdds::dds_video_encoding::from_rs2( RS2_FORMAT_YUYV ), width, height ) );
|
|
}
|
|
}
|
|
|
|
|
|
size_t lrs_device_controller::get_index_of_profile( const realdds::dds_stream_profiles & profiles,
|
|
const realdds::dds_video_stream_profile & profile ) const
|
|
{
|
|
for( size_t i = 0; i < profiles.size(); ++i )
|
|
{
|
|
auto dds_vp = std::dynamic_pointer_cast< dds_video_stream_profile >( profiles[i] );
|
|
if( dds_vp->frequency() == profile.frequency()
|
|
&& dds_vp->encoding() == profile.encoding()
|
|
&& dds_vp->width() == profile.width()
|
|
&& dds_vp->height() == profile.height() )
|
|
return i;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
size_t lrs_device_controller::get_index_of_profile( const realdds::dds_stream_profiles & profiles,
|
|
const realdds::dds_motion_stream_profile & profile ) const
|
|
{
|
|
for( size_t i = 0; i < profiles.size(); ++i )
|
|
{
|
|
if( profiles[i]->frequency() == profile.frequency() )
|
|
return i;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
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{
|
|
{ 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 },
|
|
{ realdds::topics::control::dfu_start::id, &lrs_device_controller::on_dfu_start },
|
|
{ realdds::topics::control::dfu_apply::id, &lrs_device_controller::on_dfu_apply },
|
|
};
|
|
auto it = control_handlers.find( id );
|
|
if( it == control_handlers.end() )
|
|
return false;
|
|
|
|
return (this->*(it->second))( control, reply );
|
|
}
|
|
|
|
|
|
bool lrs_device_controller::on_hardware_reset( json const & control, json & reply )
|
|
{
|
|
_dds_device_server->broadcast_disconnect();
|
|
_rs_dev.hardware_reset();
|
|
return true;
|
|
}
|
|
|
|
|
|
bool lrs_device_controller::on_hwm( json const & control, json & reply )
|
|
{
|
|
rs2::debug_protocol dp( _rs_dev );
|
|
if( ! dp )
|
|
throw std::runtime_error( "device does not have a debug protocol implemented" );
|
|
|
|
rsutils::string::hexarray data;
|
|
|
|
uint32_t 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( 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( 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( topics::control::hwm::key::build_command ).default_value( false ) )
|
|
{
|
|
reply[topics::reply::hwm::key::data] = data;
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
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[topics::reply::hwm::key::data] = data;
|
|
return true;
|
|
}
|
|
|
|
|
|
struct lrs_device_controller::dfu_support
|
|
{
|
|
rs2::device rsdev;
|
|
std::string uid;
|
|
std::weak_ptr< dds_device_server > server;
|
|
std::weak_ptr< lrs_device_controller > controller;
|
|
std::shared_ptr< realdds::dds_topic_reader > reader;
|
|
std::shared_ptr< realdds::topics::blob_msg > image;
|
|
realdds::dds_guid_prefix initiator;
|
|
|
|
std::string debug_name() const
|
|
{
|
|
if( auto dev = server.lock() )
|
|
return rsutils::string::from() << "[" << dev->debug_name() << "] ";
|
|
return {};
|
|
}
|
|
};
|
|
|
|
|
|
bool lrs_device_controller::on_dfu_start( rsutils::json const & control, rsutils::json & reply )
|
|
{
|
|
if( _dfu )
|
|
throw std::runtime_error( "DFU already in progress" );
|
|
|
|
// The FW-update-ID is used to recognize the device, even in recovery mode (serial-number is not enough)
|
|
if( ! _rs_dev.supports( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID ) )
|
|
throw std::runtime_error( "device does not support fw-update-id" );
|
|
|
|
// Either updatable (regular device) or update_device (recovery) implement the check_fw_compatibility API, from
|
|
// which we can proceed
|
|
if( ! rs2::updatable( _rs_dev ) && ! rs2::update_device( _rs_dev ) )
|
|
throw std::runtime_error( "device is not in an updatable state" );
|
|
|
|
_dfu = std::make_shared< dfu_support >();
|
|
_dfu->server = _dds_device_server;
|
|
_dfu->controller = shared_from_this();
|
|
_dfu->rsdev = _rs_dev;
|
|
_dfu->initiator
|
|
= realdds::guid_from_string( reply[realdds::topics::reply::key::sample][0].string_ref() ).guidPrefix;
|
|
_dfu->uid = _rs_dev.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID );
|
|
|
|
// Open a DFU topic and wait for the image on another thread
|
|
auto topic = topics::blob_msg::create_topic( _dds_device_server->participant(),
|
|
_dds_device_server->topic_root() + realdds::topics::DFU_TOPIC_NAME );
|
|
|
|
_dfu->reader = std::make_shared< dds_topic_reader_thread >( topic, _dds_device_server->subscriber() );
|
|
_dfu->reader->on_data_available(
|
|
[weak_dfu = std::weak_ptr< dfu_support >( _dfu )]
|
|
{
|
|
topics::blob_msg blob;
|
|
eprosima::fastdds::dds::SampleInfo sample;
|
|
while( auto dfu = weak_dfu.lock() )
|
|
{
|
|
if( ! topics::blob_msg::take_next( *dfu->reader, &blob, &sample ) )
|
|
break;
|
|
if( ! blob.is_valid() )
|
|
continue;
|
|
auto const & sender = sample.sample_identity.writer_guid().guidPrefix;
|
|
if( sender != dfu->initiator )
|
|
{
|
|
LOG_ERROR( dfu->debug_name()
|
|
<< "DFU image received from " << realdds::print_raw_guid_prefix( sender ) << " != "
|
|
<< realdds::print_raw_guid_prefix( dfu->initiator ) << " that started the DFU" );
|
|
}
|
|
else if( dfu->image )
|
|
{
|
|
LOG_ERROR( dfu->debug_name() << "More than one DFU image received!" );
|
|
}
|
|
else
|
|
{
|
|
size_t const n_bytes = blob.data().size();
|
|
auto const crc = rsutils::number::calc_crc32( blob.data().data(), blob.data().size() );
|
|
LOG_INFO( dfu->debug_name() << "DFU image received, " << n_bytes << " bytes, crc " << crc );
|
|
|
|
// Build a reply
|
|
rsutils::json j = rsutils::json::object( {
|
|
{ realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
|
|
{ "size", n_bytes },
|
|
{ "crc", crc } } );
|
|
|
|
try
|
|
{
|
|
// Check the image
|
|
rs2_error * e = nullptr;
|
|
bool is_compatible = rs2_check_firmware_compatibility( dfu->rsdev.get().get(),
|
|
blob.data().data(),
|
|
(int)blob.data().size(),
|
|
&e );
|
|
rs2::error::handle( e );
|
|
|
|
if( ! is_compatible )
|
|
throw std::runtime_error( "image is incompatible" );
|
|
|
|
// Keep it until we get a 'dfu-apply'
|
|
dfu->image = std::make_shared< topics::blob_msg >( std::move( blob ) );
|
|
}
|
|
catch( std::exception const & e )
|
|
{
|
|
j[realdds::topics::reply::key::status] = "check-fw-compat";
|
|
j[realdds::topics::reply::key::explanation] = e.what();
|
|
LOG_ERROR( dfu->debug_name() << "DFU image check failed: " << e.what() << "; exiting DFU state" );
|
|
if( auto controller = dfu->controller.lock() )
|
|
controller->_dfu.reset(); // no longer in DFU state
|
|
}
|
|
|
|
if( auto server = dfu->server.lock() )
|
|
server->publish_notification( std::move( j ) );
|
|
}
|
|
}
|
|
} );
|
|
|
|
dds_topic_reader::qos rqos( eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS );
|
|
rqos.override_from_json( _dds_device_server->participant()->settings().nested( "device", "dfu" ) );
|
|
_dfu->reader->run( rqos );
|
|
|
|
// Start a thread that will cancel if we don't receive an image in time, or if the process somehow takes too long
|
|
std::thread(
|
|
[weak_dfu = std::weak_ptr< dfu_support >( _dfu )]
|
|
{
|
|
std::this_thread::sleep_for( std::chrono::seconds( 5 ) );
|
|
if( auto dfu = weak_dfu.lock() )
|
|
{
|
|
if( ! dfu->image )
|
|
{
|
|
if( auto controller = dfu->controller.lock() )
|
|
{
|
|
LOG_ERROR( dfu->debug_name() << "DFU timed out waiting for image; resetting" );
|
|
controller->_dfu.reset(); // no longer in DFU state
|
|
}
|
|
if( auto server = dfu->server.lock() )
|
|
{
|
|
rsutils::json j = rsutils::json::object(
|
|
{ { realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
|
|
{ realdds::topics::reply::key::status, "error" },
|
|
{ realdds::topics::reply::key::explanation,
|
|
"timed out waiting for an image; resetting" } } );
|
|
server->publish_notification( std::move( j ) );
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
return;
|
|
|
|
// We have an image: sleep some more and cancel the DFU after a while if no dfu-apply is received
|
|
std::this_thread::sleep_for( std::chrono::seconds( 10 ) );
|
|
if( auto dfu = weak_dfu.lock() )
|
|
{
|
|
if( auto controller = dfu->controller.lock() )
|
|
{
|
|
LOG_ERROR( dfu->debug_name() << "DFU timed out waiting for apply; resetting" );
|
|
controller->_dfu.reset(); // no longer in DFU state
|
|
}
|
|
if( auto server = dfu->server.lock() )
|
|
{
|
|
rsutils::json j = rsutils::json::object(
|
|
{ { realdds::topics::notification::key::id, realdds::topics::notification::dfu_ready::id },
|
|
{ realdds::topics::reply::key::status, "error" },
|
|
{ realdds::topics::reply::key::explanation, "timed out; resetting" } } );
|
|
server->publish_notification( std::move( j ) );
|
|
}
|
|
}
|
|
} )
|
|
.detach();
|
|
|
|
return true; // we handled it
|
|
}
|
|
|
|
|
|
bool lrs_device_controller::on_dfu_apply( rsutils::json const & control, rsutils::json & reply )
|
|
{
|
|
if( _dfu )
|
|
{
|
|
auto const sender
|
|
= realdds::guid_from_string( reply[realdds::topics::reply::key::sample][0].string_ref() ).guidPrefix;
|
|
if( sender != _dfu->initiator )
|
|
throw std::runtime_error( "only the DFU initiator can apply/cancel" );
|
|
}
|
|
|
|
if( control.nested( realdds::topics::control::dfu_apply::key::cancel ).default_value( false ) )
|
|
{
|
|
if( _dfu )
|
|
LOG_DEBUG( _dfu->debug_name() << "DFU cancelled" );
|
|
_dfu.reset();
|
|
return true; // handled
|
|
}
|
|
|
|
if( ! _dfu )
|
|
throw std::runtime_error( "DFU not started" );
|
|
if( ! _dfu->reader )
|
|
throw std::runtime_error( "dfu-apply already received" );
|
|
|
|
// Keep the DFU alive once apply starts, even if reset somewhere else - but the reader is no longer needed
|
|
auto dfu = _dfu;
|
|
_dfu->reader.reset();
|
|
|
|
if( ! _dfu->image )
|
|
throw std::runtime_error( "no image received" );
|
|
|
|
// We want to return a reply right away; we already have an image in hand, so all that needs to happen is to apply
|
|
// it in the background and return a reply when done
|
|
std::thread(
|
|
[dfu]
|
|
{
|
|
auto on_fail = [dfu]( char const * what )
|
|
{
|
|
LOG_ERROR( "Failed to apply DFU image: " << what );
|
|
if( auto server = dfu->server.lock() )
|
|
{
|
|
rsutils::json j
|
|
= json::object( { { realdds::topics::reply::key::id, realdds::topics::reply::dfu_apply::id },
|
|
{ realdds::topics::reply::key::status, "error" } } );
|
|
if( what )
|
|
j[realdds::topics::reply::key::explanation] = what;
|
|
server->publish_notification( std::move( j ) );
|
|
}
|
|
};
|
|
|
|
try
|
|
{
|
|
// We already checked FW compatibility; to actually do a signed update, we switch to recovery mode (we need
|
|
// an update_device to do a signed update - but to get one, we need to be in recovery mode):
|
|
rs2::update_device update_device = dfu->rsdev;
|
|
if( ! update_device )
|
|
{
|
|
LOG_INFO( "Switching device (update ID " << dfu->uid << ") to recovery mode" );
|
|
if( auto server = dfu->server.lock() )
|
|
server->broadcast_disconnect( { 3, 0 } ); // timeout for ACKs
|
|
rs2::updatable( dfu->rsdev ).enter_update_state();
|
|
// NOTE: the device will go offline; the adapter will see the device disappear, and so will the controller/server!
|
|
|
|
// We need a context but cannot get one from the device... so we can just create a new one:
|
|
rsutils::json settings;
|
|
settings["dds"] = false; // don't want DDS devices
|
|
rs2::context context( settings.dump() );
|
|
|
|
// Wait for the recovery device
|
|
rsutils::time::timer timeout( std::chrono::seconds( 60 ) );
|
|
while( dfu->controller.lock() )
|
|
{
|
|
try
|
|
{
|
|
auto devs = context.query_devices( RS2_PRODUCT_LINE_ANY_INTEL );
|
|
for( uint32_t j = 0; j < devs.size(); j++ )
|
|
{
|
|
auto d = devs[j];
|
|
if( d.is< rs2::update_device >() && d.supports( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID )
|
|
&& dfu->uid == d.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID ) )
|
|
{
|
|
LOG_DEBUG( "... found DFU recovery device (update ID " << dfu->uid << ")" );
|
|
update_device = d;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
catch( std::exception const & e )
|
|
{
|
|
LOG_DEBUG( "... error looking for DFU device: " << e.what() );
|
|
}
|
|
catch( ... )
|
|
{
|
|
LOG_DEBUG( "... error looking for DFU device" );
|
|
}
|
|
|
|
if( update_device )
|
|
break;
|
|
|
|
if( timeout.has_expired() )
|
|
throw std::runtime_error( "failed to find recovery device" );
|
|
|
|
// Wait a little and retry
|
|
std::this_thread::sleep_for( std::chrono::milliseconds( 1000 ) );
|
|
}
|
|
}
|
|
if( update_device )
|
|
{
|
|
if( auto controller = dfu->controller.lock() )
|
|
{
|
|
LOG_INFO( "Updating - DO NOT SHUT DOWN" );
|
|
int progress = 0;
|
|
update_device.update(
|
|
dfu->image->data(),
|
|
[dfu, progress = int( 0 )]( float percent ) mutable
|
|
{
|
|
int new_progress = int( percent * 10 ); // update only every 10%
|
|
if( new_progress != progress )
|
|
{
|
|
progress = new_progress;
|
|
if( auto server = dfu->server.lock() )
|
|
server->publish_notification(
|
|
json::object( { { realdds::topics::notification::key::id,
|
|
realdds::topics::notification::dfu_apply::id },
|
|
{ realdds::topics::notification::dfu_apply::key::progress,
|
|
percent } } ) );
|
|
}
|
|
} );
|
|
// NOTE: the device will go offline again, and should come up normally
|
|
LOG_INFO( "DFU done" );
|
|
}
|
|
}
|
|
}
|
|
catch( std::exception const & e )
|
|
{
|
|
on_fail( e.what() );
|
|
}
|
|
catch( ... )
|
|
{
|
|
on_fail( nullptr );
|
|
}
|
|
|
|
// Whether successful or not, we're done with the DFU
|
|
if( auto controller = dfu->controller.lock() )
|
|
controller->_dfu.reset();
|
|
} )
|
|
.detach();
|
|
|
|
return true; // handled
|
|
}
|
|
|
|
|