message_manager.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 #ifndef FLATHEADERS
35 #else
36 #include "message_manager.h"
37 #include "log_wrapper.h"
38 #include "simple_message.h"
39 #endif
40 
41 // remove ROS after Melodic (bw compat for #262)
42 #if defined(SIMPLE_MESSAGE_USE_ROS) || defined(ROS)
43 #include "ros/ros.h"
44 #else
45 #include "unistd.h"
46 #endif
47 
48 #include <thread>
49 #include <chrono>
50 
51 using namespace industrial::smpl_msg_connection;
52 using namespace industrial::message_handler;
53 using namespace industrial::simple_message;
54 using namespace industrial::comms_fault_handler;
56 
57 namespace industrial
58 {
59 namespace message_manager
60 {
61 
65 MessageManager::MessageManager()
66 {
67  this->num_handlers_ = 0;
68  for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
69  {
70  this->handlers_[i] = NULL;
71  }
72  this->comms_hndlr_ = NULL;
73 }
74 
75 MessageManager::~MessageManager()
76 {
77 
78 }
79 
80 bool MessageManager::init(SmplMsgConnection* connection)
81 {
82  bool rtn = false;
83 
84  LOG_INFO("Initializing message manager with default comms fault handler");
85 
86 
87  if (NULL != connection)
88  {
89  this->getDefaultCommsFaultHandler().init(connection);
90  this->init(connection, (CommsFaultHandler*)(&this->getDefaultCommsFaultHandler()));
91  rtn = true;
92  }
93  else
94  {
95  LOG_ERROR("NULL connection passed into manager init");
96  rtn = false;
97  }
98 
99  return rtn;
100 }
101 
102 bool MessageManager::init(SmplMsgConnection* connection, CommsFaultHandler* fault_handler)
103 {
104  bool rtn = false;
105 
106  LOG_INFO("Initializing message manager");
107 
108  if (NULL != connection && NULL != fault_handler)
109  {
110  this->setConnection(connection);
111  this->getPingHandler().init(connection);
112  this->setCommsFaultHandler(fault_handler);
113 
114  if (this->add(&this->getPingHandler()))
115  {
116  rtn = true;
117  }
118  else
119  {
120  rtn = false;
121  LOG_WARN("Failed to add ping handler, manager won't respond to pings");
122  }
123 
124  }
125  else
126  {
127  LOG_ERROR("NULL connection or NULL fault handler passed into manager init");
128  rtn = false;
129  }
130 
131  return rtn;
132  }
133 
134 
135 
136 void MessageManager::spinOnce()
137 {
138  SimpleMessage msg;
139  MessageHandler* handler = NULL;
140 
141  if(!this->getConnection()->isConnected())
142  {
143  this->getCommsFaultHandler()->connectionFailCB();
144  }
145 
146  if (this->getConnection()->receiveMsg(msg))
147  {
148  LOG_COMM("Message received");
149  handler = this->getHandler(msg.getMessageType());
150 
151  if (NULL != handler)
152  {
153  LOG_DEBUG("Executing handler callback for message type: %d", handler->getMsgType());
154  handler->callback(msg);
155  }
156  else
157  {
159  {
162  this->getConnection()->sendMsg(fail);
163  LOG_WARN("Unhandled message type encounters, sending failure reply");
164  }
165  LOG_ERROR("Message callback for message type: %d, not executed", msg.getMessageType());
166  }
167  }
168  else
169  {
170  LOG_ERROR("Failed to receive incoming message");
171  this->getCommsFaultHandler()->sendFailCB();
172  }
173 }
174 
176 void mySleep(int sec)
177 {
178 #ifdef SIMPLE_MESSAGE_MOTOPLUS
179  if (ms_per_clock <= 0)
180  ms_per_clock = mpGetRtc();
181 
182  mpTaskDelay(sec * 1000 / ms_per_clock);
183 #else
184  std::this_thread::sleep_for(std::chrono::seconds(sec));
185 #endif
186 }
187 
188 void MessageManager::spin()
189 {
190  LOG_INFO("Entering message manager spin loop");
191 // remove ROS after Melodic (bw compat for #262)
192 #if defined(SIMPLE_MESSAGE_USE_ROS) || defined(ROS)
193  while (ros::ok())
194 #else
195  while (true)
196 #endif
197  {
198  this->spinOnce();
199 
200  // Throttle loop speed if waiting for a re-connection
201  if (!this->getConnection()->isConnected())
202  mySleep(5);
203  }
204 }
205 
206 bool MessageManager::add(MessageHandler * handler, bool allow_replace)
207 {
208  int idx = -1;
209  bool rtn = false;
210 
211  if (NULL != handler)
212  {
213  // If get handler returns -1 then a hander for the message type
214  // does not exist, and a new one should be added
215  idx = getHandlerIdx(handler->getMsgType());
216  if (0 > idx)
217  {
218  if (this->getMaxNumHandlers() > this->getNumHandlers())
219  {
220  this->handlers_[this->getNumHandlers()] = handler;
221  this->setNumHandlers(this->getNumHandlers() + 1);
222  LOG_INFO("Added message handler for message type: %d", handler->getMsgType());
223  rtn = true;
224  }
225  else
226  {
227  LOG_ERROR("Max number of handlers exceeded");
228  rtn = false;
229  }
230  }
231  else if (allow_replace)
232  {
233  this->handlers_[idx] = handler;
234  }
235  else
236  {
237  LOG_ERROR("Failed to add handler for: %d, handler already exists", handler->getMsgType());
238  rtn = false;
239  }
240  }
241  else
242  {
243  LOG_ERROR("NULL handler not added");
244  rtn = false;
245  }
246  return rtn;
247 }
248 
249 MessageHandler* MessageManager::getHandler(int msg_type)
250 {
251  int idx = getHandlerIdx(msg_type);
252 
253  if (idx < 0)
254  return NULL;
255  else
256  return this->handlers_[idx];
257 }
258 
259 int MessageManager::getHandlerIdx(int msg_type)
260 {
261  int rtn = -1;
262  MessageHandler* temp = NULL;
263 
264  for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++)
265  {
266  temp = this->handlers_[i];
267  if (NULL == temp) break; // end of handler-list
268 
269  if (temp->getMsgType() == msg_type)
270  {
271  rtn = i;
272  break;
273  }
274  }
275 
276  return rtn;
277 }
278 
279 } // namespace message_manager
280 } // namespace industrial
industrial::message_handler::MessageHandler::callback
bool callback(industrial::simple_message::SimpleMessage &in)
Callback function that should be executed when a message arrives DO NOT OVERRIDE THIS FUNCTION....
Definition: message_handler.cpp:88
industrial::message_handler::MessageHandler
Interface definition for message handlers. The interface defines the callback function that should ex...
Definition: message_handler.h:61
LOG_INFO
#define LOG_INFO(format,...)
Definition: log_wrapper.h:132
LOG_WARN
#define LOG_WARN(format,...)
Definition: log_wrapper.h:133
industrial::simple_message::SimpleMessage::init
bool init(int msgType, int commType, int replyCode, industrial::byte_array::ByteArray &data)
Initializes a fully populated simple message.
Definition: simple_message.cpp:70
industrial::message_manager::mySleep
void mySleep(int sec)
Definition: message_manager.cpp:176
ros.h
industrial
Definition: byte_array.h:45
simple_message.h
spinOnce
ROSCPP_DECL void spinOnce()
industrial::smpl_msg_connection
Definition: smpl_msg_connection.h:48
ros::ok
ROSCPP_DECL bool ok()
LOG_ERROR
#define LOG_ERROR(format,...)
Definition: log_wrapper.h:134
industrial::simple_message::SimpleMessage::getCommType
int getCommType()
Gets message type(see CommType)
Definition: simple_message.h:246
message_manager.h
LOG_DEBUG
#define LOG_DEBUG(format,...)
Definition: log_wrapper.h:131
industrial::simple_message::SimpleMessage
This class defines a simple messaging protocol for communicating with an industrial robot controller.
Definition: simple_message.h:164
LOG_COMM
#define LOG_COMM(format,...)
Definition: log_wrapper.h:130
industrial::simple_message
Definition: simple_message.h:49
industrial::message_handler
Definition: message_handler.h:45
industrial::comms_fault_handler
Definition: comms_fault_handler.h:37
industrial::simple_comms_fault_handler
Definition: simple_comms_fault_handler.h:47
log_wrapper.h
industrial::smpl_msg_connection::SmplMsgConnection
Defines an interface and common methods for sending simple messages (see simple_message)....
Definition: smpl_msg_connection.h:61
industrial::comms_fault_handler::CommsFaultHandler
Interface definition for communications fault handler. Defines the type of communcations faults that ...
Definition: comms_fault_handler.h:46
industrial::simple_message::CommTypes::SERVICE_REQUEST
@ SERVICE_REQUEST
Definition: simple_message.h:99
init
void init(const M_string &remappings)
industrial::simple_message::ReplyTypes::FAILURE
@ FAILURE
Definition: simple_message.h:115
msg_type
def msg_type(f)
industrial::simple_message::SimpleMessage::getMessageType
int getMessageType()
Gets message type(see StandardMsgType)
Definition: simple_message.h:239
industrial::message_handler::MessageHandler::getMsgType
int getMsgType()
Gets message type that callback expects.
Definition: message_handler.h:93
industrial::simple_message::CommTypes::SERVICE_REPLY
@ SERVICE_REPLY
Definition: simple_message.h:100
industrial::message_manager::ms_per_clock
int ms_per_clock
Definition: message_manager.cpp:175


simple_message
Author(s): Shaun Edwards
autogenerated on Wed Mar 2 2022 00:24:53