void ObstacleAvoidanceController::process_messages() {
cout << "ObstacleAvoidanceController process messages" << endl;
MessageBuffer* sub_buf = Subscriber::getBuffer();
TestMessage* tm = (TestMessage*) sub_buf->remove_head();
cout << "I received a message from " << tm->publisher() << " with counter " << tm->counter() << endl;
tm->set_counter(tm->counter() + 1);
tm->set_topic("behavior");
publish(tm);
}
void LocController::process_messages()
{
MessageBuffer* sub_buf = Subscriber::getBuffer();
TestMessage* tm = (TestMessage*)sub_buf->remove_head();
cout << "I received a message from " << tm->publisher() << " with counter " << tm->counter() << endl;
tm->set_counter(tm->counter() + 1);
tm->set_topic("sensors");
publish(tm);
}