/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, 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 "hwdrivers-precomp.h" // Precompiled headers // #include #include #include using namespace mrpt::obs; using namespace mrpt::system; using namespace mrpt::hwdrivers; using namespace mrpt::serialization; using namespace std; /*------------------------------------------------------------- Constructor -------------------------------------------------------------*/ CGenericSensor::CGenericSensor() { const char* sVerbose = getenv("MRPT_HWDRIVERS_VERBOSE"); m_verbose = (sVerbose != nullptr) && atoi(sVerbose) != 0; } /*------------------------------------------------------------- Destructor -------------------------------------------------------------*/ CGenericSensor::~CGenericSensor() { // Free objects in list, if any: m_objList.clear(); } /*------------------------------------------------------------- appendObservations -------------------------------------------------------------*/ void CGenericSensor::appendObservations( const std::vector& objs) { if (++m_grab_decimation_counter >= m_grab_decimation) { m_grab_decimation_counter = 0; std::lock_guard lock(m_csObjList); for (const auto& obj : objs) { if (!obj) continue; // It must be a CObservation or a CAction! TTimeStamp timestamp; if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CAction))) { timestamp = dynamic_cast(obj.get())->timestamp; } else if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation))) { timestamp = dynamic_cast(obj.get())->timestamp; } else THROW_EXCEPTION("Passed object must be CObservation."); // Add it: m_objList.insert(TListObsPair(timestamp, obj)); } } } /*------------------------------------------------------------- getObservations -------------------------------------------------------------*/ CGenericSensor::TListObservations CGenericSensor::getObservations() { std::lock_guard lock(m_csObjList); auto lstObjects = m_objList; m_objList.clear(); // Memory of objects will be freed by invoker. return lstObjects; } /*------------------------------------------------------------- createSensor -------------------------------------------------------------*/ CGenericSensor* CGenericSensor::createSensor(const std::string& className) { registered_sensor_classes_t& regs = get_registered_sensor_classes(); const auto it = regs.find(className); return it == regs.end() ? nullptr : it->second->ptrCreateObject(); } // Singleton CGenericSensor::registered_sensor_classes_t& CGenericSensor::get_registered_sensor_classes() { static registered_sensor_classes_t reg; return reg; } /*------------------------------------------------------------- registerClass -------------------------------------------------------------*/ void CGenericSensor::registerClass(const TSensorClassId* pNewClass) { registered_sensor_classes_t& regs = get_registered_sensor_classes(); regs[pNewClass->className] = pNewClass; } /** Loads the generic settings common to any sensor (See CGenericSensor), then * call to "loadConfig_sensorSpecific" * \exception This method throws an exception with a descriptive message if * some critical parameter is missing or has an invalid value. */ void CGenericSensor::loadConfig(const mrpt::config::CConfigFileBase& cfg, const std::string& sect) { MRPT_START m_process_rate = cfg.read_double(sect, "process_rate", 0); // Leave it to 0 so rawlog-grabber can detect // if it's not set by the user. m_max_queue_len = static_cast(cfg.read_int(sect, "max_queue_len", int(m_max_queue_len))); m_grab_decimation = static_cast(cfg.read_int(sect, "grab_decimation", int(m_grab_decimation))); m_sensorLabel = cfg.read_string(sect, "sensorLabel", m_sensorLabel); m_grab_decimation_counter = 0; loadConfig_sensorSpecific(cfg, sect); MRPT_END }