Index: /trunk/shared_serial/CMakeLists.txt =================================================================== --- /trunk/shared_serial/CMakeLists.txt (revision 8) +++ /trunk/shared_serial/CMakeLists.txt (revision 8) @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +rosbuild_genmsg() +rosbuild_gensrv() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/client.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +rosbuild_add_executable(server + src/server.cpp + src/watchdog.cpp + src/LxSerial.cpp) +#target_link_libraries(example ${PROJECT_NAME}) Index: /trunk/shared_serial/Makefile =================================================================== --- /trunk/shared_serial/Makefile (revision 8) +++ /trunk/shared_serial/Makefile (revision 8) @@ -0,0 +1,1 @@ +include $(shell rospack find mk)/cmake.mk Index: /trunk/shared_serial/include/shared_serial/LxSerial.h =================================================================== --- /trunk/shared_serial/include/shared_serial/LxSerial.h (revision 8) +++ /trunk/shared_serial/include/shared_serial/LxSerial.h (revision 8) @@ -0,0 +1,105 @@ +//============================================================================ +// Name : LxSerial.cpp +// Author : Eelko van Breda,www.dbl.tudelft.nl +// Version : 0.1 +// Copyright : Copyright (c) 2008 LGPL +// Description : serial communicatin class linux +//============================================================================ + +#ifndef LXSERIAL_H_ +#define LXSERIAL_H_ +//#define __DBG__ + +#include /* fileio */ +#include /* terminal i/o system, talks to /dev/tty* ports */ +#include /* Read function */ +#include /* ioctl function */ +#include +#include +#include +#include + +#define INVALID_DEVICE_HANDLE -1 + +class LxSerial +{ + public: + enum PortType { RS232, // Normal RS232 + RS485_EXAR, // EXAR XR16C2850 + RS485_FTDI, // FTDI FT232RL in 485 mode + RS485_SMSC, // SMSC SCH311X RS-485 mode (Versalogic Sidewinder board) + TCP + }; + enum PortSpeed { S50 = B50, // Baudrate to use for the port --> see termios.h + S75 = B75, + S110 = B110, + S134 = B134, + S150 = B150, + S200 = B200, + S300 = B300, + S600 = B600, + S1200 = B1200, + S1800 = B1800, + S2400 = B2400, + S4800 = B4800, + S9600 = B9600, + S19200 = B19200, + S38400 = B38400, + S57600 = B57600, + S115200 = B115200, + S230400 = B230400, + S460800 = B460800, + S500000 = B500000, + S576000 = B576000, + S921600 = B921600, + S1000000 = B1000000, + S1152000 = B1152000, + S1500000 = B1500000, + S2000000 = B2000000, + S2500000 = B2500000, + S3000000 = B3000000, + S3500000 = B3500000, + S4000000 = B4000000 + }; + /*return values*/ + static const int READ_ERROR = -1; + static const int COLLISION_DETECT_ERROR = -2; + static const int ECHO_TIMEOUT_ERROR = -3; + + /*magic numbers*/ + static const int COLLISION_WAIT_TIME_USEC = 10000; // microseconds + static const int ECHO_WAIT_TIME_SEC = 1; // seconds + static const int ECHO_WAIT_TIME_USEC = 0; // microseconds + static const int WAIT_FOR_DATA_DSEC = 5; // + + protected: + int hPort; // file handle to the port + std::string s_port_name; // name of the port that was opened +// bool b_initialised; // + bool b_clear_echo; // read sended characters from Rx when true + bool b_rts; // this boolean must be set to enforce setting the RTS signal without hardware contol enabled + bool b_hw_flow_control; // + bool b_socket; + termios options, old_options; // + bool wait_for_input(int *seconds, int *microseconds); // private member function to wait for port. the time variables are modified after return to reflect the time not slept + void set_port_type(LxSerial::PortType port_type); + + public: + LxSerial(); + virtual ~LxSerial(); + virtual bool port_open(const std::string& portname, LxSerial::PortType port_type); // open serial port. If overridden, make sure you set s_port_name!! + virtual bool is_port_open(); + std::string& get_port_name(); + virtual bool set_speed(LxSerial::PortSpeed baudrate ); // enumerated + virtual bool set_speed_int(const int baudrate); // Set speed by integer value directly - UNPROTECTED! + void set_clear_echo(bool clear); // clear echoed charackters from input and detect collisions on write + virtual bool port_close(); + virtual int port_read(unsigned char* buffer, int numBytes) const; + virtual int port_read(unsigned char* buffer, int numBytes, int seconds, int microseconds); + virtual int port_write(unsigned char* buffer, int numBytes); + virtual void flush_buffer(); // flush input and output buffers + +}; + + +#endif /*LXSERIAL_H_*/ Index: /trunk/shared_serial/include/shared_serial/client.h =================================================================== --- /trunk/shared_serial/include/shared_serial/client.h (revision 8) +++ /trunk/shared_serial/include/shared_serial/client.h (revision 8) @@ -0,0 +1,30 @@ +#ifndef __SS_CLIENT_H_ +#define __SS_CLIENT_H_ + +#include + +class SerialClient +{ + protected: + ros::NodeHandle nh_; + ros::ServiceClient connect_service_; + ros::ServiceClient sendto_service_; + ros::ServiceClient recv_service_; + ros::ServiceClient sendrecv_service_; + ros::Publisher send_topic_; + ros::Publisher close_topic_; + ros::Publisher flush_topic_; + + public: + void init(const char *path="/comm"); + + int connect(float timeout); + int sendto(int socket, const unsigned char *data, size_t length, float timeout); + int recv(int socket, int length, float recv_timeout, float sock_timeout, unsigned char *data, size_t *data_length); + int 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); + void send(int socket, const unsigned char *data, size_t length, float timeout); + void close(int socket); + void flush(int socket, float timeout); +}; + +#endif /* __SS_CLIENT_H_ */ Index: /trunk/shared_serial/include/shared_serial/server.h =================================================================== --- /trunk/shared_serial/include/shared_serial/server.h (revision 8) +++ /trunk/shared_serial/include/shared_serial/server.h (revision 8) @@ -0,0 +1,85 @@ +#ifndef __SS_SERVER_H_ +#define __SS_SERVER_H + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +class SerialServerLock +{ + protected: + int socket_, last_socket_; + struct timeval timeout_; + pthread_mutex_t mutex_; + pthread_cond_t condition_; + + public: + SerialServerLock(); + + int lock(int socket, float timeout); + void unlock(); + void checkTimeout(); +}; + +class SerialServer +{ + protected: + ros::NodeHandle nh_; + ros::ServiceServer connect_service_; + ros::ServiceServer sendto_service_; + ros::ServiceServer recv_service_; + ros::ServiceServer sendrecv_service_; + ros::Subscriber send_topic_; + ros::Subscriber close_topic_; + ros::Subscriber flush_topic_; + + LxSerial serial_port_; + SerialServerLock lock_; + WatchdogThread watchdog_; + + public: + SerialServer() : nh_("~") + { + advertiseServices(); + readParameters(); + } + ~SerialServer() + { + nh_.shutdown(); + } + + void checkTimeout() + { + lock_.checkTimeout(); + } + + void kickWatchdog() + { + watchdog_.kick(); + } + + void advertiseServices(); + void readParameters(); + + void callbackSend(const shared_serial::Send::ConstPtr& msg); + void callbackClose(const shared_serial::Close::ConstPtr& msg); + void callbackFlush(const shared_serial::Flush::ConstPtr& msg); + bool callbackConnect(shared_serial::Connect::Request& req, shared_serial::Connect::Response& res); + bool callbackSendTo(shared_serial::SendTo::Request& req, shared_serial::SendTo::Response& res); + bool callbackRecv(shared_serial::Recv::Request& req, shared_serial::Recv::Response& res); + bool callbackSendRecv(shared_serial::SendRecv::Request& req, shared_serial::SendRecv::Response& res); +}; + +#endif /* __SS_SERVER_H */ Index: /trunk/shared_serial/include/shared_serial/watchdog.h =================================================================== --- /trunk/shared_serial/include/shared_serial/watchdog.h (revision 8) +++ /trunk/shared_serial/include/shared_serial/watchdog.h (revision 8) @@ -0,0 +1,30 @@ +#ifndef __SS_WATCHDOG_H +#define __SS_WATCHDOG_H + +#include + +class WatchdogThread +{ + protected: + pthread_t thread_; + pthread_mutex_t mutex_; + double interval_; + bool kicked_, set_; + + public: + WatchdogThread() : set_(false) { } + WatchdogThread(double interval) : set_(false) { set(interval); } + virtual ~WatchdogThread(); + + bool set(double interval); + bool kick(); + + protected: + virtual void watch(); + virtual void bark(); + + private: + static void *watchDelegate(void *obj); +}; + +#endif /* __SS_WATCHDOG_H */ Index: /trunk/shared_serial/launch/rs232usb.lauch =================================================================== --- /trunk/shared_serial/launch/rs232usb.lauch (revision 8) +++ /trunk/shared_serial/launch/rs232usb.lauch (revision 8) @@ -0,0 +1,8 @@ + + + + + + + + Index: /trunk/shared_serial/launch/start.launch =================================================================== --- /trunk/shared_serial/launch/start.launch (revision 8) +++ /trunk/shared_serial/launch/start.launch (revision 8) @@ -0,0 +1,7 @@ + + + + + + + Index: /trunk/shared_serial/mainpage.dox =================================================================== --- /trunk/shared_serial/mainpage.dox (revision 8) +++ /trunk/shared_serial/mainpage.dox (revision 8) @@ -0,0 +1,28 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b shared_serial implements locking serial communications services. + + + + +\section codeapi Code API + +The API is internal and not meant to be used. See the message and service documentation instead. + + + + +*/ Index: /trunk/shared_serial/manifest.xml =================================================================== --- /trunk/shared_serial/manifest.xml (revision 8) +++ /trunk/shared_serial/manifest.xml (revision 8) @@ -0,0 +1,17 @@ + + + + Shared serial port with locking functionality + + + Wouter Caarls + BSD + + http://ros.org/wiki/shared_serial + + + + + + + Index: /trunk/shared_serial/msg/Close.msg =================================================================== --- /trunk/shared_serial/msg/Close.msg (revision 8) +++ /trunk/shared_serial/msg/Close.msg (revision 8) @@ -0,0 +1,4 @@ +# Release serial port lock. + +# Socket identifier from an earlier communication. +uint32 socket Index: /trunk/shared_serial/msg/Flush.msg =================================================================== --- /trunk/shared_serial/msg/Flush.msg (revision 8) +++ /trunk/shared_serial/msg/Flush.msg (revision 8) @@ -0,0 +1,8 @@ +# Flush serial port. + +# Eeither 0 (connectionless) or an unexpired socket identifier +# from an earlier communication. +uint32 socket + +# Number of seconds to keep the port locked after this communication. +float32 timeout Index: /trunk/shared_serial/msg/Send.msg =================================================================== --- /trunk/shared_serial/msg/Send.msg (revision 8) +++ /trunk/shared_serial/msg/Send.msg (revision 8) @@ -0,0 +1,11 @@ +# Send data to serial port. + +# Eeither 0 (connectionless) or an unexpired socket identifier +# from an earlier communication. +uint32 socket + +# Data to be sent. +uint8[] data + +# Number of seconds to keep the port locked after this communication. +float32 timeout Index: /trunk/shared_serial/src/LxSerial.cpp =================================================================== --- /trunk/shared_serial/src/LxSerial.cpp (revision 8) +++ /trunk/shared_serial/src/LxSerial.cpp (revision 8) @@ -0,0 +1,462 @@ +//============================================================================ +// Name : LxSerial.cpp +// Author : Eelko van Breda,www.dbl.tudelft.nl +// Version : 0.1 +// Copyright : Copyright (c) 2008 LGPL +// Description : serial communicatin class linux +//============================================================================ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "shared_serial/LxSerial.h" + + +/* constructor */ +LxSerial::LxSerial() +{ + hPort = INVALID_DEVICE_HANDLE; +// b_initialised = false; + b_clear_echo = false; + b_hw_flow_control = false; + b_socket = false; +} + +/* return name of the port that was opened */ +std::string& LxSerial::get_port_name() +{ + return s_port_name; +} + +void LxSerial::set_port_type(LxSerial::PortType port_type) +{ + switch (port_type) + { + case RS232: + b_rts = false; + break; + + case RS485_FTDI: + b_rts = false; + break; + + case RS485_EXAR: + b_rts = true; + b_clear_echo = true; + break; + + case RS485_SMSC: + b_rts = false; + b_clear_echo = true; + break; + + case TCP: + b_socket = true; + b_rts = false; + b_clear_echo = false; + break; + + default: + perror("LxSerial: no port type specified"); + break; + } +} + +/* open port */ +bool LxSerial::port_open(const std::string& portname, LxSerial::PortType port_type) +{ + // Set port type + set_port_type(port_type); + + if (b_socket) + { + int optval = 1; + + struct sockaddr_in addr; + struct hostent *server; + + hPort = socket(AF_INET, SOCK_STREAM, 0); + setsockopt(hPort, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); + + bzero((char *) &addr, sizeof(addr)); + addr.sin_family = AF_INET; + + std::string host_name = portname.substr(0, portname.find(':')); + std::string port_number = portname.substr(portname.find(':')+1); + + server = gethostbyname(host_name.c_str()); + if (!server) + { + perror("unknown remote serial host name"); + return false; + } + + bcopy((char *)server->h_addr, (char *)&addr.sin_addr.s_addr, server->h_length); + addr.sin_port = htons(atoi(port_number.c_str())); + + if (connect(hPort,(struct sockaddr *) &addr,sizeof(addr)) < 0) + { + perror("error connecting to remote serial host"); + return false; + } + } + else + { + // Open port + hPort = open(portname.c_str(), O_RDWR | O_NOCTTY);//|O_NDELAY); // open the serial device + // O_RDWR = open read an write + // The O_NOCTTY flag tells UNIX that this program doesn't want to be the "controlling terminal" for that  + // port. If you don't specify this then any input (such as keyboard abort signals and so forth) will affect  + // your process. Programs like getty(1M/8) use this feature when starting the login process, but normally a  + // user program does not want this behavior. + // The O_NDELAY flag tells UNIX that this program doesn't care what state the DCD signal line is in ­  + // whether the other end of the port is up and running. If you do not specify this flag, your process will be  + // put to sleep until the DCD signal line is the space voltage. + + if (hPort < 0) { //check if port opened correctly + perror(" Could not open serial port, aborting"); + return false; + } + + + tcgetattr(hPort, &options); // get the current termios (com port options struct) from the kernel + tcgetattr(hPort, &old_options); // get the current termios copy to restore on close + + cfsetispeed(&options, B115200); // set incomming baudrate to standard 9600, this can be changed with the function set_speed() + cfsetospeed(&options, B115200); // set outgoing baudrate to standard 9600 + + // c_cflag Control Options + // |= (..) means enabled + // &= ~(..) means not enabled + options.c_cflag |= (CLOCAL|CREAD|CS8); // CREAD = enanble receiver + // CLOCAL = Local Line, do not change owner of port + // CS8 = 8 databits + options.c_cflag &= ~(CRTSCTS|PARENB|CSTOPB); // Disable HW flow control + // no paritybit, PARENB = paritybit + // 1 stop bit, CSTOPB = 2 stop bits (1 otherwise) + if (b_hw_flow_control){ + options.c_cflag |= (CRTSCTS); // Enable HW flow control + } + + // c_lflag Line Options ( controls how input charackters are handeled by the serial driver + options.c_lflag &= ~(ECHO|ECHONL|ECHOE|ICANON|ISIG|IEXTEN); +// options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN); + // ~(..) means not enabled + // ECHO echoing of input characters + // ECHOE echo erease character as BS-SP-BS + // ECHONL mimics echo but doesn't move to next line when it ends + // ICANON cononical input = line oriented input, else RAW + // ISIG ENABLE SIGINTR, SIGSUSP, SIGDSUSP and SIGQUIT signals + // IEXTERN = enable extended functions + + // c_iflag Input Options + options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON|IXOFF); +// options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); + // ~(..) means not enabled + // IGNBRK ignore break condition + // BRKINT send a SIGINT when a break condition is detected + // PARMRK Mark parity errors + // ISTRIP strip parity bits + // INLCR map new line to CR + // IGNCR ignore CR + // ICRNL map CR to new line + // IXON Enable software flow control (outgoing) + // IXOFF Enable software flow control (incomming) + + // c_oflag output options + options.c_oflag &= ~(OPOST); + // ~(..) means not enabled + // OPOST postprocess output (if not set RAW output) + // when OPOST is disabled all other other option bits of c-Oflag are ignored + + // c_cc control character array (control caracter options and timeouts + // Timeout 0.005 sec for first byte, read minimum of 0 bytes + // timeouts are ignored in cononical input or when NDELAY option is set. + options.c_cc[VMIN] = 0; // VMIN Minimum number of characters to read + options.c_cc[VTIME] = WAIT_FOR_DATA_DSEC; // Time to wait for data (tenths of seconds) + +// fcntl(hPort, F_SETFL, FNDELAY); // don't wait for input when reading data + + if (b_rts){ // if we are using RS_485_EXAR type, we clear the RTS signal + int msc = TIOCM_RTS; + ioctl(hPort, TIOCMBIC, &msc); // Clear RTS signal + usleep(100); // WAIT FOR SIGNAL TO BE CLEARD + } + + + + if (tcsetattr(hPort,TCSANOW, &options)!=0){ // Set the new options for the port now + perror("Error: Could not set serial port settings"); + return false; + } + + usleep(100); // additional wait for correct functionality + tcflush(hPort, TCIOFLUSH); // flush terminal data + } + + // Save port name + s_port_name = portname; + + return true; +} + +bool LxSerial::is_port_open() +{ + return hPort >= 0; +} + +// speedcontrol */ +bool LxSerial::set_speed( LxSerial::PortSpeed baudrate ) +{ + if (b_socket) + return false; + + cfsetispeed(&options, baudrate); //set incoming baud rate + cfsetospeed(&options, baudrate); //set outgoing baud rate + + if (tcsetattr(hPort,TCSANOW, &options)!=0){ // Set the new options for the port now + perror("Error: Could not set serial port baudrate"); + return false; + } + usleep(100); // additional wait for correct functionality + tcflush(hPort, TCIOFLUSH); // flush terminal data + return true; +} + +bool LxSerial::set_speed_int(const int baudrate) +{ + LxSerial::PortSpeed baud; + // Baud rate conversion from index to real baud rate :( + switch (baudrate) + { + case 4000000: baud = S4000000; break; + case 3500000: baud = S3500000; break; + case 3000000: baud = S3000000; break; + case 2500000: baud = S2500000; break; + case 2000000: baud = S2000000; break; + case 1500000: baud = S1500000; break; + case 1152000: baud = S1152000; break; + case 1000000: baud = S1000000; break; + case 921600: baud = S921600; break; + case 576000: baud = S576000; break; + case 500000: baud = S500000; break; + case 460800: baud = S460800; break; + case 230400: baud = S230400; break; + case 115200: baud = S115200; break; + case 57600: baud = S57600; break; + case 38400: baud = S38400; break; + case 19200: baud = S19200; break; + case 9600: baud = S9600; break; + case 4800: baud = S4800; break; + case 2400: baud = S2400; break; + case 1800: baud = S1800; break; + case 1200: baud = S1200; break; + case 600: baud = S600; break; + case 300: baud = S300; break; + case 200: baud = S200; break; + case 150: baud = S150; break; + case 134: baud = S134; break; + case 110: baud = S110; break; + case 75: baud = S75; break; + case 50: baud = S50; break; + default: + printf("This is not a legal portspeed!\n"); + return false; + } + + set_speed(baud); + return true; +} + +// clear echoed characters from input and detect collisions on write, use for EXAR RS485 +void LxSerial::set_clear_echo(bool clear) +{ + b_clear_echo = clear; +} + +// close the serial port +bool LxSerial::port_close() +{ + if (hPort==INVALID_DEVICE_HANDLE) + return true; + + if (!b_socket) + { + if (tcsetattr(hPort,TCSANOW, &old_options)!=0) { // restore the old port settings + perror("Warning: Could not restore serial port settings."); + } + } + + if(close(hPort) == -1) { //close serial port + perror("Error: Could not close serial port."); + return false; + } + hPort = INVALID_DEVICE_HANDLE; + return true; +} + +int LxSerial::port_read(unsigned char* buffer, int numBytes) const +{ + int nBytesRead = read(hPort, buffer, numBytes); + + #ifdef __DBG__ + printf("read "); + for (int i=0;i +#include + +#include +#include +#include +#include +#include +#include +#include + +void SerialClient::init(const char *path) +{ + nh_ = ros::NodeHandle(path); + + send_topic_ = nh_.advertise("send", 10); + close_topic_ = nh_.advertise("close", 1); + flush_topic_ = nh_.advertise("flush", 1); + + connect_service_ = nh_.serviceClient("connect", true); + sendto_service_ = nh_.serviceClient("sendto", true); + recv_service_ = nh_.serviceClient("recv", true); + sendrecv_service_ = nh_.serviceClient("sendrecv", true); + + connect_service_.waitForExistence(); + sendto_service_.waitForExistence(); + recv_service_.waitForExistence(); + sendrecv_service_.waitForExistence(); +} + +int SerialClient::connect(float timeout) +{ + shared_serial::Connect srv; + + if (!connect_service_.call(srv)) + return -1; + + return srv.response.socket; +} + +int SerialClient::sendto(int socket, const unsigned char *data, size_t length, float timeout) +{ + shared_serial::SendTo srv; + + srv.request.socket = socket; + srv.request.data.resize(length); + for (size_t ii=0; ii < length; ++ii) + srv.request.data[ii] = data[ii]; + srv.request.timeout = timeout; + + if (!sendto_service_.call(srv)) + return -1; + + return srv.response.socket; +} + +int SerialClient::recv(int socket, int length, float recv_timeout, float sock_timeout, unsigned char *data, size_t *data_length) +{ + shared_serial::Recv srv; + + srv.request.socket = socket; + srv.request.length = length; + srv.request.recv_timeout = recv_timeout; + srv.request.sock_timeout = sock_timeout; + + if (!recv_service_.call(srv)) + return -1; + + *data_length = srv.response.data.size(); + for (size_t ii=0; ii < *data_length; ++ii) + data[ii] = srv.response.data[ii]; + + return srv.response.socket; +} + +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) +{ + shared_serial::SendRecv srv; + + srv.request.socket = socket; + srv.request.send_data.resize(send_length); + for (size_t ii=0; ii < send_length; ++ii) + srv.request.send_data[ii] = send_data[ii]; + srv.request.length = recv_length; + srv.request.recv_timeout = recv_timeout; + srv.request.sock_timeout = sock_timeout; + + if (!sendrecv_service_.call(srv)) + return -1; + + *recv_data_length = srv.response.recv_data.size(); + for (size_t ii=0; ii < *recv_data_length; ++ii) + recv_data[ii] = srv.response.recv_data[ii]; + + return srv.response.socket; +} + +void SerialClient::send(int socket, const unsigned char *data, size_t length, float timeout) +{ + shared_serial::Send msg; + + msg.socket = socket; + msg.data.resize(length); + for (size_t ii=0; ii < length; ++ii) + msg.data[ii] = data[ii]; + msg.timeout = timeout; + + send_topic_.publish(msg); +} + +void SerialClient::close(int socket) +{ + shared_serial::Close msg; + + msg.socket = socket; + + close_topic_.publish(msg); +} + +void SerialClient::flush(int socket, float timeout) +{ + shared_serial::Flush msg; + + msg.socket = socket; + msg.timeout = timeout; + + flush_topic_.publish(msg); +} Index: /trunk/shared_serial/src/server.cpp =================================================================== --- /trunk/shared_serial/src/server.cpp (revision 8) +++ /trunk/shared_serial/src/server.cpp (revision 8) @@ -0,0 +1,345 @@ +#include +#include +#include + +SerialServerLock::SerialServerLock() : + socket_(0), last_socket_(0) +{ + pthread_mutex_init(&mutex_, NULL); + pthread_cond_init(&condition_, NULL); +} + +void SerialServerLock::checkTimeout() +{ + pthread_mutex_lock(&mutex_); + + struct timeval tv; + gettimeofday(&tv, NULL); + + // Check timeout + if (socket_ && (tv.tv_sec > timeout_.tv_sec || + (tv.tv_sec == timeout_.tv_sec && tv.tv_usec > timeout_.tv_usec))) + { + ROS_DEBUG_STREAM("Lock " << socket_ << " expired"); + socket_ = 0; + + pthread_cond_signal(&condition_); + } + + pthread_mutex_unlock(&mutex_); +} + +int SerialServerLock::lock(int socket, float timeout) +{ + pthread_mutex_lock(&mutex_); + + // Check validity + if (socket && socket != socket_) + { + ROS_ERROR_STREAM("Unknown lock " << socket); + pthread_mutex_unlock(&mutex_); + return -1; + } + + if (!socket) + { + // Wait for port to become free + while (socket_) + { + ROS_DEBUG_STREAM("Waiting for lock"); + pthread_cond_wait(&condition_, &mutex_); + } + + // New connection + socket_ = last_socket_ = std::max(last_socket_+1, 1); + + ROS_DEBUG_STREAM("New lock " << socket_); + } + + if (timeout) + { + // Set timeout + struct timeval tv; + gettimeofday(&tv, NULL); + + tv.tv_sec += (int)timeout; + tv.tv_usec += (int)((timeout-(int)timeout)*1000000); + if (tv.tv_usec > 1000000) + { + tv.tv_sec++; + tv.tv_usec -= 1000000; + } + timeout_ = tv; + + ROS_DEBUG_STREAM("Lock " << socket_ << " will expire after " << timeout << " seconds"); + } + else + { + ROS_DEBUG_STREAM("Lock " << socket_ << " expired"); + + timeout_.tv_sec = timeout_.tv_usec = socket_ = 0; + } + + return socket_; +} + +void SerialServerLock::unlock() +{ + if (!socket_) + pthread_cond_signal(&condition_); + pthread_mutex_unlock(&mutex_); +} + +void SerialServer::advertiseServices() +{ + ROS_INFO("Registering services"); + + send_topic_ = nh_.subscribe("send", 10, &SerialServer::callbackSend, this); + close_topic_ = nh_.subscribe("close", 1, &SerialServer::callbackClose, this); + flush_topic_ = nh_.subscribe("flush", 1, &SerialServer::callbackFlush, this); + connect_service_ = nh_.advertiseService("connect", &SerialServer::callbackConnect, this); + sendto_service_ = nh_.advertiseService("sendto", &SerialServer::callbackSendTo, this); + recv_service_ = nh_.advertiseService("recv", &SerialServer::callbackRecv, this); + sendrecv_service_ = nh_.advertiseService("sendrecv", &SerialServer::callbackSendRecv, this); +} + +void SerialServer::readParameters() +{ + std::string port_name, port_type; + LxSerial::PortType lx_port_type; + int baud_rate; + double watchdog_interval; + + ROS_INFO("Reading parameters"); + + ROS_ASSERT(nh_.getParam("port_name", port_name)); + ROS_ASSERT(nh_.getParam("port_type", port_type)); + ROS_ASSERT(nh_.getParam("baud_rate", baud_rate)); + nh_.param("watchdog_interval", watchdog_interval, 1); + + if (port_type == "RS232") + lx_port_type = LxSerial::RS232; + else if (port_type == "RS485_FTDI") + lx_port_type = LxSerial::RS485_FTDI; + else if (port_type == "RS485_EXAR") + lx_port_type = LxSerial::RS485_EXAR; + else if (port_type == "RS485_SMSC") + lx_port_type = LxSerial::RS485_SMSC; + else if (port_type == "TCP") + lx_port_type = LxSerial::TCP; + else + { + ROS_FATAL_STREAM("Unknown port type " << port_type); + ROS_BREAK(); + return; + } + + ROS_ASSERT(serial_port_.port_open(port_name, lx_port_type)); + ROS_ASSERT(serial_port_.set_speed_int(baud_rate)); + ROS_ASSERT(watchdog_.set(watchdog_interval)); +} + +void SerialServer::callbackSend(const shared_serial::Send::ConstPtr& msg) +{ + int socket = lock_.lock(msg->socket, msg->timeout); + if (socket < 0) return; + + unsigned char *buf = new unsigned char[msg->data.size()]; + for (unsigned int ii=0; ii != msg->data.size(); ++ii) + buf[ii] = msg->data[ii]; + + int n = serial_port_.port_write(buf, msg->data.size()); + delete buf; + + if (n != (int)msg->data.size()) + { + ROS_ERROR("Truncated send, flushing port"); + serial_port_.flush_buffer(); + + lock_.unlock(); + return; + } + + ROS_DEBUG_STREAM("Sent " << n << " bytes"); + + lock_.unlock(); + return; +} + +void SerialServer::callbackClose(const shared_serial::Close::ConstPtr& msg) +{ + int socket = lock_.lock(msg->socket, 0); + if (socket < 0) return; + + lock_.unlock(); + return; +} + +void SerialServer::callbackFlush(const shared_serial::Flush::ConstPtr& msg) +{ + int socket = lock_.lock(msg->socket, msg->timeout); + if (socket < 0) return; + + serial_port_.flush_buffer(); + usleep(10); + serial_port_.flush_buffer(); + + ROS_INFO_STREAM("Flushed port"); + + lock_.unlock(); + return; +} + +bool SerialServer::callbackConnect(shared_serial::Connect::Request& req, shared_serial::Connect::Response& res) +{ + int socket = lock_.lock(0, req.timeout); + if (socket < 0) return false; + + res.socket = socket; + + lock_.unlock(); + return true; +} + +bool SerialServer::callbackSendTo(shared_serial::SendTo::Request& req, shared_serial::SendTo::Response& res) +{ + int socket = lock_.lock(req.socket, req.timeout); + if (socket < 0) return false; + + unsigned char *buf = new unsigned char[req.data.size()]; + for (unsigned int ii=0; ii != req.data.size(); ++ii) + buf[ii] = req.data[ii]; + + int n = serial_port_.port_write(buf, req.data.size()); + delete buf; + + if (n != (int)req.data.size()) + { + ROS_ERROR("Truncated send, flushing port"); + serial_port_.flush_buffer(); + + lock_.unlock(); + return false; + } + + ROS_DEBUG_STREAM("Sent " << n << " bytes"); + + res.socket = socket; + + lock_.unlock(); + return true; +} + +bool SerialServer::callbackRecv(shared_serial::Recv::Request& req, shared_serial::Recv::Response& res) +{ + int socket = lock_.lock(req.socket, req.sock_timeout); + if (socket < 0) return false; + + ROS_ASSERT(req.length <= 65536); + + unsigned char *buf = new unsigned char[req.length]; + int n = serial_port_.port_read(buf, req.length, (int)req.recv_timeout, (int)((req.recv_timeout-(int)req.recv_timeout)*1000000)); + + if (n < 0) + { + ROS_ERROR_STREAM("Error " << n << " while reading serial port"); + delete buf; + + serial_port_.flush_buffer(); + usleep(10); + serial_port_.flush_buffer(); + + lock_.unlock(); + return false; + } + + ROS_DEBUG_STREAM("Read " << n << " bytes"); + + res.data.resize(n); + for (int ii=0; ii != n; ++ii) + res.data[ii] = buf[ii]; + delete buf; + + res.socket = socket; + + lock_.unlock(); + return true; +} + +bool SerialServer::callbackSendRecv(shared_serial::SendRecv::Request& req, shared_serial::SendRecv::Response& res) +{ + int socket = lock_.lock(req.socket, req.sock_timeout); + if (socket < 0) return false; + + // *** Send *** + unsigned char *buf = new unsigned char[req.send_data.size()]; + for (unsigned int ii=0; ii != req.send_data.size(); ++ii) + buf[ii] = req.send_data[ii]; + + int n = serial_port_.port_write(buf, req.send_data.size()); + delete buf; + + if (n != (int)req.send_data.size()) + { + ROS_ERROR("Truncated send, flushing port"); + serial_port_.flush_buffer(); + + lock_.unlock(); + return false; + } + + ROS_DEBUG_STREAM("Sent " << n << " bytes"); + + // *** Receive *** + ROS_ASSERT(req.length <= 65536); + + buf = new unsigned char[req.length]; + n = serial_port_.port_read(buf, req.length, (int)req.recv_timeout, (int)((req.recv_timeout-(int)req.recv_timeout)*1000000)); + + if (n < 0) + { + ROS_ERROR("Error while reading serial port"); + delete buf; + + serial_port_.flush_buffer(); + usleep(10); + serial_port_.flush_buffer(); + + lock_.unlock(); + return false; + } + + ROS_DEBUG_STREAM("Read " << n << " bytes"); + + res.recv_data.resize(n); + for (int ii=0; ii != n; ++ii) + res.recv_data[ii] = buf[ii]; + delete buf; + + res.socket = socket; + + lock_.unlock(); + return true; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "shared_serial_server"); + + SerialServer serialServer; + + ROS_INFO("Spinning"); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + ros::Rate loop_rate(10); + while (ros::ok()) + { + serialServer.checkTimeout(); + serialServer.kickWatchdog(); + loop_rate.sleep(); + } + + return 0; +} Index: /trunk/shared_serial/src/watchdog.cpp =================================================================== --- /trunk/shared_serial/src/watchdog.cpp (revision 8) +++ /trunk/shared_serial/src/watchdog.cpp (revision 8) @@ -0,0 +1,65 @@ +#include +#include + +bool WatchdogThread::set(double interval) +{ + if (set_) + return false; + + interval_ = interval; + pthread_mutex_init(&mutex_, NULL); + pthread_create(&thread_, NULL, WatchdogThread::watchDelegate, this); + return set_ = true; +} + +WatchdogThread::~WatchdogThread() +{ + if (!set_) + return; + + pthread_cancel(thread_); + pthread_join(thread_, NULL); + pthread_mutex_destroy(&mutex_); +} + +void *WatchdogThread::watchDelegate(void *obj) +{ + WatchdogThread *wd = static_cast(obj); + wd->watch(); + return NULL; +} + +void WatchdogThread::watch() +{ + kicked_ = true; + + ros::Rate loop_rate(1./interval_); + while (ros::ok()) + { + pthread_mutex_lock(&mutex_); + if (!kicked_) + bark(); + kicked_ = false; + pthread_mutex_unlock(&mutex_); + loop_rate.sleep(); + pthread_testcancel(); + } +} + +bool WatchdogThread::kick() +{ + if (!set_) + return false; + + pthread_mutex_lock(&mutex_); + kicked_ = true; + pthread_mutex_unlock(&mutex_); + + return true; +} + +void WatchdogThread::bark() +{ + ROS_FATAL("Watchdog timed out"); + ROS_ISSUE_BREAK(); +} Index: /trunk/shared_serial/srv/Connect.srv =================================================================== --- /trunk/shared_serial/srv/Connect.srv (revision 8) +++ /trunk/shared_serial/srv/Connect.srv (revision 8) @@ -0,0 +1,9 @@ +# Lock the serial port. + +# Number of seconds to lock the port. +float32 timeout + +--- + +# Socket identifier for further communications. +int32 socket Index: /trunk/shared_serial/srv/Recv.srv =================================================================== --- /trunk/shared_serial/srv/Recv.srv (revision 8) +++ /trunk/shared_serial/srv/Recv.srv (revision 8) @@ -0,0 +1,22 @@ +# Receive data from serial port. + +# Either 0 (connectionless) or an unexpired socket identifier +# from an earlier communication. +uint32 socket + +# Number of bytes to receive. +uint32 length + +# Number of seconds to wait for data. +float32 recv_timeout + +# Number of seconds to keep the port locked after this communication. +float32 sock_timeout + +--- + +# Socket identifier for further communications. +uint32 socket + +# Received data. +uint8[] data Index: /trunk/shared_serial/srv/SendRecv.srv =================================================================== --- /trunk/shared_serial/srv/SendRecv.srv (revision 8) +++ /trunk/shared_serial/srv/SendRecv.srv (revision 8) @@ -0,0 +1,25 @@ +# Send data to serial port and wait for reply. + +# Either 0 (connectionless) or an unexpired socket identifier +# from an earlier communication. +uint32 socket + +# Data to send +uint8[] send_data + +# Number of bytes to receive. +uint32 length + +# Number of seconds to wait for data. +float32 recv_timeout + +# Number of seconds to keep the port locked after this communication. +float32 sock_timeout + +--- + +# Socket identifier for further communications. +uint32 socket + +# Received data. +uint8[] recv_data Index: /trunk/shared_serial/srv/SendTo.srv =================================================================== --- /trunk/shared_serial/srv/SendTo.srv (revision 8) +++ /trunk/shared_serial/srv/SendTo.srv (revision 8) @@ -0,0 +1,16 @@ +# Send data to serial port. + +# Either 0 (connectionless) or an unexpired socket identifier +# from an earlier communication. +uint32 socket + +# Data to be sent. +uint8[] data + +# Number of seconds to keep the port locked after this communication. +float32 timeout + +--- + +# Socket identifier for further communications. +uint32 socket Index: /trunk/shared_serial/stack.xml =================================================================== --- /trunk/shared_serial/stack.xml (revision 8) +++ /trunk/shared_serial/stack.xml (revision 8) @@ -0,0 +1,9 @@ + + Shared serial port with locking functionality + Wouter Caarls + BSD + + http://ros.org/wiki/shared_serial + + +