00001 #include <cstdlib>
00002 #include <iostream>
00003 #include <boost/bind.hpp>
00004 #include <boost/asio.hpp>
00005 #include "boost/date_time/posix_time/posix_time_types.hpp"
00006 #include "TimeStamp.h""
00007
00008
00009 #ifndef UDPSOCKET_INCLUDEDEF_H
00010 #define UDPSOCKET_INCLUDEDEF_H
00011
00012 union DblBuf
00013 {
00014 double d;
00015 char buf[sizeof(double)];
00016 };
00017
00018
00019 using boost::asio::ip::udp;
00020 class RCVServer
00021 {
00022 public:
00023 RCVServer(boost::asio::io_service& io_service, short port)
00024 : io_service_(io_service),
00025 socket_(io_service, udp::endpoint(udp::v4(), port))
00026 {
00027 m_CurrentValues.push_back(0.0);
00028 m_CurrentValues.push_back(0.0);
00029 m_CurrentValues.push_back(0.0);
00030 m_TimeStamp.SetNow();
00031 // RCV_service = io_service;
00032 socket_.async_receive_from(
00033 boost::asio::buffer(data_, max_length), sender_endpoint_,
00034 boost::bind(&RCVServer::handle_receive_from, this,
00035 boost::asio::placeholders::error,
00036 boost::asio::placeholders::bytes_transferred));
00037 }
00038
00039 boost::asio::io_service RCV_service;
00040
00041
00042 void handle_receive_from(const boost::system::error_code& error,
00043 size_t bytes_recvd)
00044 {
00045 std::vector<double> v;
00046 if(bytes_recvd >= 3*sizeof(double))
00047 {
00048 for(int i=0; i < 3; i++)
00049 {
00050 union DblBuf db;
00051 for(int j=0;j<sizeof(double);j++)
00052 db.buf[j] = data_[(j+i*sizeof(double))];
00053 v.push_back(db.d);
00054 }
00055 }
00056 m_CurrentValues = v;
00057 m_TimeStamp.SetNow();
00058 if (!error && bytes_recvd > 0)
00059 {
00060 socket_.async_send_to(
00061 boost::asio::buffer(data_, bytes_recvd), sender_endpoint_,
00062 boost::bind(&RCVServer::handle_send_to, this,
00063 boost::asio::placeholders::error,
00064 boost::asio::placeholders::bytes_transferred));
00065 }
00066 else
00067 {
00068 socket_.async_receive_from(
00069 boost::asio::buffer(data_, max_length), sender_endpoint_,
00070 boost::bind(&RCVServer::handle_receive_from, this,
00071 boost::asio::placeholders::error,
00072 boost::asio::placeholders::bytes_transferred));
00073 }
00074 }
00075
00076 void handle_send_to(const boost::system::error_code& error, size_t bytes_sent)
00077 {
00078 socket_.async_receive_from(
00079 boost::asio::buffer(data_, max_length), sender_endpoint_,
00080 boost::bind(&RCVServer::handle_receive_from, this,
00081 boost::asio::placeholders::error,
00082 boost::asio::placeholders::bytes_transferred));
00083 }
00084
00085 std::vector<double> getCurrentValues()
00086 {
00087 std::vector<double> ret = m_CurrentValues;
00088 Neobotix::TimeStamp curtime;
00089 curtime.SetNow();
00090 if((curtime - m_TimeStamp)>0.1)
00091 {
00092 std::vector<double> zero;
00093 zero.push_back(0.0);
00094 zero.push_back(0.0);
00095 zero.push_back(0.0);
00096 m_CurrentValues = zero;
00097 }
00098 return ret;
00099
00100 }
00101
00102 boost::asio::io_service& io_service_;
00103 private:
00104 udp::socket socket_;
00105 udp::endpoint sender_endpoint_;
00106 enum { max_length = 1024 };
00107 char data_[max_length];
00108 std::vector<double> m_CurrentValues;
00109 Neobotix::TimeStamp m_TimeStamp;
00110 };
00111
00112
00113 class SNDServer
00114 {
00115 public:
00116 SNDServer(boost::asio::io_service& io_service,
00117 const boost::asio::ip::address& multicast_address, short port)
00118 : endpoint_(multicast_address, port),
00119 socket_(io_service, udp::v4()),
00120 timer_(io_service),
00121 message_count_(0)
00122 {
00123 std::ostringstream os;
00124 std::cout << "Message " << message_count_ << "\n";
00125 os << "Message " << message_count_++;
00126 message_ = os.str();
00127
00128 socket_.async_send_to(
00129 boost::asio::buffer(message_), endpoint_,
00130 boost::bind(&SNDServer::handle_send_to, this,
00131 boost::asio::placeholders::error));
00132 }
00133
00134 void handle_send_to(const boost::system::error_code& error)
00135 {
00136 if (!error)
00137 {
00138 /*timer_.expires_from_now(boost::posix_time::seconds(1));
00139 timer_.async_wait(
00140 boost::bind(&SNDServer::handle_timeout, this,
00141 boost::asio::placeholders::error));
00142 */
00143 }
00144 else
00145 {
00146 std::cout << "Send error\n";
00147 }
00148 }
00149
00150 void handle_timeout(const boost::system::error_code& error)
00151 {
00152 if (!error)
00153 {
00154 std::ostringstream os;
00155 std::cout << "Message " << message_count_ << "\n";
00156 os << "Message " << message_count_++;
00157 message_ = os.str();
00158
00159 socket_.async_send_to(
00160 boost::asio::buffer(message_), endpoint_,
00161 boost::bind(&SNDServer::handle_send_to, this,
00162 boost::asio::placeholders::error));
00163 }
00164 else
00165 {
00166 std::cout << "error\n";
00167 }
00168 }
00169 void sendPose(double x, double y, double z)
00170 {
00171 char request[3*sizeof(double)] ;
00172 union DblBuf db;
00173 db.d = x;
00174 for(int i = 0; i < sizeof(double);i++)
00175 request[i] = db.buf[i];
00176 db.d = y;
00177 for(int i = 0; i < sizeof(double);i++)
00178 request[i+sizeof(double)] = db.buf[i];
00179 db.d = z;
00180 for(int i = 0; i < sizeof(double);i++)
00181 request[i+2*sizeof(double)] = db.buf[i];
00182 size_t request_length = 3*sizeof(double);
00183
00184
00185 socket_.async_send_to(
00186 boost::asio::buffer(request, request_length), endpoint_,
00187 boost::bind(&SNDServer::handle_send_to, this,
00188 boost::asio::placeholders::error));
00189 }
00190
00191 void sendForce(double Fx, double Fy, double Fz, double Tx, double Ty, double Tz)
00192 {
00193 char request[6*sizeof(double)] ;
00194 union DblBuf db;
00195 db.d = Fx;
00196 for(int i = 0; i < sizeof(double);i++)
00197 request[i] = db.buf[i];
00198 db.d = Fy;
00199 for(int i = 0; i < sizeof(double);i++)
00200 request[i+sizeof(double)] = db.buf[i];
00201 db.d = Fz;
00202 for(int i = 0; i < sizeof(double);i++)
00203 request[i+2*sizeof(double)] = db.buf[i];
00204 db.d = Tx;
00205 for(int i = 0; i < sizeof(double);i++)
00206 request[i+3*sizeof(double)] = db.buf[i];
00207 db.d = Ty;
00208 for(int i = 0; i < sizeof(double);i++)
00209 request[i+4*sizeof(double)] = db.buf[i];
00210 db.d = Tz;
00211 for(int i = 0; i < sizeof(double);i++)
00212 request[i+5*sizeof(double)] = db.buf[i];
00213 size_t request_length = 6*sizeof(double);
00214
00215
00216 socket_.async_send_to(
00217 boost::asio::buffer(request, request_length), endpoint_,
00218 boost::bind(&SNDServer::handle_send_to, this,
00219 boost::asio::placeholders::error));
00220 }
00221
00222 private:
00223 boost::asio::ip::udp::endpoint endpoint_;
00224 boost::asio::ip::udp::socket socket_;
00225 boost::asio::deadline_timer timer_;
00226 int message_count_;
00227 std::string message_;
00228 };
00229
00230
00231 #endif
