Zed2i-imu transform to add to robot urdf

anath93 · July 7, 2023, 1:57pm

I am trying to find ‘rpy’ to provide to my urdf. Already got the ‘xyz’ (-0.002 -0.023 0.000) coordinates using logger by enabling publish_tf and echo. Any thoughts on this ?
AS my first post I could not attach multiple media so added reply to myself for the tf2 echo.

thanks
Ani

anath93 · July 7, 2023, 1:59pm

Myzhar · July 7, 2023, 2:55pm

Hi @anath93
Welcome to the Stereolabs community.

You can use tf2 to retrieve RPY from a quaternion:

github.com

stereolabs/zed-ros-wrapper/blob/master/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp#L3665


      
          
          
            // Publish the zed camera pose if someone has subscribed to
                      if (computeTracking) {
                          mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD);
          
          
                sl::Translation translation = mLastZedPose.getTranslation();
                          sl::Orientation quat = mLastZedPose.getOrientation();
          
          
#if 0 //#ifndef NDEBUG // Enable for TF checking
                  double roll, pitch, yaw;
                  tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw);
          
          
        NODELET_DEBUG("Sensor POSE [%s -> %s] - {%.2f,%.2f,%.2f} {%.2f,%.2f,%.2f}",
                                mLeftCamFrameId.c_str(), mMapFrameId.c_str(),
                                translation.x, translation.y, translation.z,
                                roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG);
          
          
        NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mTrackingStatus));
          
          
#endif

anath93 · July 7, 2023, 3:14pm

Thank you @Myzhar for welcome and reply.

Is there anyway to get that info from CLI the factory default measurements as I am going to hard code this into my URDF and disable publish_tf ? Or do I need to write a simple program to retrieve that first as you mentioned above using tf2 ? OR would you have values for Zed2i already which I can use ?

Thanks
Ani

Myzhar · July 7, 2023, 3:38pm

There are online converters from quaternions to RPY, you simply enter the values you got from the CLI:

anath93 · July 7, 2023, 4:31pm

@Myzhar Thank you, that was quite easy to do. This is the resulting, I got below. I know for a fact that IMU is located towards left lens but to the precise location wise I would appreciate if you can confirm please.

<link name="${name}_imu_link" />
<joint name="${name}_imu_link_joint" type="fixed">
  <parent link="${name}_left_camera_frame"/>
  <child link="${name}_imu_link"/>
  <origin xyz="-0.002 -0.023 0.000" rpy="0.000 -0.000 0.000" />
</joint>

Myzhar · July 10, 2023, 7:39am

The position is good. Didn’t you set the orientation from the quaternion?

anath93 · July 10, 2023, 12:55pm

@Myzhar I had just input the Camera-IMU translation vector which I got in logger before disabling the IMU tf on zed2i yaml file which gave me xyz for the position relative to parent and yes I did apply rpy from quaternion by converting to euler’s angle in radians to input into my urdf resulting in below.

Quaternion vector was [-0.002, -0.000, -0.004, 1.000]
Translation from Zed2i logger [-0.002 -0.023 0.00022]

   <link name="${name}_imu_link" />
    <joint name="${name}_imu_link_joint" type="fixed">
      <parent link="${name}_left_camera_frame"/>
      <child link="${name}_imu_link"/>
      <origin xyz="-0.002 -0.023 0.00022" rpy="-0.004 0.00 -0.008"/>
    </joint>

anath93 · July 31, 2023, 12:07pm

@Myzhar Another quick question, is there anyway to add Namespace ? Except to alter the camera_name ?

Myzhar · July 31, 2023, 12:31pm

You can simply include the launch file into another launch file and use the group keyword to create a new namespace or modify the original launch file:
https://www.stereolabs.com/docs/api/classsl_1_1Mat.html#accab3cb8c395f6b32c897e549fef78a8

anath93 · July 31, 2023, 12:36pm

@Myzhar Thank you !!

anath93 · August 31, 2023, 3:49pm

I have noticed after updating the SDK to 4.0.6, the IMU tf keeps getting published even after Publish_tf is false as I am publishing using my robot urdf. Some screenshots are below

Tried clearing the workspace and rebuilding, but same result.