diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index c4970e3c..1dd2bf55 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -489,6 +489,14 @@ class UrDriver private: static std::string readScriptFile(const std::string& filename); + /*! + * \brief Reconnects the secondary stream used to send program to the robot. + * + * Only for use in headless mode, as it replaces the use of the URCaps program. + * + * \returns true of on successful reconnection, false otherwise + */ + bool reconnectSecondaryStream(); int rtde_frequency_; comm::INotifier notifier_; diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp index fae39351..ea5122c7 100644 --- a/src/comm/tcp_server.cpp +++ b/src/comm/tcp_server.cpp @@ -296,12 +296,13 @@ void TCPServer::readData(const int fd) } else { - if (0 < nbytesrecv) + if (nbytesrecv < 0) { if (errno == ECONNRESET) // if connection gets reset by client, we want to suppress this output { - URCL_LOG_DEBUG("client from FD %s sent a connection reset package.", fd); + URCL_LOG_DEBUG("client from FD %d sent a connection reset package.", fd); } + else { URCL_LOG_ERROR("recv() on FD %d failed.", fd); } diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 32f40a93..8803664a 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -187,7 +187,14 @@ bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read) return false; } else if (res < 0) + { + if (!(errno == EAGAIN || errno == EWOULDBLOCK)) + { + // any permanent error should be detected early + state_ = SocketState::Disconnected; + } return false; + } read = static_cast(res); return true; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9292681b..5038f798 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -542,9 +542,8 @@ bool UrDriver::sendScript(const std::string& program) { if (secondary_stream_ == nullptr) { - throw std::runtime_error("Sending script to robot requested while there is no primary interface established. " - "This " - "should not happen."); + throw std::runtime_error("Sending script to robot requested while there is no secondary interface established. " + "This should not happen."); } // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will @@ -556,12 +555,28 @@ bool UrDriver::sendScript(const std::string& program) const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); size_t written; - if (secondary_stream_->write(data, len, written)) + const auto send_script_contents = [this, program_with_newline, data, len, + &written](const std::string&& description) -> bool { + if (secondary_stream_->write(data, len, written)) + { + URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + const std::string error_message = "Could not send program to robot: " + description; + URCL_LOG_ERROR(error_message.c_str()); + return false; + }; + + if (send_script_contents("initial attempt")) { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); return true; } - URCL_LOG_ERROR("Could not send program to robot"); + + if (reconnectSecondaryStream()) + { + return send_script_contents("after reconnecting secondary stream"); + } + return false; } @@ -578,6 +593,19 @@ bool UrDriver::sendRobotProgram() } } +bool UrDriver::reconnectSecondaryStream() +{ + URCL_LOG_DEBUG("Closing secondary stream..."); + secondary_stream_->close(); + if (secondary_stream_->connect()) + { + URCL_LOG_DEBUG("Secondary stream connected"); + return true; + } + URCL_LOG_ERROR("Failed to reconnect secondary stream!"); + return false; +} + std::vector UrDriver::getRTDEOutputRecipe() { return rtde_client_->getOutputRecipe(); diff --git a/tests/test_tcp_socket.cpp b/tests/test_tcp_socket.cpp index 040c2610..a96b6595 100644 --- a/tests/test_tcp_socket.cpp +++ b/tests/test_tcp_socket.cpp @@ -360,6 +360,28 @@ TEST_F(TCPSocketTest, test_deprecated_reconnection_time_interface) EXPECT_TRUE(client_->setup(2)); } +TEST_F(TCPSocketTest, test_read_on_socket_abruptly_closed) +{ + client_->setup(); + + // Make sure the client has connected to the server, before writing to the client + EXPECT_TRUE(waitForConnectionCallback()); + + std::string send_message = "test message"; + size_t len = send_message.size(); + const uint8_t* data = reinterpret_cast(send_message.c_str()); + size_t written; + server_->write(client_fd_, data, len, written); + + // Simulate socket failure + close(client_->getSocketFD()); + + char characters; + size_t read_chars = 0; + EXPECT_FALSE(client_->read((uint8_t*)&characters, 1, read_chars)); + EXPECT_EQ(client_->getState(), comm::SocketState::Disconnected); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index d193daf5..d255cc32 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -359,6 +359,19 @@ TEST_F(UrDriverTest, target_outside_limits_pose) waitForProgramNotRunning(1000); } +TEST_F(UrDriverTest, send_robot_program_retry_on_failure) +{ + // Start robot program + g_ur_driver_->sendRobotProgram(); + EXPECT_TRUE(waitForProgramRunning(1000)); + + // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when + // switching from Remote to Local and back to Remote mode for example. + g_ur_driver_->secondary_stream_->close(); + + EXPECT_TRUE(g_ur_driver_->sendRobotProgram()); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[])