Debian Codegraph @

libs/hwdrivers/src/CImpinjRFID.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/CImpinjRFID.h>
13 #include <mrpt/system/os.h>
14
15 #include <chrono>
16 #include <iostream>
17 #include <thread>
18
19 using namespace mrpt::hwdrivers;
20 using namespace mrpt::system;
21 using namespace std::literals;
22
23 IMPLEMENTS_GENERIC_SENSOR(CImpinjRFID, mrpt::hwdrivers)
24
25 CImpinjRFID::CImpinjRFID()
26 {
27 m_sensorLabel = "RFID";
28 connected = false;
29 }
30
31 CImpinjRFID::~CImpinjRFID() { closeReader(); }
32 void CImpinjRFID::initialize()
33 {
34 // start the driver
35 // use a separate thread so the connection can be established while the
36 // program starts. This is essential because the module will stall on the
37 // accept() call until the driver executable requests a connection, and, on
38 // the other hand, if the module is not listening, the driver will fail
39 std::thread(dummy_startDriver, this).detach();
40 // system::createThreadFromObjectMethod<CImpinjRFID>(this,startDriver);
41
42 // start connection
43
44 connect();
45 }
46
47 void CImpinjRFID::dummy_startDriver(CImpinjRFID* o) { o->startDriver(); }
48 void CImpinjRFID::startDriver()
49 {
50 // start the driver
51 std::stringstream cmdline;
52 std::cout << "Waiting for the driver to start ... ";
53
54 // create the command line (executable path + parameters)
55 cmdline << driver_path << " " << reader_name.c_str() << " " << IPm.c_str() << " " << port;
56
57 // wait until the current module starts the sockets and listens to it
58 std::this_thread::sleep_for(2s);
59
60 const int ret = ::system(cmdline.str().c_str());
61 if (0 != ret)
62 std::cerr << "[CImpinjRFID::startDriver] Error (" << ret << ") invoking command:\n"
63 << cmdline.str() << std::endl;
64 }
65
66 void CImpinjRFID::loadConfig_sensorSpecific(
67 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
68 {
69 MRPT_START
70 // TEMPORARILY DISABLED
71 /* pose_x_1 = configSource.read_float(iniSection,"pose_x_1",0,true);
72 pose_y_1 = configSource.read_float(iniSection,"pose_y_1",0,true);
73 pose_z_1 = configSource.read_float(iniSection,"pose_z_1",0,true);
74 pose_roll_1 =
75 configSource.read_float(iniSection,"pose_roll_1",0,true);
76 pose_pitch_1 =
77 configSource.read_float(iniSection,"pose_pitch_1",0,true);
78 pose_yaw_1 =
79 configSource.read_float(iniSection,"pose_yaw_1",0,true);
80
81 pose_x_2 = configSource.read_float(iniSection,"pose_x_2",0,true);
82 pose_y_2 = configSource.read_float(iniSection,"pose_y_2",0,true);
83 pose_z_2 = configSource.read_float(iniSection,"pose_z_2",0,true);
84 pose_roll_2 =
85 configSource.read_float(iniSection,"pose_roll_2",0,true);
86 pose_pitch_2 =
87 configSource.read_float(iniSection,"pose_pitch_2",0,true);
88 pose_yaw_2 =
89 configSource.read_float(iniSection,"pose_yaw_2",0,true);
90 */
91 IPm = configSource.read_string(iniSection, "local_IP", "127.0.0.1", false);
92 reader_name = configSource.read_string(iniSection, "reader_name", "", true);
93 port = configSource.read_int(iniSection, "listen_port", 0, true);
94 driver_path = configSource.read_string(iniSection, "driver_path", "", true);
95
96 MRPT_END
97 }
98
99 /*---------------------------------------------------------------
100 getObservation
101 Get the power of a given network as an observation
102 ---------------------------------------------------------------*/
103 void CImpinjRFID::connect()
104 {
105 if (!connected)
106 {
107 // Start the server
108 server = std::make_unique<mrpt::comms::CServerTCPSocket>(port);
109 }
110
111 client = server->accept();
112
113 std::this_thread::sleep_for(1s);
114 connected = true;
115 }
116
117 /*---------------------------------------------------------------
118 getObservation
119 Get the power of a given network as an observation
120 ---------------------------------------------------------------*/
121 bool CImpinjRFID::getObservation(mrpt::obs::CObservationRFID& obs)
122 {
123 try
124 {
125 bool receivedSomething = false;
126 char msg[34];
127 char cmd[20];
128 char epc[24];
129 char rx_pwr[5];
130 char* tmp;
131 obs.tag_readings.clear();
132 // send an observation command to the device interface program
133 strcpy(cmd, "OBS\0");
134 client->writeAsync(cmd, 10);
135
136 // receive a reading from the sensor through the socket
137 while (client->readAsync(msg, 34, 100) > 0)
138 {
139 receivedSomething = true;
140 // the received string is in the format: ANT_PORT EPC RX_PWR
141 // ZERO_FILL
142 const char* ant_port_ptr = mrpt::system::strtok(msg, " ", &tmp);
143 if (!ant_port_ptr)
144 {
145 std::cerr << "[CImpinjRFID::getObservation] Unexpected format "
146 "in sensor data! (skipping).\n";
147 continue;
148 }
149 const char ant_port = *ant_port_ptr;
150 strcpy(epc, mrpt::system::strtok(nullptr, " ", &tmp));
151 strcpy(rx_pwr, mrpt::system::strtok(nullptr, " ", &tmp));
152
153 // Fill the observation
154 obs.tag_readings.resize(obs.tag_readings.size() + 1); // Alloc space for one more tag obs
155 mrpt::obs::CObservationRFID::TTagReading& new_tag =
156 *obs.tag_readings.rbegin(); // Get a reference to the latest
157 // new tag structure
158
159 // Fill in fields in "new_tag":
160 new_tag.antennaPort = mrpt::format("%c", ant_port);
161 new_tag.epc = std::string(epc);
162 new_tag.power = atof(rx_pwr);
163 obs.sensorLabel = m_sensorLabel;
164
165 // std::cout << "mrpt::hwdrivers::CImpinjRFID::getObservation() " <<
166 // "\n\tRXPWR: " << atof(rx_pwr) << " PWR READ: " << rx_pwr <<
167 // std::endl;
168 }
169 if (receivedSomething)
170 return true;
171 else
172 return false;
173 }
174 catch (const std::exception& e)
175 {
176 std::cerr << e.what() << std::endl;
177 return false;
178 }
179 }
180
181 /*---------------------------------------------------------------
182 getObservation
183 Get the power of a given network as an observation
184 ---------------------------------------------------------------*/
185 void CImpinjRFID::closeReader()
186 {
187 char cmd[20];
188 // send a kill command to the device interface program
189 strcpy(cmd, "END\0");
190 client->writeAsync(cmd, 10); // JL->Emil: Why 10 not strlen(cmd)? Is the
191 // server expecting a fixed length data block?
192 client->close();
193 }
194
195 void CImpinjRFID::doProcess()
196 {
197 mrpt::obs::CObservationRFID::Ptr obs = mrpt::obs::CObservationRFID::Create();
198 if (getObservation(*obs)) appendObservation(obs);
199 }