Example: hwdrivers_taobotics_imu
This example demonstrates the C++ API to read from Taobotics IMU sensors (e.g. ROS IMU HFI A9).
![https://mrpt.github.io/imgs/screenshot_hwdrivers_taobotics_imu.jpg](https://mrpt.github.io/imgs/screenshot_hwdrivers_taobotics_imu.jpg)
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; } }