IMU with Motion topic #11990

maloel · 2023-07-11T15:01:04Z

IMU DDS implementation is now combined into a single topic:

"Motion" topic has the ROS2 sensor_msgs::msg::Imu format, including orientation, linear acceleration (Accel), and angular velocity (Gyro) in one message
No metadata is sent for Motion messages
Currently, the implementation is similar to the ROS2 wrapper's unite_imu_method value 1 (copy) - added LRS-834 for interpolation
In LibRS, there is now a RS2_STREAM_MOTION in format RS2_FORMAT_COMBINED_MOTION stream

Along the way:

No more fisheye, pose, or the individual accel/gyro streams
Moved format out of general profiles and into video profiles (so no longer for motion)
Reduced device init timeout from 30 seconds to 5
Updated docs

Tracked on [LRS-796]

OhadMeir · 2023-07-13T18:29:38Z

wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/StreamFormat.java

@@ -32,9 +32,9 @@ public enum StreamFormat {
    Z16H(28),
    FG(29),
    Y411(30),
-    Y16I(31);
+    COMBINED_MOTION(31);


Was there a Y16I format at some point?
If changing this file, I think that Z16H is unused anymore

Was there a Y16I format at some point?

There was (though I don't think it was released) -- and then was removed. The rs2_stream enum no longer has it, so I made sure this reflected the current state of the enum.

We cannot go and remove the old enum values (which were released).

OhadMeir · 2023-07-16T05:37:17Z

third-party/realdds/src/topics/imu-msg.cpp

+    imu_data().orientation_covariance() = { -1., 0., 0., 0., 0., 0., 0., 0., 0. };
+    // "you can initiate the sensor covariance matrix by giving some small values on the diagonal"
+    double const cov = 0.01; // TODO from context?
+    imu_data().linear_acceleration_covariance() = { cov, 0., 0., 0., cov, 0., 0., 0., cov };


Shouldn't the first element be set to -1 (as in orientation?)

We do have linear acceleration (accel) and angular velocity (gyro).
This is a copy of the code from the ROS wrapper.

OhadMeir · 2023-07-16T07:58:47Z

src/dds/rs-dds-sensor-proxy.cpp

-    data.frame_number;      // filled in only once metadata is known
+    data.timestamp               // in ms
+        = static_cast< rs2_time_t >( realdds::time_to_double( imu.timestamp() ) * 1e3 );
+    data.timestamp_domain = RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME;  // leave default (hardware domain)


Comment about HW domain does not match actual "global time" value

I'll take out the "global time" setting and leave at default. I changed it when trying stuff.

Bottom line:
Camera-generated domains will always be HW times and, when global time is enable, converted.
Adapter-sent domains may already be global (from the adapter's side) but will be treated as HW meaning may get converted to the client's global time.

OhadMeir · 2023-07-16T08:09:49Z

src/dds/rs-dds-device-proxy.cpp

                if( video_stream )
                {
+                    auto video_profile = std::static_pointer_cast< realdds::dds_video_stream_profile >( profile );
                    auto added_stream_profile = sensor.add_video_stream(


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

OK, changing

OhadMeir · 2023-07-16T08:49:23Z

src/dds/rs-dds-device-proxy.cpp

-            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() << ' '


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

OhadMeir · 2023-07-16T12:42:01Z

tools/dds/dds-adapter/lrs-device-controller.cpp

-        }
-
-        server->init_profiles( profiles, default_profile_index );
-
        // Set stream intrinsics


Remove this comment, it is now redundant and does not relate to metadata handling in this if statement

OhadMeir · 2023-07-16T12:47:04Z

tools/dds/dds-adapter/lrs-device-controller.cpp

        {
            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)


Comment correct to video streams only

Yeah but it's inside a block only for video streams... You want me to change?

OhadMeir · 2023-07-16T12:48:08Z

tools/dds/dds-adapter/lrs-device-controller.cpp

+                       stream_profiles.end(),
+                       [&]( const rs2::stream_profile & sp )
+                       {
+                           switch( sp.stream_type() )


Why switch and not if

There were other cases, maybe? Changed to if.

OhadMeir · 2023-07-16T14:02:29Z

tools/dds/dds-adapter/lrs-device-controller.cpp

                    if( auto video = std::dynamic_pointer_cast< realdds::dds_video_stream_server >( server ) )
+                    {
+                        // RS2_STREAM_DEPTH, RS2_STREAM_COLOR, RS2_STREAM_INFRARED


Confidence?
I think that this comment is redundant. Same for the motion comment.

OhadMeir · 2023-07-16T14:05:07Z

tools/dds/dds-adapter/lrs-device-controller.cpp

-            }
+                    {
+                        // RS2_STREAM_GYRO, RS2_STREAM_ACCEL
+                        static realdds::topics::imu_msg imu;


static variables here can be members.

Yeah but then you have to #include the imu message, etc.
I chose to split the start() call into two parts and pass the IMU "context" (message plus mutex) in.

OhadMeir · 2023-07-16T14:06:27Z

tools/dds/dds-adapter/lrs-device-controller.cpp

+                    auto const stream_type = stream_profile.stream_type();
+                    dds_time const timestamp  // in sec.nsec
+                        ( static_cast< long double >( f.get_timestamp() ) / 1e3 );
+
                    if( auto video = std::dynamic_pointer_cast< realdds::dds_video_stream_server >( server ) )


Please move this logic to a helper function.
Let's keep out lamdas small :-)

Easier said than done. Right now I left it the same (though changed -- check it out).

Looks like all you need to pass the helper function is a reference to sensor

OhadMeir · 2023-07-16T14:09:16Z

tools/dds/dds-adapter/lrs-device-controller.cpp

+            {
+                if( sp.stream_type() != RS2_STREAM_ACCEL )
+                    continue;
+                if( sp.format() != rs2_profile.format() )


Is format check necessary?

Probably not...

OhadMeir · 2023-07-16T14:12:16Z

third-party/realdds/doc/stream-configurations.md

+
+#### Metadata
+
+A single metadata message is expected for all messages from the same sensor (so one for depth, IR left, IR right).


I think that we send a metadata message per active stream, not per active sensor

Right now, yes, you're correct. And after talking to Evgeni, I think I'll take that statement back.

OhadMeir · 2023-07-18T07:25:28Z

tools/dds/dds-adapter/lrs-device-controller.cpp

@@ -464,6 +456,28 @@ find_profile( std::shared_ptr< realdds::dds_stream_server > const & stream,
 }


+std::shared_ptr< realdds::dds_stream_server >
+lrs_device_controller::frame_to_streaming_server( rs2::frame const & f, rs2::stream_profile * p_profile ) const


Why do we need p_profile and all this code handling it? Just to avoid calling f.get_profile() outside this function?

Well, yes -- it's actually two calls into librealsense and populations of data. Why do something twice?

Much less readable, not relevant for all cases and I am not sure if it more efficient...
Had to read several times to understand all this handling with profile/p_profile/profile_.
Passing another parameter, creating a default initialized profile that is not always needed, if statement... just to avoid an API call...

OhadMeir · 2023-07-18T07:31:00Z

tools/dds/dds-adapter/lrs-device-controller.cpp

+                    std::mutex mutex;
+                };
+                sensor.start(
+                    [this, imu = std::make_shared< imu_context >()]( rs2::frame f )


When called from two different threads imu will be the same object for both? Isn't another instance is created?

No, there should be a new make_shared each time a sensor is started.

I.e., when the on_start_sensor lambda is called.

OhadMeir · 2023-07-18T07:31:48Z

tools/dds/dds-adapter/lrs-device-controller.cpp

+                        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 );


I think that std::lock_guard is more efficient

Umm, yes, I don't know how it got there. But essentially the same, maybe slightly more overhead. I'll fix if other changes are needed.

OhadMeir

LGTM

maloel added 22 commits July 11, 2023 17:51

add ROS2 Imu topics

b905151

added stream-configurations to device docs

eaf77f8

basic imu-msg impl

a7eaa4b

move start_streaming impl from dds-stream-server to video- and motion-

2e2d78a

IMU -> motion

6d55d77

remove fisheye, pose streams; gyro+accel -> motion

1f9d75d

send motion intrinsics in 2 parts; report gyro, auto activate accel

78d60dd

move format to dds_video_stream_profile; remove MXYZ

d706bc2

additional docs

35f8d56

add RS2_STREAM_MOTION and RS2_FORMAT_COMBINED_MOTION

13076ae

dds motion in librealsense side

d894543

fixes

b9820a9

update pyrealdds

5a0e10b

adjust libCI outputs

3de25bf

fix merge issues

b4e6ca7

fix intrinsics & extrinsics tests

6135477

fix device-init test

7171e70

fix d455 libci server

a6ea118

fix librs-device-properties test

d4fbd63

fix librs-formats-conversion test

54ee7a1

fix stream-sensor-bridge test

dc986b1

reduce dds device init timeout to 5 (from 30)

bf0f805

maloel requested a review from OhadMeir July 11, 2023 15:01

maloel marked this pull request as ready for review July 11, 2023 15:53

maloel added 2 commits July 12, 2023 08:58

add imu_msg to pyrealdds

9fb84b7

test.remote timeouts reduced 30 -> 10

f0062fe

OhadMeir reviewed Jul 13, 2023

View reviewed changes

OhadMeir reviewed Jul 16, 2023

View reviewed changes

CR fixes

b522026

OhadMeir reviewed Jul 16, 2023

View reviewed changes

CR fixes

816537a

OhadMeir reviewed Jul 18, 2023

View reviewed changes

OhadMeir approved these changes Jul 18, 2023

View reviewed changes

maloel merged commit 97f9b0e into IntelRealSense:dds Jul 18, 2023
15 checks passed

maloel deleted the dds branch July 18, 2023 09:46


		#### Metadata

		A single metadata message is expected for all messages from the same sensor (so one for depth, IR left, IR right).

IMU with Motion topic #11990

IMU with Motion topic #11990

Conversation

maloel commented Jul 11, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

OhadMeir left a comment

Choose a reason for hiding this comment

maloel commented Jul 11, 2023 •

edited

Loading