client.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <shared_serial/client.h>
00003 
00004 #include <shared_serial/Connect.h>
00005 #include <shared_serial/Send.h>
00006 #include <shared_serial/SendTo.h>
00007 #include <shared_serial/Recv.h>
00008 #include <shared_serial/SendRecv.h>
00009 #include <shared_serial/Close.h>
00010 #include <shared_serial/Flush.h>
00011 
00012 void SerialClient::init(const char *path)
00013 {
00014   nh_ = ros::NodeHandle(path);
00015 
00016   send_topic_ = nh_.advertise<shared_serial::Send>("send", 10);
00017   close_topic_ = nh_.advertise<shared_serial::Close>("close", 1);
00018   flush_topic_ = nh_.advertise<shared_serial::Flush>("flush", 1);
00019 
00020   connect_service_ = nh_.serviceClient<shared_serial::Connect>("connect", true);
00021   sendto_service_ = nh_.serviceClient<shared_serial::SendTo>("sendto", true);
00022   recv_service_ = nh_.serviceClient<shared_serial::Recv>("recv", true);
00023   sendrecv_service_ = nh_.serviceClient<shared_serial::SendRecv>("sendrecv", true);
00024 
00025   connect_service_.waitForExistence();
00026   sendto_service_.waitForExistence();
00027   recv_service_.waitForExistence();
00028   sendrecv_service_.waitForExistence();
00029 }
00030 
00031 int SerialClient::connect(float timeout)
00032 {
00033   shared_serial::Connect srv;
00034 
00035   if (!connect_service_.call(srv))
00036     return -1;
00037 
00038   return srv.response.socket;
00039 }
00040 
00041 int SerialClient::sendto(int socket, const unsigned char *data, size_t length, float timeout)
00042 {
00043   shared_serial::SendTo srv;
00044 
00045   srv.request.socket = socket;
00046   srv.request.data.resize(length);
00047   for (size_t ii=0; ii < length; ++ii)
00048     srv.request.data[ii] = data[ii];
00049   srv.request.timeout = timeout;
00050 
00051   if (!sendto_service_.call(srv))
00052     return -1;
00053 
00054   return srv.response.socket;
00055 }
00056 
00057 int SerialClient::recv(int socket, int length, float recv_timeout, float sock_timeout, unsigned char *data, size_t *data_length)
00058 {
00059   shared_serial::Recv srv;
00060 
00061   srv.request.socket = socket;
00062   srv.request.length = length;
00063   srv.request.recv_timeout = recv_timeout;
00064   srv.request.sock_timeout = sock_timeout;
00065 
00066   if (!recv_service_.call(srv))
00067     return -1;
00068 
00069   *data_length = srv.response.data.size();
00070   for (size_t ii=0; ii < *data_length; ++ii)
00071     data[ii] = srv.response.data[ii];
00072 
00073   return srv.response.socket;
00074 }
00075 
00076 int SerialClient::sendrecv(int socket, const unsigned char *send_data, size_t send_length, size_t recv_length, float recv_timeout, float sock_timeout, unsigned char *recv_data, size_t *recv_data_length)
00077 {
00078   shared_serial::SendRecv srv;
00079 
00080   srv.request.socket = socket;
00081   srv.request.send_data.resize(send_length);
00082   for (size_t ii=0; ii < send_length; ++ii)
00083     srv.request.send_data[ii] = send_data[ii];
00084   srv.request.length = recv_length;
00085   srv.request.recv_timeout = recv_timeout;
00086   srv.request.sock_timeout = sock_timeout;
00087 
00088   if (!sendrecv_service_.call(srv))
00089     return -1;
00090 
00091   *recv_data_length = srv.response.recv_data.size();
00092   for (size_t ii=0; ii < *recv_data_length; ++ii)
00093     recv_data[ii] = srv.response.recv_data[ii];
00094 
00095   return srv.response.socket;
00096 }
00097 
00098 void SerialClient::send(int socket, const unsigned char *data, size_t length, float timeout)
00099 {
00100   shared_serial::Send msg;
00101 
00102   msg.socket = socket;
00103   msg.data.resize(length);
00104   for (size_t ii=0; ii < length; ++ii)
00105     msg.data[ii] = data[ii];
00106   msg.timeout = timeout;
00107 
00108   send_topic_.publish(msg);
00109 }
00110 
00111 void SerialClient::close(int socket)
00112 {
00113   shared_serial::Close msg;
00114 
00115   msg.socket = socket;
00116 
00117   close_topic_.publish(msg);
00118 }
00119 
00120 void SerialClient::flush(int socket, float timeout)
00121 {
00122   shared_serial::Flush msg;
00123 
00124   msg.socket = socket;
00125   msg.timeout = timeout;
00126 
00127   flush_topic_.publish(msg);
00128 }


shared_serial
Author(s): Wouter Caarls
autogenerated on Thu Jun 6 2019 19:47:36