source: trunk/shared_serial/src/client.cpp

Last change on this file was 49, checked in by wcaarls, 11 years ago

Reinstated rosbuild shared_serial (catkin version is now on github)

File size: 3.4 KB
RevLine 
[8]1#include <ros/ros.h>
2#include <shared_serial/client.h>
3
4#include <shared_serial/Connect.h>
5#include <shared_serial/Send.h>
6#include <shared_serial/SendTo.h>
7#include <shared_serial/Recv.h>
8#include <shared_serial/SendRecv.h>
9#include <shared_serial/Close.h>
10#include <shared_serial/Flush.h>
11
12void SerialClient::init(const char *path)
13{
14  nh_ = ros::NodeHandle(path);
15
16  send_topic_ = nh_.advertise<shared_serial::Send>("send", 10);
17  close_topic_ = nh_.advertise<shared_serial::Close>("close", 1);
18  flush_topic_ = nh_.advertise<shared_serial::Flush>("flush", 1);
19
20  connect_service_ = nh_.serviceClient<shared_serial::Connect>("connect", true);
21  sendto_service_ = nh_.serviceClient<shared_serial::SendTo>("sendto", true);
22  recv_service_ = nh_.serviceClient<shared_serial::Recv>("recv", true);
23  sendrecv_service_ = nh_.serviceClient<shared_serial::SendRecv>("sendrecv", true);
24
25  connect_service_.waitForExistence();
26  sendto_service_.waitForExistence();
27  recv_service_.waitForExistence();
28  sendrecv_service_.waitForExistence();
29}
30
31int SerialClient::connect(float timeout)
32{
33  shared_serial::Connect srv;
34
35  if (!connect_service_.call(srv))
36    return -1;
37
38  return srv.response.socket;
39}
40
41int SerialClient::sendto(int socket, const unsigned char *data, size_t length, float timeout)
42{
43  shared_serial::SendTo srv;
44
45  srv.request.socket = socket;
46  srv.request.data.resize(length);
47  for (size_t ii=0; ii < length; ++ii)
48    srv.request.data[ii] = data[ii];
49  srv.request.timeout = timeout;
50
51  if (!sendto_service_.call(srv))
52    return -1;
53
54  return srv.response.socket;
55}
56
57int SerialClient::recv(int socket, int length, float recv_timeout, float sock_timeout, unsigned char *data, size_t *data_length)
58{
59  shared_serial::Recv srv;
60
61  srv.request.socket = socket;
62  srv.request.length = length;
63  srv.request.recv_timeout = recv_timeout;
64  srv.request.sock_timeout = sock_timeout;
65
66  if (!recv_service_.call(srv))
67    return -1;
68
69  *data_length = srv.response.data.size();
70  for (size_t ii=0; ii < *data_length; ++ii)
71    data[ii] = srv.response.data[ii];
72
73  return srv.response.socket;
74}
75
76int 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)
77{
78  shared_serial::SendRecv srv;
79
80  srv.request.socket = socket;
81  srv.request.send_data.resize(send_length);
82  for (size_t ii=0; ii < send_length; ++ii)
83    srv.request.send_data[ii] = send_data[ii];
84  srv.request.length = recv_length;
85  srv.request.recv_timeout = recv_timeout;
86  srv.request.sock_timeout = sock_timeout;
87
88  if (!sendrecv_service_.call(srv))
89    return -1;
90
91  *recv_data_length = srv.response.recv_data.size();
92  for (size_t ii=0; ii < *recv_data_length; ++ii)
93    recv_data[ii] = srv.response.recv_data[ii];
94
95  return srv.response.socket;
96}
97
98void SerialClient::send(int socket, const unsigned char *data, size_t length, float timeout)
99{
100  shared_serial::Send msg;
101
102  msg.socket = socket;
103  msg.data.resize(length);
104  for (size_t ii=0; ii < length; ++ii)
105    msg.data[ii] = data[ii];
106  msg.timeout = timeout;
107
108  send_topic_.publish(msg);
109}
110
111void SerialClient::close(int socket)
112{
113  shared_serial::Close msg;
114
115  msg.socket = socket;
116
117  close_topic_.publish(msg);
118}
119
120void SerialClient::flush(int socket, float timeout)
121{
122  shared_serial::Flush msg;
123
124  msg.socket = socket;
125  msg.timeout = timeout;
126
127  flush_topic_.publish(msg);
128}
Note: See TracBrowser for help on using the repository browser.