| 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 |
} |