message_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *       * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *       * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *       * Neither the name of the Southwest Research Institute, nor the names
00016  *       of its contributors may be used to endorse or promote products derived
00017  *       from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 #ifndef FLATHEADERS
00032 #include "simple_message/message_manager.h"
00033 #include "simple_message/log_wrapper.h"
00034 #include "simple_message/simple_message.h"
00035 #else
00036 #include "message_manager.h"
00037 #include "log_wrapper.h"
00038 #include "simple_message.h"
00039 #endif
00040 
00041 #ifdef ROS
00042 #include "ros/ros.h"
00043 #else
00044 #include "unistd.h"
00045 #endif
00046 
00047 using namespace industrial::smpl_msg_connection;
00048 using namespace industrial::message_handler;
00049 using namespace industrial::simple_message;
00050 using namespace industrial::comms_fault_handler;
00051 using namespace industrial::simple_comms_fault_handler;
00052 
00053 namespace industrial
00054 {
00055 namespace message_manager
00056 {
00057 
00061 MessageManager::MessageManager()
00062 {
00063   this->num_handlers_ = 0;
00064   for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
00065   {
00066     this->handlers_[i] = NULL;
00067   }
00068   this->comms_hndlr_ = NULL;
00069 }
00070 
00071 MessageManager::~MessageManager()
00072 {
00073 
00074 }
00075 
00076 bool MessageManager::init(SmplMsgConnection* connection)
00077 {
00078   bool rtn = false;
00079 
00080   LOG_INFO("Initializing message manager with default comms fault handler");
00081 
00082 
00083   if (NULL != connection)
00084   {
00085     this->getDefaultCommsFaultHandler().init(connection);
00086     this->init(connection, (CommsFaultHandler*)(&this->getDefaultCommsFaultHandler()));
00087     rtn = true;
00088   }
00089   else
00090   {
00091     LOG_ERROR("NULL connection passed into manager init");
00092     rtn = false;
00093   }
00094 
00095   return rtn;
00096 }
00097 
00098 bool MessageManager::init(SmplMsgConnection* connection, CommsFaultHandler* fault_handler)
00099 {
00100   bool rtn = false;
00101 
00102     LOG_INFO("Initializing message manager");
00103 
00104     if (NULL != connection && NULL != fault_handler)
00105     {
00106       this->setConnection(connection);
00107       this->getPingHandler().init(connection);
00108       this->setCommsFaultHandler(fault_handler);
00109 
00110       if (this->add(&this->getPingHandler()))
00111       {
00112         rtn = true;
00113       }
00114       else
00115       {
00116         rtn = false;
00117         LOG_WARN("Failed to add ping handler, manager won't respond to pings");
00118       }
00119 
00120     }
00121     else
00122     {
00123       LOG_ERROR("NULL connection or NULL fault handler passed into manager init");
00124       rtn = false;
00125     }
00126 
00127     return rtn;
00128   }
00129 
00130 
00131 
00132 void MessageManager::spinOnce()
00133 {
00134   SimpleMessage msg;
00135   MessageHandler* handler = NULL;
00136 
00137   if(!this->getConnection()->isConnected())
00138   {
00139     this->getCommsFaultHandler()->connectionFailCB();
00140   }
00141 
00142   if (this->getConnection()->receiveMsg(msg))
00143   {
00144     LOG_COMM("Message received");
00145     handler = this->getHandler(msg.getMessageType());
00146 
00147     if (NULL != handler)
00148     {
00149       LOG_DEBUG("Executing handler callback for message type: %d", handler->getMsgType());
00150       handler->callback(msg);
00151     }
00152     else
00153     {
00154       if (CommTypes::SERVICE_REQUEST == msg.getCommType())
00155       {
00156         simple_message::SimpleMessage fail;
00157         fail.init(msg.getMessageType(), CommTypes::SERVICE_REPLY, ReplyTypes::FAILURE);
00158         this->getConnection()->sendMsg(fail);
00159         LOG_WARN("Unhandled message type encounters, sending failure reply");
00160       }
00161       LOG_ERROR("Message callback for message type: %d, not executed", msg.getMessageType());
00162     }
00163   }
00164   else
00165   {
00166     LOG_ERROR("Failed to receive incoming message");
00167     this->getCommsFaultHandler()->sendFailCB();
00168   }
00169 }
00170 
00171 int ms_per_clock;
00172 void mySleep(int sec)
00173 {
00174 #ifdef MOTOPLUS
00175   if (ms_per_clock <= 0)
00176     ms_per_clock = mpGetRtc();
00177 
00178   mpTaskDelay(sec * 1000 / ms_per_clock);
00179 #else
00180   sleep(sec);
00181 #endif
00182 }
00183 
00184 void MessageManager::spin()
00185 {
00186   LOG_INFO("Entering message manager spin loop");
00187 #ifdef ROS
00188   while (ros::ok())
00189 #else
00190   while (true)
00191 #endif
00192   {
00193     this->spinOnce();
00194 
00195     // Throttle loop speed if waiting for a re-connection
00196     if (!this->getConnection()->isConnected())
00197       mySleep(5);
00198   }
00199 }
00200 
00201 bool MessageManager::add(MessageHandler * handler, bool allow_replace)
00202 {
00203   int idx = -1;
00204   bool rtn = false;
00205 
00206   if (NULL != handler)
00207   {
00208     // If get handler returns -1 then a hander for the message type
00209     // does not exist, and a new one should be added
00210     idx = getHandlerIdx(handler->getMsgType());
00211     if (0 > idx)
00212     {
00213       if (this->getMaxNumHandlers() > this->getNumHandlers())
00214       {
00215         this->handlers_[this->getNumHandlers()] = handler;
00216         this->setNumHandlers(this->getNumHandlers() + 1);
00217         LOG_INFO("Added message handler for message type: %d", handler->getMsgType());
00218         rtn = true;
00219       }
00220       else
00221       {
00222         LOG_ERROR("Max number of handlers exceeded");
00223         rtn = false;
00224       }
00225     }
00226     else if (allow_replace)
00227     {
00228       this->handlers_[idx] = handler;
00229     }
00230     else
00231     {
00232       LOG_ERROR("Failed to add handler for: %d, handler already exists", handler->getMsgType());
00233       rtn = false;
00234     }
00235   }
00236   else
00237   {
00238     LOG_ERROR("NULL handler not added");
00239     rtn = false;
00240   }
00241   return rtn;
00242 }
00243 
00244 MessageHandler* MessageManager::getHandler(int msg_type)
00245 {
00246   int idx = getHandlerIdx(msg_type);
00247 
00248   if (idx < 0)
00249     return NULL;
00250   else
00251     return this->handlers_[idx];
00252 }
00253 
00254 int MessageManager::getHandlerIdx(int msg_type)
00255 {
00256   int rtn = -1;
00257   MessageHandler* temp = NULL;
00258 
00259   for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
00260   {
00261     temp = this->handlers_[i];
00262     if (NULL == temp) break;  // end of handler-list
00263 
00264     if (temp->getMsgType() == msg_type)
00265     {
00266         rtn = i;
00267         break;
00268     }
00269   }
00270 
00271   return rtn;
00272 }
00273 
00274 } // namespace message_manager
00275 } // namespace industrial


simple_message
Author(s): Shaun Edwards
autogenerated on Tue Jan 17 2017 21:10:02