59 #ifndef CALLBACKHANDLERS_HPP 60 #define CALLBACKHANDLERS_HPP 63 #include <boost/foreach.hpp> 65 #include <boost/function.hpp> 68 #include <boost/thread.hpp> 70 #include <boost/thread/condition.hpp> 71 #include <boost/tokenizer.hpp> 73 #include <boost/algorithm/string/join.hpp> 75 #include <boost/date_time/posix_time/posix_time.hpp> 77 #include <boost/asio.hpp> 79 #include <boost/bind.hpp> 80 #include <boost/format.hpp> 81 #include <boost/asio/serial_port.hpp> 82 #include <boost/thread/mutex.hpp> 117 bool Wait(
const boost::posix_time::time_duration& timeout)
119 boost::mutex::scoped_lock lock(
mutex_);
132 template <
typename T>
140 virtual const T&
Get() {
return message_; }
144 boost::mutex::scoped_lock lock(
mutex_);
147 if (!mMessage.
Read<T>(message_, message_key))
149 std::ostringstream ss;
150 ss <<
"Read unsuccessful: mosaic decoder error for message with ID (empty field if non-determinable)" << mMessage.
MessageID() <<
". Reason unknown.";
151 throw std::runtime_error(ss.str());
152 ROS_INFO(
"%s", ss.str().c_str());
155 }
catch (std::runtime_error& e)
157 std::ostringstream ss;
158 ss <<
"Read unsuccessful: mosaic decoder error for message with ID " << mMessage.
MessageID() <<
".\nReason: " << e.what();
159 throw std::runtime_error(ss.str());
160 ROS_INFO(
"%s", ss.str().c_str());
165 if (func_) func_(message_);
184 typedef std::multimap<std::string, boost::shared_ptr<AbstractCallbackHandler>>
CallbackMap;
188 template <
typename T>
193 boost::mutex::scoped_lock lock(callback_mutex_);
195 callbackmap_.insert(std::make_pair(message_key, boost::shared_ptr<AbstractCallbackHandler>(handler)));
196 CallbackMap::key_type key = message_key;
197 ROS_DEBUG(
"Key successfully inserted to multimap: %s", ((
unsigned int) callbackmap_.count(key)) ?
"true" :
"false");
206 boost::mutex::scoped_lock lock(callback_mutex_);
207 CallbackMap::key_type key = mMessage.
MessageID();
208 std::string ID_temp = mMessage.
MessageID();
209 if (!(ID_temp ==
"4013" || ID_temp ==
"4027" || ID_temp ==
"4001"|| ID_temp ==
"5908"))
212 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
216 callback->second->Handle(mMessage, callback->first);
218 catch (std::runtime_error& e)
220 throw std::runtime_error(e.what());
227 CallbackMap::key_type key =
"NavSatFix";
228 std::string ID_temp = mMessage.
MessageID();
229 if (ID_temp ==
"4007")
232 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
236 callback->second->Handle(mMessage, callback->first);
238 catch (std::runtime_error& e)
240 throw std::runtime_error(e.what());
248 CallbackMap::key_type key =
"PoseWithCovarianceStamped";
249 std::string ID_temp = mMessage.
MessageID();
250 if (ID_temp ==
"4007")
253 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
257 callback->second->Handle(mMessage, callback->first);
259 catch (std::runtime_error& e)
261 throw std::runtime_error(e.what());
269 CallbackMap::key_type key =
"GPST";
270 std::string ID_temp = mMessage.
MessageID();
271 if (ID_temp ==
"4007")
274 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key); callback != callbackmap_.upper_bound(key); ++callback)
278 callback->second->Handle(mMessage, callback->first);
280 catch (std::runtime_error& e)
282 throw std::runtime_error(e.what());
290 CallbackMap::key_type key1 =
"GPSFix";
291 std::string ID_temp = mMessage.
MessageID();
292 if (ID_temp ==
"4007")
295 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1); callback != callbackmap_.upper_bound(key1); ++callback)
299 callback->second->Handle(mMessage, callback->first);
301 catch (std::runtime_error& e)
303 throw std::runtime_error(e.what());
307 CallbackMap::key_type key2 = mMessage.
MessageID();
308 if (ID_temp ==
"4013" || ID_temp ==
"4027" || ID_temp ==
"4001" || ID_temp ==
"5908")
312 for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2); callback != callbackmap_.upper_bound(key2); ++callback)
316 callback->second->Handle(mMessage, callback->first);
318 catch (std::runtime_error& e)
320 throw std::runtime_error(e.what());
339 if (mMessage.
IsSBF())
341 unsigned long sbf_block_length;
342 sbf_block_length = (
unsigned long) mMessage.
BlockLength();
343 ROS_DEBUG(
"ROSaic reading SBF block %s with %lu bytes...", mMessage.
MessageID().c_str(), sbf_block_length);
344 if (sbf_block_length < 15)
351 boost::char_separator<char> sep(
"\r");
352 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
353 std::size_t nmea_size = std::min(mMessage.
SegmentEnd(),
static_cast<std::size_t
>(89));
354 std::string block_in_string(reinterpret_cast<const char*>(mMessage.
Pos()), nmea_size);
355 tokenizer tokens(block_in_string, sep);
356 ROS_DEBUG(
"The NMEA message is ready to be parsed. It reads: %s", (*tokens.begin()).c_str());
360 std::size_t response_size = mMessage.
SegmentEnd();
361 std::string block_in_string(reinterpret_cast<const char*>(mMessage.
Pos()), response_size);
362 ROS_DEBUG(
"mosaic's response contains %li bytes and reads:\n %s", response_size, block_in_string.c_str());
371 ROS_ERROR(
"Invalid command just sent to mosaic!");
376 std::string cd(reinterpret_cast<const char*>(mMessage.
Pos()), 4);
378 ROS_INFO_COND(
cd_count == 0,
"The connection descriptor for the TCP connection is %s", cd.c_str());
382 boost::mutex::scoped_lock lock(
cd_mutex);
393 catch (std::runtime_error& e)
395 ROS_DEBUG(
"Without a (circular) buffer and a new thread, we would have faced an incomplete message right now. %s", e.what());
396 throw (static_cast<std::size_t>(mMessage.
Pos() - data));
bool publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
Represents ensemble of (to be constructed) ROS messages, to be handled at once by this class...
boost::condition_variable condition_
std::string MessageID()
Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embell...
const uint8_t * Search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Defines a class that can deal with a buffer of size bytes_transferred that is handed over from async_...
CallbackHandler(const Callback &func=Callback())
bool Wait(const boost::posix_time::time_duration &timeout)
CallbackMap Insert(std::string message_key, typename CallbackHandler< T >::Callback callback)
const uint8_t * End()
Gets the end position in the read buffer.
uint16_t BlockLength()
Gets the length of the SBF block.
bool IsErrorMessage()
Determines whether data_ currently points to an error message reply from mosaic.
boost::condition_variable response_condition
Condition variable complementing "response_mutex".
void Handle(mosaicMessage &mMessage, std::string message_key)
bool Found()
Has an NMEA message or SBF block been found in the buffer?
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
void Handle(mosaicMessage &mMessage)
const uint8_t * Pos()
Gets the current position in the read buffer.
bool cd_received
Determines whether the connection descriptor was received from mosaic.
void ReadCallback(const uint8_t *data, std::size_t &size)
Searches for mosaic messages that could potentially be decoded/parsed/published.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool response_received
Determines whether a command reply was received from mosaic.
static boost::mutex callback_mutex_
std::multimap< std::string, boost::shared_ptr< AbstractCallbackHandler > > CallbackMap
Key is std::string and represents the ROS message key, value is boost::shared_ptr<CallbackHandler> ...
std::string mosaic_tcp_port
Mosaic TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
boost::mutex response_mutex
Mutex to control changes of global variable "response_received".
bool Read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF block) and populates message with the necessary content (mapped 1-to-1...
bool IsSBF()
Determines whether data_ currently points to an SBF block.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
boost::mutex cd_mutex
Mutex to control changes of global variable "cd_received".
Can search buffer for messages, read/parse them, and so on.
virtual void Handle(mosaicMessage &mMessage, std::string message_key)=0
Abstract class representing a generic callback handler, includes high-level functionality such as wai...
bool publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
boost::condition_variable cd_condition
Condition variable complementing "cd_mutex".
bool publish_posewithcovariancestamped
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
boost::function< void(const T &)> Callback