Example: hwdrivers_taobotics_imu

This example demonstrates the C++ API to read from Taobotics IMU sensors (e.g. ROS IMU HFI A9).

Short video: https://www.youtube.com/shorts/qaaP9BmZYmo

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CTaoboticsIMU.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>

#include <chrono>
#include <iostream>
#include <thread>

std::string serialPort;  // Name of the serial port to open

// ------------------------------------------------------
//              Test_IMU
// ------------------------------------------------------
void TestIMU()
{
  mrpt::hwdrivers::CTaoboticsIMU imu;

  if (!serialPort.empty())
  {
    std::cout << "Using serial port: " << serialPort << std::endl;
    imu.setSerialPort(serialPort);
  }
  // otherwise, use default port.

  // Load config from INI file:
  // imu.loadConfig( CConfigFile( "./config.ini") ,"IMU");

  std::cout << "Trying to initialize the sensor..." << std::endl;
  imu.initialize();  // This will raise an exception on error

  mrpt::gui::CDisplayWindow3D win("IMU", 1024, 800);

  auto glIMU = mrpt::opengl::stock_objects::CornerXYZ();
  {
    auto& scene = win.get3DSceneAndLock();
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());
    scene->insert(glIMU);

    win.unlockAccess3DScene();
  }

  const double t0 = mrpt::Clock::nowDouble();
  size_t totalObsCount = 0;

  while (1)
  {
    imu.doProcess();

    const auto lstObs = imu.getObservations();
    if (lstObs.empty()) continue;

    auto o = std::dynamic_pointer_cast<mrpt::obs::CObservationIMU>(lstObs.begin()->second);
    ASSERT_(o);

    std::cout << o->asString() << "\n";

    const auto imuPose = mrpt::poses::CPose3D::FromQuaternion(mrpt::math::CQuaternionDouble(
        o->get(mrpt::obs::IMU_ORI_QUAT_W), o->get(mrpt::obs::IMU_ORI_QUAT_X),
        o->get(mrpt::obs::IMU_ORI_QUAT_Y), o->get(mrpt::obs::IMU_ORI_QUAT_Z)));

    // Sensor rate:
    const double t1 = mrpt::Clock::nowDouble();
    totalObsCount += lstObs.size();

    double sensorRate = totalObsCount / (t1 - t0);

    // Update 3D view:
    {
      // auto& scene =
      win.get3DSceneAndLock();

      glIMU->setPose(imuPose);

      win.addTextMessage(5, 5, mrpt::format("Sensor Rate: %.02f Hz", sensorRate));

      win.unlockAccess3DScene();
    }
    win.forceRepaint();
  };
}

int main(int argc, char** argv)
{
  try
  {
    if (argc > 1)
    {
      serialPort = std::string(argv[1]);
    }

    TestIMU();
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "Error: " << mrpt::exception_to_str(e) << std::endl;
    return -1;
  }
}