AsyncReadBuffer.h
Go to the documentation of this file.
00001 
00034 #include <boost/bind.hpp>
00035 #include <boost/asio.hpp>
00036 #include <boost/function.hpp>
00037 
00038 #include <ros/ros.h>
00039 
00040 
00041 template<typename AsyncReadStream>
00042 class AsyncReadBuffer
00043 {
00044 public:
00045   AsyncReadBuffer(AsyncReadStream& s, size_t capacity,
00046                   boost::function<void(const boost::system::error_code&)> error_callback)
00047        : stream_(s), error_callback_(error_callback) {
00048     mem_.resize(capacity);
00049     ROS_ASSERT_MSG(error_callback_, "Bad error callback passed to read buffer.");
00050   }
00051 
00052   void read(size_t read_count, boost::function<void(ros::serialization::IStream&)> callback) {
00053      if (read_count > mem_.size()) {
00054       // Insufficient room in the buffer for the requested bytes,
00055       ROS_ERROR_STREAM_NAMED("async_read", "Requested to read " << read_count << " bytes, but buffer capacity is only " << mem_.size() << ".");
00056       error_callback_(boost::system::errc::make_error_code(boost::system::errc::no_buffer_space));
00057       return;
00058     }
00059 
00060     boost::asio::async_read(stream_,
00061         boost::asio::buffer(&mem_[0], read_count),
00062         boost::asio::transfer_at_least(read_count),
00063         boost::bind(&AsyncReadBuffer::read_cb, this,
00064                     boost::asio::placeholders::error,
00065                     boost::asio::placeholders::bytes_transferred,
00066                     callback));
00067   }
00068 
00069 private:
00070   void read_cb(const boost::system::error_code& error, size_t bytes_transferred,
00071                boost::function<void(ros::serialization::IStream&)> callback) {
00072     if (error) {
00073       error_callback_(error);
00074     } else {
00075       ROS_DEBUG_STREAM_NAMED("async_read", "Transferred " << bytes_transferred << " byte(s).");
00076 
00077       ros::serialization::IStream stream(&mem_[0], bytes_transferred);
00078       ROS_ASSERT_MSG(callback, "Bad read callback function.");
00079       callback(stream);
00080     }
00081   }
00082 
00083   AsyncReadStream& stream_;
00084   std::vector<uint8_t> mem_;
00085   boost::function<void(const boost::system::error_code&)> error_callback_;
00086 };
00087 
00088 


rosserial_server
Author(s): Mike Purvis
autogenerated on Fri Aug 28 2015 12:44:56