diff --git a/scripts/make_library.py b/scripts/make_library.py index a2afe6e..a8edd7d 100755 --- a/scripts/make_library.py +++ b/scripts/make_library.py @@ -460,7 +460,7 @@ def _write_deserializer(self, f): f.write(' size_t offset = 0;\n') for d in self.data: d.deserialize(f) - f.write(' return offset;\n'); + f.write(' return offset;\n'); f.write(' }\n') f.write('\n') diff --git a/src/ros_lib/RosQtSocket.cpp b/src/ros_lib/RosQtSocket.cpp index 7cb6f09..e311717 100644 --- a/src/ros_lib/RosQtSocket.cpp +++ b/src/ros_lib/RosQtSocket.cpp @@ -1,46 +1,51 @@ #include "ros/RosQtSocket.h" -#define DEFAULT_PORT 11411 - using std::string; using std::cerr; using std::endl; RosQtSocket::RosQtSocket(QObject *parent): - QObject(parent) + QObject(parent), + socket_valid_(false) { - QObject::connect(&m_socket, SIGNAL(readyRead()), this, SIGNAL(readyRead())); - QObject::connect(&m_socket, SIGNAL(connected()), this, SLOT(onConnected())); + QObject::connect(&socket_, SIGNAL(readyRead()), this, SIGNAL(readyRead())); + QObject::connect(&socket_, SIGNAL(connected()), this, SLOT(onConnected())); } void RosQtSocket::doConnect() { - if(m_socket.state() == QAbstractSocket::UnconnectedState) + if(socket_.state() == QAbstractSocket::UnconnectedState) { - m_socket.connectToHost(m_address, m_port); + socket_.connectToHost(address_, port_); } } -void RosQtSocket::init(const std::string &server_hostname) +void RosQtSocket::open(const std::string &server_hostname, uint16_t port) { - m_address = QHostAddress(QString::fromStdString(server_hostname)); - m_port = DEFAULT_PORT; + socket_valid_ = true; + address_ = QHostAddress(QString::fromStdString(server_hostname)); + port_ = port; doConnect(); } +void RosQtSocket::close() +{ + socket_.close(); +} + int64_t RosQtSocket::read(unsigned char *data, int max_length) { - if(m_socket.state() != QAbstractSocket::ConnectedState) + if(socket_.state() != QAbstractSocket::ConnectedState) { std::cerr << "Failed to receive data from server, not connected " << std::endl; doConnect(); return -1; } - int64_t result = m_socket.read((char*)data, max_length); + int64_t result = socket_.read((char*)data, max_length); if(result < 0) { std::cerr << "Failed to receive data from server" << std::endl; @@ -52,7 +57,7 @@ int64_t RosQtSocket::read(unsigned char *data, int max_length) bool RosQtSocket::write(const unsigned char *data, int length) { - if(m_socket.state() != QAbstractSocket::ConnectedState) + if(socket_valid_ && socket_.state() != QAbstractSocket::ConnectedState) { std::cerr << "Failed to write data to the server, not connected " << std::endl; doConnect(); @@ -60,7 +65,7 @@ bool RosQtSocket::write(const unsigned char *data, int length) } else { - qint64 result = m_socket.write((const char*)data, length); + qint64 result = socket_.write((const char*)data, length); if(result != length) { std::cerr << "Failed to write all the data to the server" << std::endl; @@ -78,5 +83,5 @@ unsigned long RosQtSocket::time() void RosQtSocket::onConnected() { - m_socket.setSocketOption(QAbstractSocket::LowDelayOption, 1); + socket_.setSocketOption(QAbstractSocket::LowDelayOption, 1); } diff --git a/src/ros_lib/ros/RosQtSocket.h b/src/ros_lib/ros/RosQtSocket.h index 72b60a4..f2b405d 100644 --- a/src/ros_lib/ros/RosQtSocket.h +++ b/src/ros_lib/ros/RosQtSocket.h @@ -43,7 +43,9 @@ class RosQtSocket : public QObject void doConnect(); - void init (const std::string &server_hostname); + void open (const std::string &server_hostname, uint16_t port); + + void close (); int64_t read (unsigned char *data, int max_length); @@ -58,9 +60,10 @@ private slots: void readyRead(); private: - QTcpSocket m_socket; - quint16 m_port; - QHostAddress m_address; + QTcpSocket socket_; + bool socket_valid_; + quint16 port_; + QHostAddress address_; }; #endif diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h index c59ab9d..5cee37f 100644 --- a/src/ros_lib/ros/node_handle.h +++ b/src/ros_lib/ros/node_handle.h @@ -121,14 +121,15 @@ class NodeHandle : public NodeHandleBase_ uint32_t OUTPUT_SIZE; /* - * Setup Functions - */ + * Setup Functions + */ public: NodeHandle(uint32_t input_size=10000, uint32_t output_size=10000, QObject *parent=0) : NodeHandleBase_(parent), INPUT_SIZE(input_size), OUTPUT_SIZE(output_size), - configured_(false) + configured_(false), + isActive_(false) { message_in.resize(INPUT_SIZE); message_out.resize(OUTPUT_SIZE); @@ -149,20 +150,23 @@ class NodeHandle : public NodeHandleBase_ spinOnce(); } - /* Start serial, initialize buffers */ - void initNode(){ - - } - /* Start a named port, which may be network server IP, initialize buffers */ - void initNode(const std::string &portName){ - hardware_.init(portName); + void open(const std::string &hostName, uint16_t port = 11411){ + isActive_ = true; + hardware_.open(hostName, port); mode_ = 0; bytes_ = 0; index_ = 0; topic_ = 0; } + void close() + { + isActive_ = false; + configured_ = false; + hardware_.close(); + } + protected: //State machine variables for spinOnce int mode_; @@ -172,6 +176,7 @@ class NodeHandle : public NodeHandleBase_ unsigned int checksum_; bool configured_; + bool isActive_; // true if we should try to connect /* used for syncing the time */ uint32_t last_sync_time; @@ -187,6 +192,9 @@ class NodeHandle : public NodeHandleBase_ virtual int spinOnce(){ unsigned char buffer[READ_BUFFER_SIZE]; + if(!isActive_) + return 0; + /* restart if timed out */ uint32_t c_time = hardware_.time(); if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ diff --git a/test/Basic/Basic.pro.user b/test/Basic/Basic.pro.user index 2fe0772..4761995 100644 --- a/test/Basic/Basic.pro.user +++ b/test/Basic/Basic.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/test/Basic/node.cpp b/test/Basic/node.cpp index d1ed24d..e5044a8 100644 --- a/test/Basic/node.cpp +++ b/test/Basic/node.cpp @@ -18,7 +18,7 @@ Node::Node(): std::string ros_master = "127.0.0.1"; printf ("Connecting to server at %s\n", ros_master.c_str()); - nh.initNode (ros_master.c_str()); + nh.open (ros_master.c_str()); nh.subscribe (poseSub); nh.advertise(chatter);