From 34a92308a3baa2edc6bc1dcd19db8735bbf70176 Mon Sep 17 00:00:00 2001 From: Johnson Shih Date: Fri, 10 Aug 2018 15:41:40 -0700 Subject: [PATCH] Fix test_roscpp build issues on Windows --- test/test_roscpp/CMakeLists.txt | 4 + test/test_roscpp/test/CMakeLists.txt | 3 + test/test_roscpp/test/src/check_master.cpp | 14 ++ test/test_roscpp/test/src/service_adv_a.cpp | 7 - .../test/src/service_adv_zombie.cpp | 5 + .../test/src/service_call_zombie.cpp | 4 + .../test/src/service_exception.cpp | 9 ++ test/test_roscpp/test/test_poll_set.cpp | 139 +++++++++++++++++- test/test_roscpp/test/test_transport_tcp.cpp | 2 + 9 files changed, 174 insertions(+), 13 deletions(-) diff --git a/test/test_roscpp/CMakeLists.txt b/test/test_roscpp/CMakeLists.txt index 41ed980f49..5197673622 100644 --- a/test/test_roscpp/CMakeLists.txt +++ b/test/test_roscpp/CMakeLists.txt @@ -15,6 +15,10 @@ if(CATKIN_ENABLE_TESTING) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + if(WIN32) + add_definitions(-DNOGDI) + endif() + # Testing message and service files. See comment below about subdirectory support. add_message_files( NOINSTALL diff --git a/test/test_roscpp/test/CMakeLists.txt b/test/test_roscpp/test/CMakeLists.txt index 05dae977d3..271738bd66 100644 --- a/test/test_roscpp/test/CMakeLists.txt +++ b/test/test_roscpp/test/CMakeLists.txt @@ -11,6 +11,9 @@ endif() catkin_add_gtest(${PROJECT_NAME}-test_poll_set test_poll_set.cpp) if(TARGET ${PROJECT_NAME}-test_poll_set) target_link_libraries(${PROJECT_NAME}-test_poll_set ${catkin_LIBRARIES}) + if(WIN32) + target_link_libraries(${PROJECT_NAME}-test_poll_set ws2_32) + endif() endif() catkin_add_gtest(${PROJECT_NAME}-test_transport_tcp test_transport_tcp.cpp) diff --git a/test/test_roscpp/test/src/check_master.cpp b/test/test_roscpp/test/src/check_master.cpp index 4392f2b91f..e4e878c4fc 100644 --- a/test/test_roscpp/test/src/check_master.cpp +++ b/test/test_roscpp/test/src/check_master.cpp @@ -46,6 +46,20 @@ using namespace XmlRpc; bool g_should_exist = true; +#ifdef _WIN32 +int setenv(const char *name, const char *value, int overwrite) +{ + if(!overwrite) + { + size_t envsize = 0; + const errno_t errcode = getenv_s(&envsize, NULL, 0, name); + if(errcode || envsize) + return errcode; + } + return _putenv_s(name, value); +} +#endif + TEST(CheckMaster, checkMaster) { ASSERT_EQ(ros::master::check(), g_should_exist); diff --git a/test/test_roscpp/test/src/service_adv_a.cpp b/test/test_roscpp/test/src/service_adv_a.cpp index 1cf6fcc0af..52179fe91a 100644 --- a/test/test_roscpp/test/src/service_adv_a.cpp +++ b/test/test_roscpp/test/src/service_adv_a.cpp @@ -36,13 +36,6 @@ #include "ros/ros.h" #include -#include -#include -#include -#include -#include -#include - bool srvCallback(test_roscpp::TestStringString::Request &, test_roscpp::TestStringString::Response &res) { diff --git a/test/test_roscpp/test/src/service_adv_zombie.cpp b/test/test_roscpp/test/src/service_adv_zombie.cpp index e5508c5d63..7f1308efea 100644 --- a/test/test_roscpp/test/src/service_adv_zombie.cpp +++ b/test/test_roscpp/test/src/service_adv_zombie.cpp @@ -53,7 +53,12 @@ int main(int argc, char** argv) for(int i = 0; i < 10; ++i) { ros::spinOnce(); + +#ifndef _WIN32 usleep(100*1000); +#else + Sleep(100); +#endif } // Exit immediately without calling any atexit hooks diff --git a/test/test_roscpp/test/src/service_call_zombie.cpp b/test/test_roscpp/test/src/service_call_zombie.cpp index 206b4e47f0..12a3e05155 100644 --- a/test/test_roscpp/test/src/service_call_zombie.cpp +++ b/test/test_roscpp/test/src/service_call_zombie.cpp @@ -65,7 +65,11 @@ main(int argc, char** argv) ros::init(argc, argv, "service_call"); ros::NodeHandle nh; +#ifndef _WIN32 sleep(10); +#else + Sleep(10 * 1000); +#endif int ret = RUN_ALL_TESTS(); diff --git a/test/test_roscpp/test/src/service_exception.cpp b/test/test_roscpp/test/src/service_exception.cpp index 05d166ee98..4b0a0a93a4 100644 --- a/test/test_roscpp/test/src/service_exception.cpp +++ b/test/test_roscpp/test/src/service_exception.cpp @@ -5,6 +5,10 @@ #include "ros/ros.h" #include "std_srvs/Empty.h" #include +#ifdef _MSC_VER + // Have to be able to encode wchar LogStrings on windows. +# include +#endif #include #include @@ -72,7 +76,12 @@ TEST(roscpp, ServiceThrowingException) const std::list& list = appender->getList(); for (std::list::const_iterator it = list.begin(); it != list.end(); it++) { +#ifdef _MSC_VER + LOG4CXX_ENCODE_CHAR(tmpstr, (*it)->getMessage()); // has to handle LogString with wchar types. + const std::string& msg = tmpstr; // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro +#else const log4cxx::LogString& msg = (*it)->getMessage(); +#endif size_t pos_error = msg.find("Service call failed:"); size_t pos_exception = msg.find(EXCEPTION); if (pos_error != std::string::npos && pos_exception != std::string::npos) diff --git a/test/test_roscpp/test/test_poll_set.cpp b/test/test_roscpp/test/test_poll_set.cpp index 61a20088b9..a4ca6caf34 100644 --- a/test/test_roscpp/test/test_poll_set.cpp +++ b/test/test_roscpp/test/test_poll_set.cpp @@ -35,7 +35,9 @@ #include #include "ros/poll_set.h" -#include +#ifndef _WIN32 +# include +#endif #include @@ -44,22 +46,145 @@ using namespace ros; +int set_nonblocking(int &socket) +{ +#ifndef _WIN32 + if (fcntl(socket, F_SETFL, O_NONBLOCK) == -1) + { + return errno; + } +#else + u_long non_blocking = 1; + if (ioctlsocket(socket, FIONBIO, &non_blocking) != 0) + { + return WSAGetLastError(); + } +#endif + return 0; +} + +int create_socket_pair(int socket_pair[2]) +{ +#ifndef _WIN32 + return socketpair(AF_UNIX, SOCK_STREAM, 0, socket_pair); +#else + socket_pair[0] = INVALID_SOCKET; + socket_pair[1] = INVALID_SOCKET; + + /********************* + ** Listen Socket + **********************/ + socket_fd_t listen_socket = INVALID_SOCKET; + listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (listen_socket == INVALID_SOCKET) + { + return WSAGetLastError(); + } + + // allow it to be bound to an address already in use - do we actually need this? + int reuse = 1; + if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&reuse), static_cast(sizeof(reuse))) == SOCKET_ERROR) + { + ::closesocket(listen_socket); + return WSAGetLastError(); + } + + union + { + struct sockaddr_in inaddr; + struct sockaddr addr; + } a; + + memset(&a, 0, sizeof(a)); + a.inaddr.sin_family = AF_INET; + a.inaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + // For TCP/IP, if the port is specified as zero, the service provider assigns + // a unique port to the application from the dynamic client port range. + a.inaddr.sin_port = 0; + + if (bind(listen_socket, &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) + { + ::closesocket(listen_socket); + return WSAGetLastError(); + } + + // we need this below because the system auto filled in some entries, e.g. port # + socklen_t addrlen = static_cast(sizeof(a.inaddr)); + if (getsockname(listen_socket, &a.addr, &addrlen) == SOCKET_ERROR) + { + ::closesocket(listen_socket); + return WSAGetLastError(); + } + // max 1 connection permitted + if (listen(listen_socket, 1) == SOCKET_ERROR) + { + ::closesocket(listen_socket); + return WSAGetLastError(); + } + + /********************* + ** Connection + **********************/ + DWORD overlapped_flag = 0; + socket_pair[0] = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, overlapped_flag); + if (socket_pair[0] == INVALID_SOCKET) + { + ::closesocket(listen_socket); + ::closesocket(socket_pair[0]); + return WSAGetLastError(); + } + + // reusing the information from above to connect to the listener + if (connect(socket_pair[0], &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) + { + ::closesocket(listen_socket); + ::closesocket(socket_pair[0]); + return WSAGetLastError(); + } + + /********************* + ** Accept + **********************/ + socket_pair[1] = accept(listen_socket, NULL, NULL); + if (socket_pair[1] == INVALID_SOCKET) + { + ::closesocket(listen_socket); + ::closesocket(socket_pair[0]); + return WSAGetLastError(); + } + + /********************* + ** Cleanup + **********************/ + ::closesocket(listen_socket); // the listener has done its job. + return 0; +#endif +} + class Poller : public testing::Test { public: Poller() { +#ifdef _WIN32 + WSADATA wsaData; + WSAStartup(MAKEWORD(2, 0), &wsaData); +#endif } ~Poller() { ::close(sockets_[0]); ::close(sockets_[1]); + +#ifdef _WIN32 + WSACleanup(); +#endif } void waitThenSignal() { - usleep(100000); + boost::this_thread::sleep(boost::posix_time::microseconds(100000)); poll_set_.signal(); } @@ -68,15 +193,15 @@ class Poller : public testing::Test virtual void SetUp() { - if(socketpair(AF_UNIX, SOCK_STREAM, 0, sockets_) != 0) + if (create_socket_pair(sockets_) != 0) { FAIL(); } - if(fcntl(sockets_[0], F_SETFL, O_NONBLOCK) == -1) + if(set_nonblocking(sockets_[0]) != 0) { FAIL(); } - if(fcntl(sockets_[1], F_SETFL, O_NONBLOCK) == -1) + if(set_nonblocking(sockets_[1]) != 0) { FAIL(); } @@ -390,7 +515,7 @@ TEST_F(Poller, signal) poll_set_.update(-1); // wait for poll_set_.signal_mutex_ to be unlocked after invoking signal() - usleep(50000); + boost::this_thread::sleep(boost::posix_time::microseconds(50000)); } @@ -398,7 +523,9 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); +#ifndef _WIN32 signal(SIGPIPE, SIG_IGN); +#endif return RUN_ALL_TESTS(); } diff --git a/test/test_roscpp/test/test_transport_tcp.cpp b/test/test_roscpp/test/test_transport_tcp.cpp index 74d28284d9..202db72013 100644 --- a/test/test_roscpp/test/test_transport_tcp.cpp +++ b/test/test_roscpp/test/test_transport_tcp.cpp @@ -417,7 +417,9 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); +#ifndef _WIN32 signal(SIGPIPE, SIG_IGN); +#endif return RUN_ALL_TESTS(); }