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.
230 lines
8.2 KiB
230 lines
8.2 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
|
|
|
|
#include <realdds/dds-device-server.h>
|
|
#include <realdds/dds-stream-server.h>
|
|
#include <realdds/dds-participant.h>
|
|
#include <realdds/dds-option.h>
|
|
#include <realdds/dds-log-consumer.h>
|
|
#include <realdds/topics/device-info-msg.h>
|
|
|
|
#include "lrs-device-watcher.h"
|
|
#include "lrs-device-controller.h"
|
|
|
|
#include <tclap/CmdLine.h>
|
|
#include <tclap/ValueArg.h>
|
|
|
|
#include <rsutils/os/executable-name.h>
|
|
#include <rsutils/os/special-folder.h>
|
|
#include <rsutils/easylogging/easyloggingpp.h>
|
|
#include <rsutils/json.h>
|
|
#include <rsutils/json-config.h>
|
|
|
|
#include <string>
|
|
#include <iostream>
|
|
#include <map>
|
|
#include <set>
|
|
|
|
using namespace TCLAP;
|
|
using namespace realdds;
|
|
|
|
|
|
std::string get_topic_root( std::string const & name, std::string const & serial_number )
|
|
{
|
|
// Build device root path (we use a device model only name like DXXX)
|
|
// example: /realsense/D435/11223344
|
|
constexpr char const * DEVICE_NAME_PREFIX = "Intel RealSense ";
|
|
constexpr size_t DEVICE_NAME_PREFIX_CCH = 16;
|
|
// We don't need the prefix in the path
|
|
std::string model_name = name;
|
|
if( model_name.length() > DEVICE_NAME_PREFIX_CCH
|
|
&& 0 == strncmp( model_name.data(), DEVICE_NAME_PREFIX, DEVICE_NAME_PREFIX_CCH ) )
|
|
{
|
|
model_name.erase( 0, DEVICE_NAME_PREFIX_CCH );
|
|
}
|
|
for( auto it = model_name.begin(); it != model_name.end(); ++it )
|
|
if( *it == ' ' )
|
|
*it = '_'; // e.g., 'D4xx Recovery'
|
|
constexpr char const * RS_ROOT = "realsense/";
|
|
return RS_ROOT + model_name + '_' + serial_number;
|
|
}
|
|
|
|
|
|
topics::device_info rs2_device_to_info( rs2::device const & dev )
|
|
{
|
|
rsutils::json j;
|
|
|
|
// Name is mandatory
|
|
std::string const name = dev.get_info( RS2_CAMERA_INFO_NAME );
|
|
j[realdds::topics::device_info::key::name] = name;
|
|
|
|
if( dev.supports( RS2_CAMERA_INFO_SERIAL_NUMBER ) )
|
|
j[realdds::topics::device_info::key::serial] = dev.get_info( RS2_CAMERA_INFO_SERIAL_NUMBER );
|
|
if( dev.supports( RS2_CAMERA_INFO_PRODUCT_LINE ) )
|
|
j["product-line"] = dev.get_info( RS2_CAMERA_INFO_PRODUCT_LINE );
|
|
if( dev.supports( RS2_CAMERA_INFO_CAMERA_LOCKED ) )
|
|
j["locked"] = ( strcmp( dev.get_info( RS2_CAMERA_INFO_CAMERA_LOCKED ), "YES" ) == 0 );
|
|
|
|
// FW update ID is a must for DFU; all cameras should have one
|
|
std::string const serial_number = dev.get_info( RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID );
|
|
j["fw-update-id"] = serial_number;
|
|
if( auto update_device = rs2::update_device( dev ) )
|
|
{
|
|
j[realdds::topics::device_info::key::recovery] = true;
|
|
if( dev.supports( RS2_CAMERA_INFO_PRODUCT_ID ) )
|
|
// Append the ID so we have it
|
|
j[realdds::topics::device_info::key::name] = name + " [" + dev.get_info( RS2_CAMERA_INFO_PRODUCT_ID ) + "]";
|
|
}
|
|
|
|
if( dev.supports( RS2_CAMERA_INFO_FIRMWARE_VERSION ) )
|
|
j["fw-version"] = dev.get_info( RS2_CAMERA_INFO_FIRMWARE_VERSION );
|
|
|
|
// Build device topic root path
|
|
j[realdds::topics::device_info::key::topic_root] = get_topic_root( name, serial_number );
|
|
|
|
return topics::device_info::from_json( j );
|
|
}
|
|
|
|
|
|
static rsutils::json load_settings( rsutils::json const & local_settings )
|
|
{
|
|
// Load the realsense configuration file settings
|
|
std::string const filename = rsutils::os::get_special_folder( rsutils::os::special_folder::app_data ) + RS2_CONFIG_FILENAME;
|
|
auto config = rsutils::json_config::load_from_file( filename );
|
|
|
|
// Take just the 'context' part
|
|
config = rsutils::json_config::load_settings( config, "context", "config-file" );
|
|
|
|
// Take the "dds" settings only
|
|
config = config.nested( "dds" );
|
|
|
|
// We should always have DDS enabled
|
|
if( config.is_object() )
|
|
config.erase( "enabled" );
|
|
|
|
// Patch the given local settings into the configuration
|
|
config.override( local_settings, "local settings" );
|
|
|
|
return config;
|
|
}
|
|
|
|
|
|
int main( int argc, char * argv[] )
|
|
try
|
|
{
|
|
dds_domain_id domain = -1; // from settings; default to 0
|
|
CmdLine cmd( "librealsense rs-dds-adapter tool, use CTRL + C to stop..", ' ' );
|
|
ValueArg< dds_domain_id > domain_arg( "d",
|
|
"domain",
|
|
"Select domain ID to publish on",
|
|
false,
|
|
0,
|
|
"0-232" );
|
|
SwitchArg debug_arg( "", "debug", "Enable debug logging", false );
|
|
|
|
cmd.add( domain_arg );
|
|
cmd.add( debug_arg );
|
|
cmd.parse( argc, argv );
|
|
|
|
// Configure the same logger as librealsense
|
|
rsutils::configure_elpp_logger( debug_arg.isSet() );
|
|
// Intercept DDS messages and redirect them to our own logging mechanism
|
|
eprosima::fastdds::dds::Log::ClearConsumers();
|
|
eprosima::fastdds::dds::Log::RegisterConsumer( realdds::log_consumer::create() );
|
|
|
|
if( debug_arg.isSet() )
|
|
{
|
|
rs2::log_to_console( RS2_LOG_SEVERITY_DEBUG );
|
|
eprosima::fastdds::dds::Log::SetVerbosity( eprosima::fastdds::dds::Log::Info );
|
|
}
|
|
else
|
|
{
|
|
rs2::log_to_console( RS2_LOG_SEVERITY_ERROR );
|
|
eprosima::fastdds::dds::Log::SetVerbosity( eprosima::fastdds::dds::Log::Error );
|
|
}
|
|
|
|
if( domain_arg.isSet() )
|
|
{
|
|
domain = domain_arg.getValue();
|
|
if( domain > 232 )
|
|
{
|
|
std::cerr << "Invalid domain value, enter a value in the range [0, 232]" << std::endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
}
|
|
|
|
std::cout << "Starting RS DDS Adapter.." << std::endl;
|
|
|
|
// Create a DDS participant
|
|
auto participant = std::make_shared< dds_participant >();
|
|
{
|
|
rsutils::json dds_settings = rsutils::json::object();
|
|
dds_settings = load_settings( dds_settings );
|
|
participant->init( domain, "rs-dds-adapter", std::move( dds_settings ) );
|
|
}
|
|
|
|
struct device_handler
|
|
{
|
|
topics::device_info info;
|
|
std::shared_ptr< dds_device_server > server;
|
|
std::shared_ptr< tools::lrs_device_controller > controller;
|
|
};
|
|
std::map< rs2::device, device_handler > device_handlers_list;
|
|
|
|
std::cout << "Start listening to RS devices.." << std::endl;
|
|
|
|
// Create a RealSense context
|
|
rsutils::json j = {
|
|
{ "dds", false }, // Don't discover DDS devices from the network, we want local devices only
|
|
{ "format-conversion", "basic" } // Don't convert raw sensor formats (except interleaved) will be done by receiver
|
|
};
|
|
rs2::context ctx( j.dump() );
|
|
|
|
// Run the LRS device watcher
|
|
tools::lrs_device_watcher dev_watcher( ctx );
|
|
dev_watcher.run(
|
|
// Handle a device connection
|
|
[&]( rs2::device dev ) {
|
|
|
|
auto dev_info = rs2_device_to_info( dev );
|
|
|
|
// Create a dds-device-server for this device
|
|
auto dds_device_server
|
|
= std::make_shared< realdds::dds_device_server >( participant, dev_info.topic_root() );
|
|
|
|
// Create a lrs_device_manager for this device
|
|
std::shared_ptr< tools::lrs_device_controller > lrs_device_controller
|
|
= std::make_shared< tools::lrs_device_controller >( dev, dds_device_server );
|
|
|
|
// Finally, we're ready to broadcast it
|
|
dds_device_server->broadcast( dev_info );
|
|
|
|
// Keep a pair of device controller and server per RS device
|
|
device_handlers_list.emplace( dev, device_handler{ dev_info, dds_device_server, lrs_device_controller } );
|
|
},
|
|
// Handle a device disconnection
|
|
[&]( rs2::device dev ) {
|
|
// Remove the dds-server for this device
|
|
auto const & handler = device_handlers_list.at( dev );
|
|
|
|
device_handlers_list.erase( dev );
|
|
} );
|
|
|
|
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), 0);// Pend until CTRL + C is pressed
|
|
|
|
std::cout << "Shutting down rs-dds-adapter..." << std::endl;
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|
|
catch( const rs2::error & e )
|
|
{
|
|
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args()
|
|
<< "):\n " << e.what() << std::endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
catch( const std::exception & e )
|
|
{
|
|
std::cerr << e.what() << std::endl;
|
|
return EXIT_FAILURE;
|
|
}
|