void MouseMonitorNodeBeagle::run()
	{
		ros::Rate r(sampleFrequency);

		while (true) {
			processSensorData(sensor1, &sensor1Data);
			processSensorData(sensor2, &sensor2Data);

			ros::spinOnce();
			r.sleep();
		}
	}
Exemple #2
0
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()));
}