Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

IMU with Motion topic #11990

Merged
merged 26 commits into from
Jul 18, 2023
Merged

IMU with Motion topic #11990

Show file tree
Hide file tree
Changes from 24 commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
b905151
add ROS2 Imu topics
maloel Jun 21, 2023
eaf77f8
added stream-configurations to device docs
maloel Jun 25, 2023
a7eaa4b
basic imu-msg impl
maloel Jun 25, 2023
2e2d78a
move start_streaming impl from dds-stream-server to video- and motion-
maloel Jun 25, 2023
6d55d77
IMU -> motion
maloel Jun 25, 2023
1f9d75d
remove fisheye, pose streams; gyro+accel -> motion
maloel Jun 25, 2023
78d60dd
send motion intrinsics in 2 parts; report gyro, auto activate accel
maloel Jun 27, 2023
d706bc2
move format to dds_video_stream_profile; remove MXYZ
maloel Jul 10, 2023
35f8d56
additional docs
maloel Jul 10, 2023
13076ae
add RS2_STREAM_MOTION and RS2_FORMAT_COMBINED_MOTION
maloel Jul 10, 2023
d894543
dds motion in librealsense side
maloel Jul 10, 2023
b9820a9
fixes
maloel Jul 11, 2023
5a0e10b
update pyrealdds
maloel Jul 11, 2023
3de25bf
adjust libCI outputs
maloel Jul 11, 2023
b4e6ca7
fix merge issues
maloel Jul 11, 2023
6135477
fix intrinsics & extrinsics tests
maloel Jul 11, 2023
7171e70
fix device-init test
maloel Jul 11, 2023
a6ea118
fix d455 libci server
maloel Jul 11, 2023
d4fbd63
fix librs-device-properties test
maloel Jul 11, 2023
54ee7a1
fix librs-formats-conversion test
maloel Jul 11, 2023
dc986b1
fix stream-sensor-bridge test
maloel Jul 11, 2023
bf0f805
reduce dds device init timeout to 5 (from 30)
maloel Jul 11, 2023
9fb84b7
add imu_msg to pyrealdds
maloel Jul 12, 2023
f0062fe
test.remote timeouts reduced 30 -> 10
maloel Jul 13, 2023
b522026
CR fixes
maloel Jul 16, 2023
816537a
CR fixes
maloel Jul 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 22 additions & 6 deletions common/rendering.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -624,6 +624,18 @@ namespace rs2
}
break;
}
case RS2_FORMAT_COMBINED_MOTION:
{
auto & motion = *reinterpret_cast< const rs2_combined_motion * >( frame.get_data() );
draw_motion_data( (float)motion.linear_acceleration.x,
(float)motion.linear_acceleration.y,
(float)motion.linear_acceleration.z );
draw_motion_data( (float)motion.angular_velocity.x,
(float)motion.angular_velocity.y,
(float)motion.angular_velocity.z,
false ); // Don't clear previous draw
break;
}
case RS2_FORMAT_Y16:
case RS2_FORMAT_Y10BPACK:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data);
Expand Down Expand Up @@ -799,7 +811,7 @@ namespace rs2
draw_text((int)(xy.x - w / 2), (int)xy.y, text);
}

void draw_motion_data(float x, float y, float z)
void draw_motion_data(float x, float y, float z, bool clear=true)
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
Expand All @@ -808,7 +820,8 @@ namespace rs2

glViewport(0, 0, 768, 768);
glClearColor(0, 0, 0, 1);
glClear(GL_COLOR_BUFFER_BIT);
if( clear )
glClear(GL_COLOR_BUFFER_BIT);

glMatrixMode(GL_PROJECTION);
glLoadIdentity();
Expand All @@ -823,11 +836,14 @@ namespace rs2

glRotatef(-135, 0.0f, 1.0f, 0.0f);

draw_axes();
if( clear )
{
draw_axes();

draw_circle(1, 0, 0, 0, 1, 0);
draw_circle(0, 1, 0, 0, 0, 1);
draw_circle(1, 0, 0, 0, 0, 1);
draw_circle( 1, 0, 0, 0, 1, 0 );
draw_circle( 0, 1, 0, 0, 0, 1 );
draw_circle( 1, 0, 0, 0, 0, 1 );
}

const auto canvas_size = 230;
const auto vec_threshold = 0.2f;
Expand Down
10 changes: 10 additions & 0 deletions include/librealsense2/h/rs_sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ typedef enum rs2_stream
RS2_STREAM_GPIO , /**< Signals from external device connected through GPIO */
RS2_STREAM_POSE , /**< 6 Degrees of Freedom pose data, calculated by RealSense device */
RS2_STREAM_CONFIDENCE , /**< 4 bit per-pixel depth confidence level */
RS2_STREAM_MOTION , /**< Native stream of combined motion data (incl. accel & gyro) */
RS2_STREAM_COUNT
} rs2_stream;
const char* rs2_stream_to_string(rs2_stream stream);
Expand Down Expand Up @@ -89,6 +90,7 @@ typedef enum rs2_format
RS2_FORMAT_Z16H , /**< DEPRECATED! - Variable-length Huffman-compressed 16-bit depth values. */
RS2_FORMAT_FG , /**< 16-bit per-pixel frame grabber format. */
RS2_FORMAT_Y411 , /**< 12-bit per-pixel. */
RS2_FORMAT_COMBINED_MOTION , /**< Combined motion data, as in the combined_motion structure */
RS2_FORMAT_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
} rs2_format;
const char* rs2_format_to_string(rs2_format format);
Expand All @@ -100,6 +102,14 @@ typedef struct rs2_extrinsics
float translation[3]; /**< Three-element translation vector, in meters */
} rs2_extrinsics;

/** \brief RS2_STREAM_MOTION / RS2_FORMAT_COMBINED_MOTION content is similar to ROS2's Imu message */
typedef struct rs2_combined_motion
{
struct { double x, y, z, w; } orientation;
struct { double x, y, z; } angular_velocity;
struct { double x, y, z; } linear_acceleration;
} rs2_combined_motion;

/**
* Deletes sensors list, any sensors created from this list will remain unaffected
* \param[in] info_list list to delete
Expand Down
95 changes: 58 additions & 37 deletions src/dds/rs-dds-device-proxy.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,19 +30,13 @@ static rs2_stream to_rs2_stream_type( std::string const & type_string )
{ "depth", RS2_STREAM_DEPTH },
{ "color", RS2_STREAM_COLOR },
{ "ir", RS2_STREAM_INFRARED },
{ "fisheye", RS2_STREAM_FISHEYE },
{ "gyro", RS2_STREAM_GYRO },
{ "accel", RS2_STREAM_ACCEL },
{ "gpio", RS2_STREAM_GPIO },
{ "pose", RS2_STREAM_POSE },
{ "motion", RS2_STREAM_MOTION },
{ "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;
}
throw invalid_value_exception( "unknown stream type '" + type_string + "'" );

return it->second;
}

Expand Down Expand Up @@ -92,7 +86,7 @@ static rs2_motion_stream to_rs2_motion_stream( rs2_stream const stream_type,
prof.index = sidx.index;
prof.uid = sidx.sid;
prof.fps = profile->frequency();
prof.fmt = static_cast< rs2_format >( profile->format().to_rs2() );
prof.fmt = RS2_FORMAT_COMBINED_MOTION;

memcpy( prof.intrinsics.data, intrinsics.data.data(), sizeof( prof.intrinsics.data ) );
memcpy( prof.intrinsics.noise_variances,
Expand Down Expand Up @@ -174,25 +168,26 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
auto const & default_profile = profiles[stream->default_profile_index()];
for( auto & profile : profiles )
{
LOG_DEBUG( " " << profile->details_to_string() );
if( video_stream )
{
auto video_profile = std::static_pointer_cast< realdds::dds_video_stream_profile >( profile );
auto added_stream_profile = sensor.add_video_stream(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not call this raw_stream_profile? And the comment can apply to both motion and video

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, changing

to_rs2_video_stream( stream_type,
sidx,
std::static_pointer_cast< realdds::dds_video_stream_profile >( profile ),
video_stream->get_intrinsics() ),
to_rs2_video_stream( stream_type, sidx, video_profile, video_stream->get_intrinsics() ),
profile == default_profile );
_stream_name_to_profiles[stream->name()].push_back( added_stream_profile ); // for extrinsics
}
else if( motion_stream )
{
auto added_stream_profile = sensor.add_motion_stream(
auto motion_profile = std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile );
auto raw_motion_profile = sensor.add_motion_stream(
to_rs2_motion_stream( stream_type,
sidx,
std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile ),
motion_stream->get_intrinsics() ),
motion_profile,
motion_stream->get_gyro_intrinsics() ),
profile == default_profile );
_stream_name_to_profiles[stream->name()].push_back( added_stream_profile ); // for extrinsics
_stream_name_to_profiles[stream->name()].push_back( raw_motion_profile );
// NOTE: the raw motion profile will be cloned and overriden by the format converter!
}
}

Expand All @@ -211,28 +206,43 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_

for( auto & sensor_info : sensor_name_to_info )
{
LOG_DEBUG( sensor_info.first );
sensor_info.second.proxy->initialization_done();

// Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph.
for( auto & profile : sensor_info.second.proxy->get_stream_profiles() )
{
sid_index type_and_index( profile->get_stream_type(), profile->get_stream_index());
if( auto p = std::dynamic_pointer_cast< librealsense::video_stream_profile_interface >( profile ) )
{
LOG_DEBUG( " " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' '
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that framerate should follow the resolution and format after.
E.g. 1280x720@30 RGB8 (instead of 1280x720 RGB8 @ 30)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK :)

<< p->get_width() << 'x' << p->get_height() << ' ' << get_string( p->get_format() )
<< " @ " << p->get_framerate() );
}
else if( auto p = std::dynamic_pointer_cast<librealsense::motion_stream_profile_interface>( profile ) )
{
LOG_DEBUG( " " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' '
<< get_string( p->get_format() ) << " @ " << p->get_framerate() );
}
sid_index type_and_index( profile->get_stream_type(), profile->get_stream_index() );

auto & streams = sensor_info.second.proxy->streams();

sid_index sidx = type_and_index_to_dds_stream_sidx[type_and_index];
sid_index sidx = type_and_index_to_dds_stream_sidx.at( type_and_index );
auto stream_iter = streams.find( sidx );
if( stream_iter == streams.end() )
{
LOG_DEBUG( "Did not find dds_stream of profile (" << profile->get_stream_type() << ", "
<< profile->get_stream_index() << ")" );
LOG_DEBUG( " no dds stream" );
continue;
}

profile->set_unique_id( stream_iter->first.sid );
profile->set_unique_id( sidx.sid ); // Was lost on clone

// NOTE: the 'initialization_done' call above creates target profiles from the raw profiles we supplied it.
// The raw profile intrinsics will be overriden to call the target's intrinsics function (which by default
// calls the raw again, creating an infinite loop), so we must override the target:
set_profile_intrinsics( profile, stream_iter->second );

_stream_name_to_profiles[stream_iter->second->name()].push_back( profile ); // For extrinsics
_stream_name_to_profiles.at( stream_iter->second->name() ).push_back( profile ); // For extrinsics

tag_default_profile_of_stream( profile, stream_iter->second );
}
Expand All @@ -250,17 +260,23 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
} );
}

// According to extrinsics_graph (in environment.h) we need 3 steps
// According to extrinsics_graph (in environment.h) we need 3 steps:

// 1. Register streams with extrinsics between them
for( auto & from_stream : _stream_name_to_librs_stream )
if( _dds_dev->has_extrinsics() )
{
for( auto & to_stream : _stream_name_to_librs_stream )
for( auto & from_stream : _stream_name_to_librs_stream )
{
if( from_stream.first != to_stream.first )
for( auto & to_stream : _stream_name_to_librs_stream )
{
const auto & dds_extr = _dds_dev->get_extrinsics( from_stream.first, to_stream.first );
if( dds_extr )
if( from_stream.first != to_stream.first )
{
auto const dds_extr = _dds_dev->get_extrinsics( from_stream.first, to_stream.first );
if( ! dds_extr )
{
LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first );
continue;
}
rs2_extrinsics extr = to_rs2_extrinsics( dds_extr );
environment::get_instance().get_extrinsics_graph().register_extrinsics( *from_stream.second,
*to_stream.second,
Expand All @@ -269,6 +285,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
}
}
}

// 2. Register all profiles
for( auto & it : _stream_name_to_profiles )
{
Expand All @@ -277,6 +294,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
environment::get_instance().get_extrinsics_graph().register_profile( *profile );
}
}

// 3. Link profile to it's stream
for( auto & it : _stream_name_to_librs_stream )
{
Expand Down Expand Up @@ -348,7 +366,7 @@ void dds_device_proxy::set_motion_profile_intrinsics( std::shared_ptr< stream_pr
std::shared_ptr< realdds::dds_motion_stream > stream ) const
{
auto msp = std::dynamic_pointer_cast< motion_stream_profile >( profile );
auto & stream_intrinsics = stream->get_intrinsics();
auto & stream_intrinsics = stream->get_gyro_intrinsics();
rs2_motion_device_intrinsic intr;
memcpy( intr.data, stream_intrinsics.data.data(), sizeof( intr.data ) );
memcpy( intr.noise_variances, stream_intrinsics.noise_variances.data(), sizeof( intr.noise_variances ) );
Expand All @@ -369,20 +387,23 @@ std::shared_ptr< dds_sensor_proxy > dds_device_proxy::create_sensor( const std::


// Tagging converted profiles. dds_sensor_proxy::add_video/motion_stream tagged the raw profiles.
void dds_device_proxy::tag_default_profile_of_stream( const std::shared_ptr<stream_profile_interface> & profile,
const std::shared_ptr< const realdds::dds_stream > & stream ) const
void dds_device_proxy::tag_default_profile_of_stream(
const std::shared_ptr< stream_profile_interface > & profile,
const std::shared_ptr< const realdds::dds_stream > & stream ) const
{
auto const & dds_default_profile = stream->default_profile();

if( profile->get_stream_type() == to_rs2_stream_type( stream->type_string() ) &&
profile->get_format() == dds_default_profile->format().to_rs2() &&
profile->get_framerate() == dds_default_profile->frequency() )
{
auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile );
auto dds_vsp = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( dds_default_profile );
if( vsp && dds_vsp &&
( vsp->get_width() != dds_vsp->width() || vsp->get_height() != dds_vsp->height() ) )
return; // Video profiles of incompatible resolutions
if( vsp && dds_vsp )
{
if( vsp->get_width() != dds_vsp->width() || vsp->get_height() != dds_vsp->height()
|| vsp->get_format() != dds_vsp->format().to_rs2() )
return; // Video profiles of incompatible resolutions
}

profile->tag_profile( PROFILE_TAG_DEFAULT );
}
Expand Down
Loading
Loading