42 #if defined(SIMPLE_MESSAGE_USE_ROS) || defined(ROS)
59 namespace message_manager
65 MessageManager::MessageManager()
67 this->num_handlers_ = 0;
68 for (
unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
70 this->handlers_[i] = NULL;
72 this->comms_hndlr_ = NULL;
75 MessageManager::~MessageManager()
84 LOG_INFO(
"Initializing message manager with default comms fault handler");
87 if (NULL != connection)
89 this->getDefaultCommsFaultHandler().init(connection);
95 LOG_ERROR(
"NULL connection passed into manager init");
106 LOG_INFO(
"Initializing message manager");
108 if (NULL != connection && NULL != fault_handler)
110 this->setConnection(connection);
111 this->getPingHandler().init(connection);
112 this->setCommsFaultHandler(fault_handler);
114 if (this->add(&this->getPingHandler()))
121 LOG_WARN(
"Failed to add ping handler, manager won't respond to pings");
127 LOG_ERROR(
"NULL connection or NULL fault handler passed into manager init");
136 void MessageManager::spinOnce()
141 if(!this->getConnection()->isConnected())
143 this->getCommsFaultHandler()->connectionFailCB();
146 if (this->getConnection()->receiveMsg(msg))
162 this->getConnection()->sendMsg(fail);
163 LOG_WARN(
"Unhandled message type encounters, sending failure reply");
170 LOG_ERROR(
"Failed to receive incoming message");
171 this->getCommsFaultHandler()->sendFailCB();
178 #ifdef SIMPLE_MESSAGE_MOTOPLUS
184 std::this_thread::sleep_for(std::chrono::seconds(sec));
188 void MessageManager::spin()
190 LOG_INFO(
"Entering message manager spin loop");
192 #if defined(SIMPLE_MESSAGE_USE_ROS) || defined(ROS)
201 if (!this->getConnection()->isConnected())
218 if (this->getMaxNumHandlers() > this->getNumHandlers())
220 this->handlers_[this->getNumHandlers()] = handler;
221 this->setNumHandlers(this->getNumHandlers() + 1);
227 LOG_ERROR(
"Max number of handlers exceeded");
231 else if (allow_replace)
233 this->handlers_[idx] = handler;
256 return this->handlers_[idx];
259 int MessageManager::getHandlerIdx(
int msg_type)
264 for (
unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
266 temp = this->handlers_[i];
267 if (NULL == temp)
break;