Debian Codegraph @

libs/hwdrivers/src/CGenericSensor.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CGenericSensor.h>
13 #include <mrpt/obs/CAction.h>
14 #include <mrpt/obs/CObservation.h>
15
16 using namespace mrpt::obs;
17 using namespace mrpt::system;
18 using namespace mrpt::hwdrivers;
19 using namespace mrpt::serialization;
20 using namespace std;
21
22 /*-------------------------------------------------------------
23 Constructor
24 -------------------------------------------------------------*/
25 CGenericSensor::CGenericSensor()
26 {
27 const char* sVerbose = getenv("MRPT_HWDRIVERS_VERBOSE");
28 m_verbose = (sVerbose != nullptr) && atoi(sVerbose) != 0;
29 }
30
31 /*-------------------------------------------------------------
32 Destructor
33 -------------------------------------------------------------*/
34 CGenericSensor::~CGenericSensor()
35 {
36 // Free objects in list, if any:
37 m_objList.clear();
38 }
39
40 /*-------------------------------------------------------------
41 appendObservations
42 -------------------------------------------------------------*/
43 void CGenericSensor::appendObservations(
44 const std::vector<mrpt::serialization::CSerializable::Ptr>& objs)
45 {
46 if (++m_grab_decimation_counter >= m_grab_decimation)
47 {
48 m_grab_decimation_counter = 0;
49
50 std::lock_guard<std::mutex> lock(m_csObjList);
51
52 for (const auto& obj : objs)
53 {
54 if (!obj) continue;
55
56 // It must be a CObservation or a CAction!
57 TTimeStamp timestamp;
58
59 if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CAction)))
60 {
61 timestamp = dynamic_cast<CAction*>(obj.get())->timestamp;
62 }
63 else if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
64 {
65 timestamp = dynamic_cast<CObservation*>(obj.get())->timestamp;
66 }
67 else
68 THROW_EXCEPTION("Passed object must be CObservation.");
69
70 // Add it:
71 m_objList.insert(TListObsPair(timestamp, obj));
72 }
73 }
74 }
75
76 /*-------------------------------------------------------------
77 getObservations
78 -------------------------------------------------------------*/
79 CGenericSensor::TListObservations CGenericSensor::getObservations()
80 {
81 std::lock_guard<std::mutex> lock(m_csObjList);
82 auto lstObjects = m_objList;
83 m_objList.clear(); // Memory of objects will be freed by invoker.
84 return lstObjects;
85 }
86
87 /*-------------------------------------------------------------
88
89 createSensor
90
91 -------------------------------------------------------------*/
92 CGenericSensor* CGenericSensor::createSensor(const std::string& className)
93 {
94 registered_sensor_classes_t& regs = get_registered_sensor_classes();
95 const auto it = regs.find(className);
96 return it == regs.end() ? nullptr : it->second->ptrCreateObject();
97 }
98
99 // Singleton
100 CGenericSensor::registered_sensor_classes_t& CGenericSensor::get_registered_sensor_classes()
101 {
102 static registered_sensor_classes_t reg;
103 return reg;
104 }
105
106 /*-------------------------------------------------------------
107 registerClass
108 -------------------------------------------------------------*/
109 void CGenericSensor::registerClass(const TSensorClassId* pNewClass)
110 {
111 registered_sensor_classes_t& regs = get_registered_sensor_classes();
112 regs[pNewClass->className] = pNewClass;
113 }
114
115 /** Loads the generic settings common to any sensor (See CGenericSensor), then
116 * call to "loadConfig_sensorSpecific"
117 * \exception This method throws an exception with a descriptive message if
118 * some critical parameter is missing or has an invalid value.
119 */
120 void CGenericSensor::loadConfig(const mrpt::config::CConfigFileBase& cfg, const std::string& sect)
121 {
122 MRPT_START
123
124 m_process_rate =
125 cfg.read_double(sect, "process_rate", 0); // Leave it to 0 so rawlog-grabber can detect
126 // if it's not set by the user.
127 m_max_queue_len = static_cast<size_t>(cfg.read_int(sect, "max_queue_len", int(m_max_queue_len)));
128 m_grab_decimation =
129 static_cast<size_t>(cfg.read_int(sect, "grab_decimation", int(m_grab_decimation)));
130
131 m_sensorLabel = cfg.read_string(sect, "sensorLabel", m_sensorLabel);
132
133 m_grab_decimation_counter = 0;
134
135 loadConfig_sensorSpecific(cfg, sect);
136
137 MRPT_END
138 }