source: trunk/shared_serial/include/shared_serial/server.h @ 48

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

Imported shared_serial at revision 2117

File size: 2.1 KB
Line 
1#ifndef __SS_SERVER_H_
2#define __SS_SERVER_H
3
4#include <stdint.h>
5#include <sys/time.h>
6#include <pthread.h>
7
8#include <ros/ros.h>
9#include <shared_serial/LxSerial.h>
10#include <shared_serial/watchdog.h>
11
12#include <shared_serial/Connect.h>
13#include <shared_serial/Send.h>
14#include <shared_serial/SendTo.h>
15#include <shared_serial/Recv.h>
16#include <shared_serial/SendRecv.h>
17#include <shared_serial/Close.h>
18#include <shared_serial/Flush.h>
19
20class SerialServerLock
21{
22  protected:
23    int socket_, last_socket_;
24    struct timeval timeout_;
25    pthread_mutex_t mutex_;
26    pthread_cond_t condition_;
27   
28  public:
29    SerialServerLock();
30   
31    int lock(int socket, float timeout);
32    void unlock();
33    void checkTimeout();
34};
35
36class SerialServer
37{
38  protected:
39    ros::NodeHandle nh_;
40    ros::ServiceServer connect_service_;
41    ros::ServiceServer sendto_service_;
42    ros::ServiceServer recv_service_;
43    ros::ServiceServer sendrecv_service_;
44    ros::Subscriber    send_topic_;
45    ros::Subscriber    close_topic_;
46    ros::Subscriber    flush_topic_;
47   
48    LxSerial serial_port_;
49    SerialServerLock lock_;
50    WatchdogThread watchdog_;
51
52  public:
53    SerialServer() : nh_("~")
54    {
55      advertiseServices();
56      readParameters();
57    }
58    ~SerialServer()
59    {
60      nh_.shutdown();
61    }
62   
63    void checkTimeout()
64    {
65      lock_.checkTimeout();
66    }
67   
68    void kickWatchdog()
69    {
70      watchdog_.kick();
71    }
72   
73    void advertiseServices();
74    void readParameters();
75   
76    void callbackSend(const shared_serial::Send::ConstPtr& msg);
77    void callbackClose(const shared_serial::Close::ConstPtr& msg);
78    void callbackFlush(const shared_serial::Flush::ConstPtr& msg);
79    bool callbackConnect(shared_serial::Connect::Request& req, shared_serial::Connect::Response& res);
80    bool callbackSendTo(shared_serial::SendTo::Request& req, shared_serial::SendTo::Response& res);
81    bool callbackRecv(shared_serial::Recv::Request& req, shared_serial::Recv::Response& res);
82    bool callbackSendRecv(shared_serial::SendRecv::Request& req, shared_serial::SendRecv::Response& res);
83};
84
85#endif /* __SS_SERVER_H */
Note: See TracBrowser for help on using the repository browser.