void MouseMonitorNodeBeagle::run()
{
ros::Rate r(sampleFrequency);
while (true) {
processSensorData(sensor1, &sensor1Data);
processSensorData(sensor2, &sensor2Data);
ros::spinOnce();
r.sleep();
}
}
void Comm5TCP::ParseData(const unsigned char* data, const size_t len)
{
buffer.append((const char*)data, len);
std::stringstream stream(buffer);
std::string line;
while (std::getline(stream, line, '\n')) {
line = rtrim(line);
if (startsWith(line, "211")) {
std::vector<std::string> tokens = tokenize(line);
if (tokens.size() < 2)
break;
unsigned int relaybitfield = ::strtol(tokens[1].c_str(), 0, 16);
for (int i = 0; i < 16; ++i) {
bool on = (relaybitfield & (1 << i)) != 0 ? true : false;
SendSwitch(i + 1, 1, 255, on, 0, "Relay " + boost::lexical_cast<std::string>(i + 1));
}
}
else if (startsWith(line, "210") && (!startsWith(line, "210 OK"))) {
processSensorData(line);
}
}
// Trim consumed bytes.
buffer.erase(0, buffer.length() - static_cast<unsigned int>(stream.rdbuf()->in_avail()));
}
