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-2023, 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;
    }
}