From 56dbe8c0f9ccf8fbea5f56c5d2e3419da15a07d9 Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Thu, 8 Sep 2016 12:59:00 -0400 Subject: [PATCH] Integration tests for rosserial (#243) --- .gitignore | 1 + .travis.yml | 2 +- rosserial_client/package.xml | 1 + .../src/ros_lib/ros/node_handle.h | 10 +- rosserial_client/src/ros_lib/ros/publisher.h | 2 +- .../src/ros_lib/ros/service_client.h | 4 +- .../src/ros_lib/ros/service_server.h | 4 +- rosserial_client/src/ros_lib/ros/time.h | 3 +- rosserial_python/nodes/serial_node.py | 29 +++-- rosserial_test/CMakeLists.txt | 37 +++++++ .../include/rosserial_test/fixture.h | 104 ++++++++++++++++++ .../include/rosserial_test/helpers.h | 20 ++++ rosserial_test/include/rosserial_test/ros.h | 41 +++++++ rosserial_test/package.xml | 22 ++++ .../scripts/generate_client_ros_lib | 62 +++++++++++ rosserial_test/src/publish_subscribe.cpp | 79 +++++++++++++ .../test/rosserial_python_serial.test | 9 ++ .../test/rosserial_python_socket.test | 9 ++ .../test/rosserial_server_serial.test | 11 ++ .../test/rosserial_server_socket.test | 12 ++ 20 files changed, 439 insertions(+), 23 deletions(-) create mode 100644 rosserial_test/CMakeLists.txt create mode 100644 rosserial_test/include/rosserial_test/fixture.h create mode 100644 rosserial_test/include/rosserial_test/helpers.h create mode 100644 rosserial_test/include/rosserial_test/ros.h create mode 100644 rosserial_test/package.xml create mode 100755 rosserial_test/scripts/generate_client_ros_lib create mode 100644 rosserial_test/src/publish_subscribe.cpp create mode 100644 rosserial_test/test/rosserial_python_serial.test create mode 100644 rosserial_test/test/rosserial_python_socket.test create mode 100644 rosserial_test/test/rosserial_server_serial.test create mode 100644 rosserial_test/test/rosserial_server_socket.test diff --git a/.gitignore b/.gitignore index 06e82a7b1..db4be29e9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.pyc +*.swp rosserial_server/doc/ diff --git a/.travis.yml b/.travis.yml index 138259ff8..a7187faf6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,5 +24,5 @@ script: - source /opt/ros/$CI_ROS_DISTRO/setup.bash - catkin_make install - catkin_make tests - - catkin_make run_tests + - devel/env.sh catkin_make run_tests -j1 - catkin_test_results build diff --git a/rosserial_client/package.xml b/rosserial_client/package.xml index b4eab6487..f23cccb58 100644 --- a/rosserial_client/package.xml +++ b/rosserial_client/package.xml @@ -12,6 +12,7 @@ http://ros.org/wiki/rosserial_client catkin + rosbash rosserial_msgs std_msgs diff --git a/rosserial_client/src/ros_lib/ros/node_handle.h b/rosserial_client/src/ros_lib/ros/node_handle.h index 9babaff4f..f84042325 100644 --- a/rosserial_client/src/ros_lib/ros/node_handle.h +++ b/rosserial_client/src/ros_lib/ros/node_handle.h @@ -68,7 +68,7 @@ #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data -#include "msg.h" +#include "ros/msg.h" namespace ros { @@ -80,10 +80,10 @@ namespace ros { }; } -#include "publisher.h" -#include "subscriber.h" -#include "service_server.h" -#include "service_client.h" +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "ros/service_server.h" +#include "ros/service_client.h" namespace ros { diff --git a/rosserial_client/src/ros_lib/ros/publisher.h b/rosserial_client/src/ros_lib/ros/publisher.h index 1217362a1..7b20bfa90 100644 --- a/rosserial_client/src/ros_lib/ros/publisher.h +++ b/rosserial_client/src/ros_lib/ros/publisher.h @@ -36,7 +36,7 @@ #define _ROS_PUBLISHER_H_ #include "rosserial_msgs/TopicInfo.h" -#include "node_handle.h" +#include "ros/node_handle.h" namespace ros { diff --git a/rosserial_client/src/ros_lib/ros/service_client.h b/rosserial_client/src/ros_lib/ros/service_client.h index 06522f289..2031fb17e 100644 --- a/rosserial_client/src/ros_lib/ros/service_client.h +++ b/rosserial_client/src/ros_lib/ros/service_client.h @@ -37,8 +37,8 @@ #include "rosserial_msgs/TopicInfo.h" -#include "publisher.h" -#include "subscriber.h" +#include "ros/publisher.h" +#include "ros/subscriber.h" namespace ros { diff --git a/rosserial_client/src/ros_lib/ros/service_server.h b/rosserial_client/src/ros_lib/ros/service_server.h index 67a3e0ad5..8d0873882 100644 --- a/rosserial_client/src/ros_lib/ros/service_server.h +++ b/rosserial_client/src/ros_lib/ros/service_server.h @@ -37,8 +37,8 @@ #include "rosserial_msgs/TopicInfo.h" -#include "publisher.h" -#include "subscriber.h" +#include "ros/publisher.h" +#include "ros/subscriber.h" namespace ros { diff --git a/rosserial_client/src/ros_lib/ros/time.h b/rosserial_client/src/ros_lib/ros/time.h index 6141261f6..24958132f 100644 --- a/rosserial_client/src/ros_lib/ros/time.h +++ b/rosserial_client/src/ros_lib/ros/time.h @@ -35,11 +35,10 @@ #ifndef ROS_TIME_H_ #define ROS_TIME_H_ +#include "ros/duration.h" #include #include -#include "ros/duration.h" - namespace ros { void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); diff --git a/rosserial_python/nodes/serial_node.py b/rosserial_python/nodes/serial_node.py index 9fb617d02..a815c8742 100755 --- a/rosserial_python/nodes/serial_node.py +++ b/rosserial_python/nodes/serial_node.py @@ -37,10 +37,12 @@ import rospy from rosserial_python import SerialClient, RosSerialServer +from serial import SerialException +from time import sleep import multiprocessing import sys - + if __name__=="__main__": rospy.init_node("serial_node") @@ -55,11 +57,11 @@ # TODO: do we really want command line params in addition to parameter server params? sys.argv = rospy.myargv(argv=sys.argv) - if len(sys.argv) == 2 : + if len(sys.argv) >= 2 : port_name = sys.argv[1] if len(sys.argv) == 3 : tcp_portnum = int(sys.argv[2]) - + if port_name == "tcp" : server = RosSerialServer(tcp_portnum, fork_server) rospy.loginfo("Waiting for socket connections on port %d" % tcp_portnum) @@ -75,11 +77,18 @@ process.join() rospy.loginfo("All done") - else : # Use serial port - rospy.loginfo("Connecting to %s at %d baud" % (port_name,baud) ) - client = SerialClient(port_name, baud) - try: - client.run() - except KeyboardInterrupt: - pass + else : # Use serial port + while not rospy.is_shutdown(): + rospy.loginfo("Connecting to %s at %d baud" % (port_name,baud) ) + try: + client = SerialClient(port_name, baud) + client.run() + except KeyboardInterrupt: + break + except SerialException: + sleep(1.0) + continue + except OSError: + sleep(1.0) + continue diff --git a/rosserial_test/CMakeLists.txt b/rosserial_test/CMakeLists.txt new file mode 100644 index 000000000..655f5b0e9 --- /dev/null +++ b/rosserial_test/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosserial_test) + +find_package(catkin REQUIRED COMPONENTS roscpp rosserial_client rosserial_python rosserial_server rostest) + +catkin_package() + +if(CATKIN_ENABLE_TESTING) + # Generate a client library for the test harnesses to use. + add_custom_command( + OUTPUT ${PROJECT_BINARY_DIR}/include/rosserial + COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh rosrun rosserial_test generate_client_ros_lib ${PROJECT_BINARY_DIR}/include + ) + add_custom_target(${PROJECT_NAME}_rosserial_lib DEPENDS ${PROJECT_BINARY_DIR}/include/rosserial) + add_dependencies(${PROJECT_NAME}_rosserial_lib std_msgs_genpy rosserial_msgs_genpy) + + include_directories( + include ${PROJECT_BINARY_DIR}/include ${catkin_INCLUDE_DIRS} + ) + + # Helper for building and linking test executables. + function(add_rosserial_test_executable test_source) + set(target ${PROJECT_NAME}_${test_source}) + add_executable(${target} EXCLUDE_FROM_ALL src/${test_source}) + add_dependencies(${target} ${PROJECT_NAME}_rosserial_lib) + target_link_libraries(${target} ${GTEST_LIBRARIES} ${catkin_LIBRARIES}) + add_dependencies(tests ${target}) + endfunction() + + add_rosserial_test_executable(publish_subscribe) + + add_rostest(test/rosserial_server_socket.test) + add_rostest(test/rosserial_server_serial.test) + add_rostest(test/rosserial_python_socket.test) + # Disabled due to reconnect logic in rosserial_python not being robust enough. + # add_rostest(test/rosserial_python_serial.test) +endif() diff --git a/rosserial_test/include/rosserial_test/fixture.h b/rosserial_test/include/rosserial_test/fixture.h new file mode 100644 index 000000000..fe7fa1800 --- /dev/null +++ b/rosserial_test/include/rosserial_test/fixture.h @@ -0,0 +1,104 @@ + +#include + +#include +#include +#include +#include + +#include "ros/ros.h" + +namespace rosserial { +#include "rosserial_test/ros.h" +} + +class AbstractSetup { +public: + virtual void SetUp()=0; + virtual void TearDown()=0; + int fd; +}; + +class SerialSetup : public AbstractSetup { +public: + virtual void SetUp() { + ASSERT_NE(-1, fd = posix_openpt( O_RDWR | O_NOCTTY | O_NDELAY )); + ASSERT_NE(-1, grantpt(fd)); + ASSERT_NE(-1, unlockpt(fd)); + + char* pty_name; + ASSERT_TRUE((pty_name = ptsname(fd)) != NULL); + + ros::param::get("~port", symlink_name); + symlink(pty_name, symlink_name.c_str()); + } + virtual void TearDown() { + unlink(symlink_name.c_str()); + close(fd); + } + std::string symlink_name; +}; + +class SocketSetup : public AbstractSetup { +public: + virtual void SetUp() { + ros::param::get("~tcp_port", tcp_port); + memset(&serv_addr, 0, sizeof(serv_addr)); + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(tcp_port); + ASSERT_GE(inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr), 0); + + // Try a bunch of times; we don't know how long it will take for the + // server to come up. + for (int attempt = 0; attempt < 10; attempt++) + { + fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + ASSERT_GE(fd, 0); + if (connect(fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) >= 0) + { + // Connection successful, set nonblocking and return. + fcntl(fd, F_SETFL, O_NONBLOCK); + return; + } + close(fd); + ros::Duration(0.5).sleep(); + } + FAIL() << "Unable to connect to rosserial socket server."; + } + virtual void TearDown() { + close(fd); + } + struct sockaddr_in serv_addr; + int tcp_port; +}; + +class SingleClientFixture : public ::testing::Test { +protected: + static void SetModeFromParam() { + std::string mode; + ros::param::get("~mode", mode); + ROS_INFO_STREAM("Using test mode [" << mode << "]"); + if (mode == "socket") { + setup = new SocketSetup(); + } else if (mode == "serial") { + setup = new SerialSetup(); + } else { + FAIL() << "Mode specified other than 'serial' or 'socket'."; + } + } + virtual void SetUp() { + if (setup == NULL) SetModeFromParam(); + setup->SetUp(); + rosserial::ClientComms::fd = setup->fd; + } + virtual void TearDown() { + setup->TearDown(); + } + + rosserial::ros::NodeHandle client_nh; + ros::NodeHandle nh; + static AbstractSetup* setup; +}; +AbstractSetup* SingleClientFixture::setup = NULL; + + diff --git a/rosserial_test/include/rosserial_test/helpers.h b/rosserial_test/include/rosserial_test/helpers.h new file mode 100644 index 000000000..2f8552a76 --- /dev/null +++ b/rosserial_test/include/rosserial_test/helpers.h @@ -0,0 +1,20 @@ + +template +class Callback { +public: + Callback() : times_called(0) + { + } + + void callback(const Msg msg) + { + times_called++; + last_msg = msg; + } + + Msg last_msg; + int times_called; +}; +typedef Callback StringCallback; + + diff --git a/rosserial_test/include/rosserial_test/ros.h b/rosserial_test/include/rosserial_test/ros.h new file mode 100644 index 000000000..7c24add96 --- /dev/null +++ b/rosserial_test/include/rosserial_test/ros.h @@ -0,0 +1,41 @@ +#ifndef ROSSERIAL_ROS_H +#define ROSSERIAL_ROS_H + +#include "rosserial/ros/node_handle.h" +#include "rosserial/duration.cpp" +#include "rosserial/time.cpp" +#include + +class ClientComms { +public: + // Can smuggle in an fd representing either the back end of + // a socket or serial pty, and run the same tests over both. + static int fd; + + // Accessible to be manipulated by tests, for test behaviours + // dependent on the passage of time. + static unsigned long millis; + + void init() { + } + int read() { + unsigned char ch; + ssize_t ret = ::read(fd, &ch, 1); + return ret == 1 ? ch : -1; + } + void write(uint8_t* data, int length) { + ::write(fd, data, length); + } + unsigned long time() { + return millis; + } +}; + +int ClientComms::fd = -1; +unsigned long ClientComms::millis = 0; + +namespace ros { +typedef NodeHandle_ NodeHandle; +} + +#endif // ROSSERIAL_ROS_H diff --git a/rosserial_test/package.xml b/rosserial_test/package.xml new file mode 100644 index 000000000..a6f1e12d5 --- /dev/null +++ b/rosserial_test/package.xml @@ -0,0 +1,22 @@ + + + rosserial_test + 0.0.0 + + A specialized harness which allows end-to-end integration testing of the + rosserial client and server components. + + + Mike Purvis + + BSD + + catkin + + roscpp + gtest + rosserial_client + rosserial_python + rosserial_server + rostest + diff --git a/rosserial_test/scripts/generate_client_ros_lib b/rosserial_test/scripts/generate_client_ros_lib new file mode 100755 index 000000000..5292d8280 --- /dev/null +++ b/rosserial_test/scripts/generate_client_ros_lib @@ -0,0 +1,62 @@ +#!/usr/bin/env python + +__usage__ = """ +make_libraries generates the rosserial library files. It +is passed the output folder. This version does not copy a ros.h, +as that is provided by the test harnesses. For use only by the +rosserial_test CMake setup. +""" + +import rospkg +from rosserial_client.make_library import * +from shutil import copytree +from os import path, sep, walk +import re + +ROS_TO_EMBEDDED_TYPES = { + 'bool' : ('bool', 1, PrimitiveDataType, []), + 'byte' : ('int8_t', 1, PrimitiveDataType, []), + 'int8' : ('int8_t', 1, PrimitiveDataType, []), + 'char' : ('uint8_t', 1, PrimitiveDataType, []), + 'uint8' : ('uint8_t', 1, PrimitiveDataType, []), + 'int16' : ('int16_t', 2, PrimitiveDataType, []), + 'uint16' : ('uint16_t', 2, PrimitiveDataType, []), + 'int32' : ('int32_t', 4, PrimitiveDataType, []), + 'uint32' : ('uint32_t', 4, PrimitiveDataType, []), + 'int64' : ('int64_t', 8, PrimitiveDataType, []), + 'uint64' : ('uint64_t', 4, PrimitiveDataType, []), + 'float32' : ('float', 4, PrimitiveDataType, []), + 'float64' : ('double', 4, PrimitiveDataType, []), + 'time' : ('ros::Time', 8, TimeDataType, ['ros/time']), + 'duration': ('ros::Duration', 8, TimeDataType, ['ros/duration']), + 'string' : ('char*', 0, StringDataType, []), + 'Header' : ('std_msgs::Header', 0, MessageDataType, ['std_msgs/Header']) +} + +# need correct inputs +if (len(sys.argv) < 2): + print __usage__ + exit() + +# output path +path = path.join(sys.argv[1], 'rosserial') +print "\nExporting to %s" % path + +rospack = rospkg.RosPack() +rosserial_client_copy_files(rospack, path + sep) +rosserial_generate(rospack, path, ROS_TO_EMBEDDED_TYPES) + +# Rewrite includes to find headers in a subdirectory. This is important in the context of +# test nodes as we must distinguish the rosserial client headers from roscpp headers of +# the same name. +for dname, dirs, files in walk(path): + for fname in files: + fpath = os.path.join(dname, fname) + with open(fpath) as f: + s = f.read() + with open(fpath, "w") as f: + f.write(re.sub('^#include "([^"]+)"', + '#include "rosserial/\\1"', + s, flags=re.MULTILINE)) + + diff --git a/rosserial_test/src/publish_subscribe.cpp b/rosserial_test/src/publish_subscribe.cpp new file mode 100644 index 000000000..c2ad02d49 --- /dev/null +++ b/rosserial_test/src/publish_subscribe.cpp @@ -0,0 +1,79 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +namespace rosserial { +#include "rosserial_test/ros.h" +#include "rosserial/std_msgs/String.h" +} + +#include +#include "rosserial_test/fixture.h" +#include "rosserial_test/helpers.h" + +/** + * Single message published from a rosserial-connected client, + * verified from a roscpp Subscriber. + */ +TEST_F(SingleClientFixture, single_publish) { + // Rosserial client set up to publish simple message. + rosserial::std_msgs::String string_msg; + rosserial::ros::Publisher client_pub("chatter", &string_msg); + client_nh.advertise(client_pub); + client_nh.initNode(); + char s[] = "from-rosserial-client"; + string_msg.data = s; + + // Roscpp subscriber to receive the message from the client. + StringCallback str_callback; + ros::Subscriber check_sub = nh.subscribe("chatter", 1, &StringCallback::callback, &str_callback); + + for(int attempt = 0; attempt < 50; attempt++) { + client_pub.publish(&string_msg); + client_nh.spinOnce(); + ros::spinOnce(); + if (str_callback.times_called > 0) break; + ros::Duration(0.1).sleep(); + } + EXPECT_GT(str_callback.times_called, 0); + EXPECT_STREQ(s, str_callback.last_msg.data.c_str()); +} + +int rosserial_string_cb_count = 0; +std::string last_string; + +static void rosserial_string_cb(const rosserial::std_msgs::String& msg) +{ + rosserial_string_cb_count++; + last_string = std::string(msg.data); +} + +/** + * Single message sent from a roscpp Publisher, received + * by a rosserial client subscriber. + */ +TEST_F(SingleClientFixture, single_subscribe) { + rosserial::ros::Subscriber client_sub("chatter", rosserial_string_cb); + client_nh.subscribe(client_sub); + client_nh.initNode(); + + ros::Publisher pub = nh.advertise("chatter", 1); + + std_msgs::String string_msg; + string_msg.data = "to-rosserial-client"; + for(int attempt = 0; attempt < 50; attempt++) { + pub.publish(string_msg); + ros::spinOnce(); + client_nh.spinOnce(); + if (rosserial_string_cb_count > 0) break; + ros::Duration(0.1).sleep(); + } + EXPECT_GT(rosserial_string_cb_count, 0); + EXPECT_EQ(string_msg.data, last_string); +} + +int main(int argc, char **argv){ + ros::init(argc, argv, "test_publish_subscribe"); + ros::start(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/rosserial_test/test/rosserial_python_serial.test b/rosserial_test/test/rosserial_python_serial.test new file mode 100644 index 000000000..22898c9f2 --- /dev/null +++ b/rosserial_test/test/rosserial_python_serial.test @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/rosserial_test/test/rosserial_python_socket.test b/rosserial_test/test/rosserial_python_socket.test new file mode 100644 index 000000000..538554aff --- /dev/null +++ b/rosserial_test/test/rosserial_python_socket.test @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/rosserial_test/test/rosserial_server_serial.test b/rosserial_test/test/rosserial_server_serial.test new file mode 100644 index 000000000..72feaf30c --- /dev/null +++ b/rosserial_test/test/rosserial_server_serial.test @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/rosserial_test/test/rosserial_server_socket.test b/rosserial_test/test/rosserial_server_socket.test new file mode 100644 index 000000000..bc0337212 --- /dev/null +++ b/rosserial_test/test/rosserial_server_socket.test @@ -0,0 +1,12 @@ + + + + + + + + + + +