Index: tags/distros/fuerte/shared_serial/CMakeLists.txt
===================================================================
--- tags/distros/fuerte/shared_serial/CMakeLists.txt	(revision 26)
+++ tags/distros/fuerte/shared_serial/CMakeLists.txt	(revision 26)
@@ -0,0 +1,33 @@
+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})
+
+rosbuild_make_distribution(0.1.0)
Index: tags/distros/fuerte/shared_serial/Makefile
===================================================================
--- tags/distros/fuerte/shared_serial/Makefile	(revision 26)
+++ tags/distros/fuerte/shared_serial/Makefile	(revision 26)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: tags/distros/fuerte/shared_serial/include/shared_serial/LxSerial.h
===================================================================
--- tags/distros/fuerte/shared_serial/include/shared_serial/LxSerial.h	(revision 26)
+++ tags/distros/fuerte/shared_serial/include/shared_serial/LxSerial.h	(revision 26)
@@ -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 <fcntl.h>																/* fileio */
+#include <termios.h>   															/* terminal i/o system, talks to /dev/tty* ports  */
+#include <unistd.h>																/* Read function */
+#include <sys/ioctl.h>															/* ioctl function */
+#include <iostream>
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+
+#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();
+			 ~LxSerial();
+		bool	port_open(const std::string& portname, LxSerial::PortType port_type);	// open serial port. If overridden, make sure you set s_port_name!!
+		bool	is_port_open();
+		std::string&	get_port_name();
+		bool	set_speed(LxSerial::PortSpeed baudrate );						// enumerated
+		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
+		bool	port_close();
+		int	port_read(unsigned char* buffer, int numBytes) const;
+		int 	port_read(unsigned char* buffer, int numBytes, int seconds, int microseconds);
+		int	port_write(unsigned char* buffer, int numBytes);
+		void	flush_buffer();													// flush input and output buffers
+
+};
+
+
+#endif /*LXSERIAL_H_*/
Index: tags/distros/fuerte/shared_serial/include/shared_serial/client.h
===================================================================
--- tags/distros/fuerte/shared_serial/include/shared_serial/client.h	(revision 26)
+++ tags/distros/fuerte/shared_serial/include/shared_serial/client.h	(revision 26)
@@ -0,0 +1,93 @@
+#ifndef __SS_CLIENT_H_
+#define __SS_CLIENT_H_
+
+#include <ros/ros.h>
+
+/// shared_serial client interface class.
+class SerialClient
+{
+  protected:
+    ros::NodeHandle nh_;                  ///< ROS node handle.
+    ros::ServiceClient connect_service_;  ///< Service client for connect function.
+    ros::ServiceClient sendto_service_;   ///< Service client for sendto function.
+    ros::ServiceClient recv_service_;     ///< Service client for recv function.
+    ros::ServiceClient sendrecv_service_; ///< Service client for sendrecv function.
+    ros::Publisher     send_topic_;       ///< Publisher for send function.
+    ros::Publisher     close_topic_;      ///< Published for close function.
+    ros::Publisher     flush_topic_;      ///< Publisher for flush function.
+
+  public:
+    /// Initialize interface.
+    /** \param path Path to server node. */
+    void init(const char *path="/comm");
+
+    /// Lock serial port.
+    /**
+     * \param timeout Lock time in [s]
+     * \returns Socket identifier.
+     */
+    int connect(float timeout);
+    
+    /// Send data over serial port.
+    /**
+     * \param socket Socket identifier (when 0, the port is locked first).
+     * \param data Data to send.
+     * \param length Length of \c data parameter.
+     * \param timeout Lock time in [s].
+     * \returns Socket identifier.
+     */
+    int sendto(int socket, const unsigned char *data, size_t length, float timeout);
+    
+    /// Receive data from serial port.
+    /**
+     * \param sock Socket identifier (when 0, the port is locked first).
+     * \param length Length of data to receive.
+     * \param recv_timeout Time to wait for data in [s].
+     * \param sock_timeout Lock time in [s].
+     * \param data Data buffer, at least \c length bytes long.
+     * \param[out] data_length Number of bytes received.
+     * \returns Socket identifier.
+     */
+    int recv(int socket, int length, float recv_timeout, float sock_timeout, unsigned char *data, size_t *data_length);
+    
+    /// Coalesced send and receive.
+    /**
+     * \param socket Socket identifier (when 0, the port is locked first).
+     * \param send_data Data to send.
+     * \param send_length Length of \c send_data parameter in bytes.
+     * \param recv_length Length of data to receive.
+     * \param recv_timeout Time to wait for data in [s].
+     * \param sock_timeout Lock time in [s].
+     * \param recv_data Receive data buffer, at least \c recv_length bytes long.
+     * \param[out] recv_data_length Number of bytes received.
+     * \returns Socket identifier.
+     */
+    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);
+    
+    /// Send data over already locked serial port.
+    /**
+     * \param socket Socket identifier.
+     * \param data Data to send.
+     * \param length Length of \c data parameter in bytes.
+     * \param timeout Lock time in [s].
+     * \note This function is asynchronous.
+     */
+    void send(int socket, const unsigned char *data, size_t length, float timeout);
+    
+    /// Unlock serial port.
+    /**
+     * \param socket Socket identifier.
+     * \note This function is asynchronous.
+     */
+    void close(int socket);
+    
+    /// Flush serial port.
+    /**
+     * \param socket Socket identifier.
+     * \param timeout Lock time in [s].
+     * \note This function is asynchronous.
+     */
+    void flush(int socket, float timeout);
+};
+
+#endif /* __SS_CLIENT_H_ */
Index: tags/distros/fuerte/shared_serial/include/shared_serial/server.h
===================================================================
--- tags/distros/fuerte/shared_serial/include/shared_serial/server.h	(revision 26)
+++ tags/distros/fuerte/shared_serial/include/shared_serial/server.h	(revision 26)
@@ -0,0 +1,85 @@
+#ifndef __SS_SERVER_H_
+#define __SS_SERVER_H
+
+#include <stdint.h>
+#include <sys/time.h>
+#include <pthread.h>
+
+#include <ros/ros.h>
+#include <shared_serial/LxSerial.h>
+#include <shared_serial/watchdog.h>
+
+#include <shared_serial/Connect.h>
+#include <shared_serial/Send.h>
+#include <shared_serial/SendTo.h>
+#include <shared_serial/Recv.h>
+#include <shared_serial/SendRecv.h>
+#include <shared_serial/Close.h>
+#include <shared_serial/Flush.h>
+
+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: tags/distros/fuerte/shared_serial/include/shared_serial/watchdog.h
===================================================================
--- tags/distros/fuerte/shared_serial/include/shared_serial/watchdog.h	(revision 26)
+++ tags/distros/fuerte/shared_serial/include/shared_serial/watchdog.h	(revision 26)
@@ -0,0 +1,30 @@
+#ifndef __SS_WATCHDOG_H
+#define __SS_WATCHDOG_H
+
+#include <pthread.h>
+
+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: tags/distros/fuerte/shared_serial/launch/rs232usb.launch
===================================================================
--- tags/distros/fuerte/shared_serial/launch/rs232usb.launch	(revision 26)
+++ tags/distros/fuerte/shared_serial/launch/rs232usb.launch	(revision 26)
@@ -0,0 +1,8 @@
+<launch>
+	<node name="comm" pkg="shared_serial" type="server">
+		<param name="port_name" value="/dev/ttyUSB0"/>
+		<param name="port_type" value="RS232"/>
+		<param name="baud_rate" value="9600"/>
+		<param name="watchdog_interval" value="2"/>
+	</node>
+</launch>
Index: tags/distros/fuerte/shared_serial/launch/start.launch
===================================================================
--- tags/distros/fuerte/shared_serial/launch/start.launch	(revision 26)
+++ tags/distros/fuerte/shared_serial/launch/start.launch	(revision 26)
@@ -0,0 +1,7 @@
+<launch>
+	<node name="motor_comm" pkg="shared_serial" type="server">
+		<param name="port_name" value="/dev/ttyUSB0"/>
+		<param name="port_type" value="RS485_FTDI"/>
+		<param name="baud_rate" value="921600"/>
+	</node>
+</launch>
Index: tags/distros/fuerte/shared_serial/mainpage.dox
===================================================================
--- tags/distros/fuerte/shared_serial/mainpage.dox	(revision 26)
+++ tags/distros/fuerte/shared_serial/mainpage.dox	(revision 26)
@@ -0,0 +1,28 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b shared_serial implements locking serial communications services.
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+The client API is a single class, called SerialClient. You can also use the messages and services defined in the <a href="index-msg.html">message and service documentation</a>.
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
Index: tags/distros/fuerte/shared_serial/manifest.xml
===================================================================
--- tags/distros/fuerte/shared_serial/manifest.xml	(revision 26)
+++ tags/distros/fuerte/shared_serial/manifest.xml	(revision 26)
@@ -0,0 +1,17 @@
+<package>
+  <description brief="shared_serial">
+
+    Shared serial port with locking functionality
+
+  </description>
+  <author>Wouter Caarls</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/shared_serial</url>
+  <depend package="roscpp"/>
+  
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lshared_serial"/>
+  </export>
+
+</package>
Index: tags/distros/fuerte/shared_serial/msg/Close.msg
===================================================================
--- tags/distros/fuerte/shared_serial/msg/Close.msg	(revision 26)
+++ tags/distros/fuerte/shared_serial/msg/Close.msg	(revision 26)
@@ -0,0 +1,4 @@
+# Release serial port lock.
+
+# Socket identifier from an earlier communication.
+uint32 socket
Index: tags/distros/fuerte/shared_serial/msg/Flush.msg
===================================================================
--- tags/distros/fuerte/shared_serial/msg/Flush.msg	(revision 26)
+++ tags/distros/fuerte/shared_serial/msg/Flush.msg	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/msg/Send.msg
===================================================================
--- tags/distros/fuerte/shared_serial/msg/Send.msg	(revision 26)
+++ tags/distros/fuerte/shared_serial/msg/Send.msg	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/src/LxSerial.cpp
===================================================================
--- tags/distros/fuerte/shared_serial/src/LxSerial.cpp	(revision 26)
+++ tags/distros/fuerte/shared_serial/src/LxSerial.cpp	(revision 26)
@@ -0,0 +1,459 @@
+//============================================================================
+// 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 <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <poll.h>
+#include <endian.h>
+
+#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<nBytesRead;i++)
+		{ printf("%02X ",buffer[i]); }
+	printf("(%d)\n",nBytesRead);
+	#endif
+
+	return nBytesRead;
+}
+
+int 	LxSerial::port_read(unsigned char* buffer, int numBytes, int seconds, int microseconds)
+{
+	// Init time variables (they are decreased by wait_for_input)
+	int s = seconds;
+	int us = microseconds;
+	int nBytesRead = 0;
+	while (nBytesRead < numBytes)
+	{
+		if( wait_for_input( &s, &us) )
+		{
+			int partialRead = read(hPort, buffer + nBytesRead, numBytes - nBytesRead); // Read data
+			nBytesRead += partialRead;
+		}
+		else if (nBytesRead < 0)
+		{
+			#ifdef __DBG__
+			printf("Read Timeout... \n");
+			#endif
+			return READ_ERROR;
+		}
+		else
+			break;
+	}
+
+	#ifdef __DBG__
+	printf("read  ");
+	for (int i=0;i<nBytesRead;i++)
+		{ printf("%02X ",buffer[i]); }
+	printf("(%d)\n",nBytesRead);
+	#endif
+
+	return nBytesRead;
+}
+
+bool	LxSerial::wait_for_input(int *seconds, int *microseconds)
+{
+	fd_set readset;
+	timeval timeout;
+	timeout.tv_sec = *seconds;										// seconds
+	timeout.tv_usec = *microseconds;									// microseconds
+	FD_ZERO(&readset);     											// clear file discriptor
+	FD_SET(hPort, &readset);										// set filediscripter for port
+	int res = select(hPort+1, &readset, NULL, NULL, &timeout); 	// wait till readable data is in the buffer
+	*seconds = timeout.tv_sec;
+	*microseconds = timeout.tv_usec;
+	return res == 1;
+}
+
+int		LxSerial::port_write(unsigned char* buffer, int numBytes)
+{
+	int msc = TIOCM_RTS;
+	if (b_rts){														// control the RTS signal if needed
+		ioctl(hPort, TIOCMBIS, &msc);								// before you write set RTS signal
+		usleep(1000);												// wait until the RTS signal is high
+	}
+
+	int numBytesWritten = write(hPort, buffer, numBytes);			// write data
+
+	if (numBytes != numBytesWritten){
+		perror("Error while writing to serial port");
+		assert(numBytes == numBytesWritten);
+	}
+
+	#ifdef __DBG__
+	printf("write ");
+	for (int i=0;i<numBytes;i++)
+		{ printf("%02X ",buffer[i]); }
+	printf("(%d)",numBytesWritten);
+	printf("\n");
+	#endif
+	
+	tcdrain(hPort);		  											// Wait till all the data in the buffer is transmitted
+
+	if (b_rts){														// control the RTS signal if needed
+		ioctl(hPort, TIOCMBIC, &msc);  								// Clear Ready To Send signal
+	}
+
+	if (b_clear_echo)
+	{												// Read echo from line and detect collisions if neened
+		unsigned char* echo_buffer = new unsigned char[numBytes];
+		int nBytesRead = 0;
+		int s = ECHO_WAIT_TIME_SEC;
+		int us = ECHO_WAIT_TIME_USEC;
+		while (nBytesRead < numBytesWritten)
+		{
+			if (wait_for_input(&s , &us))
+				nBytesRead = read(hPort, echo_buffer + nBytesRead, numBytes - nBytesRead);		// Read back ECHO of sended data
+			else
+			{
+				#ifdef __DBG__
+				printf("echo read back timeout\n");
+				#endif
+				delete[] echo_buffer;
+				return ECHO_TIMEOUT_ERROR;
+			}
+		}
+
+
+		#ifdef __DBG__
+		printf("echo  ");
+		for (int i=0;i<nBytesRead;i++)
+			{ printf("%02X ",buffer[i]); }
+		printf("(%d)\n",nBytesRead);
+		#endif
+
+		if (nBytesRead!=numBytesWritten)
+		{
+			#ifdef __DBG__
+			printf("Warning: nr. of characters echoed not correct\n");
+			#endif
+			delete[] echo_buffer;
+			return READ_ERROR;
+		}
+
+		if (memcmp(buffer,echo_buffer, numBytes)!=0)
+		{
+			#ifdef __DBG__
+			printf("collition detect\n");
+			#endif
+			usleep(COLLISION_WAIT_TIME_USEC);							//wait for a while
+			tcflush(hPort, TCIFLUSH); 								//empty all data that was in read buffer but has not been read
+			delete[] echo_buffer;
+			return COLLISION_DETECT_ERROR;
+		}
+		delete[] echo_buffer;
+	}
+
+	return numBytesWritten;
+}
+
+void LxSerial::flush_buffer()
+{
+	tcflush(hPort, TCIOFLUSH);										// flush data buffer
+}
+
+LxSerial::~LxSerial()
+{
+	if (hPort != INVALID_DEVICE_HANDLE)
+		port_close();
+}
Index: tags/distros/fuerte/shared_serial/src/client.cpp
===================================================================
--- tags/distros/fuerte/shared_serial/src/client.cpp	(revision 26)
+++ tags/distros/fuerte/shared_serial/src/client.cpp	(revision 26)
@@ -0,0 +1,128 @@
+#include <ros/ros.h>
+#include <shared_serial/client.h>
+
+#include <shared_serial/Connect.h>
+#include <shared_serial/Send.h>
+#include <shared_serial/SendTo.h>
+#include <shared_serial/Recv.h>
+#include <shared_serial/SendRecv.h>
+#include <shared_serial/Close.h>
+#include <shared_serial/Flush.h>
+
+void SerialClient::init(const char *path)
+{
+  nh_ = ros::NodeHandle(path);
+
+  send_topic_ = nh_.advertise<shared_serial::Send>("send", 10);
+  close_topic_ = nh_.advertise<shared_serial::Close>("close", 1);
+  flush_topic_ = nh_.advertise<shared_serial::Flush>("flush", 1);
+
+  connect_service_ = nh_.serviceClient<shared_serial::Connect>("connect", true);
+  sendto_service_ = nh_.serviceClient<shared_serial::SendTo>("sendto", true);
+  recv_service_ = nh_.serviceClient<shared_serial::Recv>("recv", true);
+  sendrecv_service_ = nh_.serviceClient<shared_serial::SendRecv>("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: tags/distros/fuerte/shared_serial/src/server.cpp
===================================================================
--- tags/distros/fuerte/shared_serial/src/server.cpp	(revision 26)
+++ tags/distros/fuerte/shared_serial/src/server.cpp	(revision 26)
@@ -0,0 +1,345 @@
+#include <ros/assert.h>
+#include <shared_serial/server.h>
+#include <shared_serial/watchdog.h>
+
+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<double>("watchdog_interval", watchdog_interval, 10);
+
+  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: tags/distros/fuerte/shared_serial/src/watchdog.cpp
===================================================================
--- tags/distros/fuerte/shared_serial/src/watchdog.cpp	(revision 26)
+++ tags/distros/fuerte/shared_serial/src/watchdog.cpp	(revision 26)
@@ -0,0 +1,65 @@
+#include <ros/ros.h>
+#include <shared_serial/watchdog.h>
+
+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<WatchdogThread*>(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: tags/distros/fuerte/shared_serial/srv/Connect.srv
===================================================================
--- tags/distros/fuerte/shared_serial/srv/Connect.srv	(revision 26)
+++ tags/distros/fuerte/shared_serial/srv/Connect.srv	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/srv/Recv.srv
===================================================================
--- tags/distros/fuerte/shared_serial/srv/Recv.srv	(revision 26)
+++ tags/distros/fuerte/shared_serial/srv/Recv.srv	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/srv/SendRecv.srv
===================================================================
--- tags/distros/fuerte/shared_serial/srv/SendRecv.srv	(revision 26)
+++ tags/distros/fuerte/shared_serial/srv/SendRecv.srv	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/srv/SendTo.srv
===================================================================
--- tags/distros/fuerte/shared_serial/srv/SendTo.srv	(revision 26)
+++ tags/distros/fuerte/shared_serial/srv/SendTo.srv	(revision 26)
@@ -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: tags/distros/fuerte/shared_serial/stack.xml
===================================================================
--- tags/distros/fuerte/shared_serial/stack.xml	(revision 26)
+++ tags/distros/fuerte/shared_serial/stack.xml	(revision 26)
@@ -0,0 +1,19 @@
+<stack>
+  <description brief="shared_serial">Shared serial port with locking functionality</description>
+  <author email="w.caarls@tudelft.nl">Wouter Caarls</author>
+  <maintainer email="w.caarls@tudelft.nl">Wouter Caarls</maintainer>
+  <license>BSD</license>
+  <copyright>TU Delft Robotics Institute</copyright>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/shared_serial</url>
+  
+  <!-- catkin -->
+  <name>shared_serial</name>
+  <version>0.1.0</version>
+  <build_depends>ros_comm</build_depends>
+  <depends>ros_comm</depends>
+  
+  <!-- rosbuild -->
+  <depend stack="ros"/>
+  <depend stack="ros_comm"/>
+</stack>
