rostest tool是ROS提供的测试工具,其实质是roslaunch tool的功能扩展,但是使用rostest测试时候通常rostest运行的文件是扩展名为.test的XML格式文件,且文件内包含<test> tag。
注意:roslaunch可以运行扩展名为.test的文件但是不会执行<test>标签内的代码(即测试node不会启动)。
下面主要介绍rostest使用步骤,并借用ROS一些tutorial代码辅助进行分析。
1 编写test node
ROS中test node编写支持用2种框架(C++ gtest框架和python unittest框架)来编写code,需要注意的是对于用gtest编写的code是100%支持,用python unittest编写的code需要稍微封装一下。以下分别简要介绍2种编写test node的方法。
1.1 使用gtest编写test node
关于google test详细用法请参考:https://github.com/google/googletest
通常是在要测试package目录下创建1个子目录test,在test目录下添加测试代码。
(1)简单函数调用方式
#include "test_common.h"
#include <tf/transform_listener.h>
// TEST CASES
TEST_F(DiffDriveControllerTest, testForward)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// zero everything before test
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
ros::Duration(0.1).sleep();
// get initial odom
nav_msgs::Odometry old_odom = getLastOdom();
// send a velocity command of 0.1 m/s
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
// wait for 10s
ros::Duration(10.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
EXPECT_LT(fabs(dz), EPS);
// convert to rpy and test that way
double roll_old, pitch_old, yaw_old;
double roll_new, pitch_new, yaw_new;
tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
EXPECT_LT(fabs(roll_new - roll_old), EPS);
EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
}
TEST_F(DiffDriveControllerTest, testTurn)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// zero everything before test
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
ros::Duration(0.1).sleep();
// get initial odom
nav_msgs::Odometry old_odom = getLastOdom();
// send a velocity command
cmd_vel.angular.z = M_PI/10.0;
publish(cmd_vel);
// wait for 10s
ros::Duration(10.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot rotated PI around z, changes in the other fields should be ~~0
EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS);
EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS);
EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
// convert to rpy and test that way
double roll_old, pitch_old, yaw_old;
double roll_new, pitch_new, yaw_new;
tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
EXPECT_LT(fabs(roll_new - roll_old), EPS);
EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS);
}
TEST_F(DiffDriveControllerTest, testOdomFrame)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the odom frame exist
EXPECT_TRUE(listener.frameExists("odom"));
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}
(2)处理exception
由于gtest并没有处理exception的机制,所以如果你测试的代码会throw exception,你应该自己添加try/catch来捕获exception,然后适当使用宏ADD_FAILURE 或者FAIL 。宏ADD_FAILURE 表示non-fatal失效, 而FAIL表示fatal失效。
#include <stdexcept> // for std::runtime_error
#include <gtest/gtest.h>
#include "map_server/image_loader.h"
#include "test_constants.h"
/* Try to load a valid PNG file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file.
*
* This test can fail on OS X, due to an apparent limitation of the
* underlying SDL_Image library. */
TEST(MapServer, loadValidPNG)
{
try
{
std_srvs::StaticMap::response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false);
EXPECT_FLOAT_EQ(map_resp.map.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.width, g_valid_image_width);
EXPECT_EQ(map_resp.map.height, g_valid_image_height);
for(unsigned int i=0; i < map_resp.map.width * map_resp.map.height; i++)
EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
}
catch(...)
{
ADD_FAILURE() << "Uncaught exception : " << "This is OK on OS X";
}
}
/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file. */
TEST(MapServer, loadValidBMP)
{
try
{
std_srvs::StaticMap::response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false);
EXPECT_FLOAT_EQ(map_resp.map.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.width, g_valid_image_width);
EXPECT_EQ(map_resp.map.height, g_valid_image_height);
for(unsigned int i=0; i < map_resp.map.width * map_resp.map.height; i++)
EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
}
catch(...)
{
ADD_FAILURE() << "Uncaught exception";
}
}
/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file. */
TEST(MapServer, loadValidBMP)
{
try
{
std_srvs::StaticMap::response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false);
EXPECT_FLOAT_EQ(map_resp.map.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.width, g_valid_image_width);
EXPECT_EQ(map_resp.map.height, g_valid_image_height);
for(unsigned int i=0; i < map_resp.map.width * map_resp.map.height; i++)
EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
}
catch(...)
{
ADD_FAILURE() << "Uncaught exception";
}
}
/* Try to load an invalid file. Succeeds if a std::runtime_error exception
* is thrown. */
TEST(MapServer, loadInvalidFile)
{
try
{
std_srvs::StaticMap::response map_resp;
map_server::loadMapFromFile(&map_resp, "foo", 0.1, false);
}
catch(std::runtime_error &e)
{
SUCCEED();
return;
}
catch(...)
{
FAIL() << "Uncaught exception";
}
ADD_FAILURE() << "Didn't throw exception as expected";
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
1.2 使用python unittest编写test node
ROS下使用python unittest框架测试,分为2种情况(ROS-Node-Level集成测试和Code-Level python单元测试)。
(1)ROS-Node-Level集成测试
以下文件test_talker.py,创建test node( test_talker
) ,测试rospy_tutorials下的talker节点的测试code。
#!/usr/bin/env python
import sys, unittest, time
import rospy, rostest
from std_msgs.msg import String
class TestTalker(unittest.TestCase):
def __init__(self, *args):
super(TestTalker, self).__init__(*args)
self.success = False
def callback(self, data):
self.success = data.data and data.data.startswith('hello world')
def test_talker(self):
rospy.init_node('test_talker')
rospy.Subscriber("chatter", String, self.callback)
timeout_t = time.time() + 10.0
while (not rospy.is_shutdown() and
not self.success and time.time() < timeout_t):
time.sleep(0.1)
self.assert_(self.success)
if __name__ == '__main__':
rostest.rosrun('basics', 'talker_test', TestTalker, sys.argv)
说明:
rostest.rosrun(package_name, test_name, test_case_class, sysargs=None)
package_name
- Name of ROS package to record these results as. Test results are aggregated by package name.
test_name
- Name to use for test. This name will be used in the filename of the test results as well as in the XML results reporting.
sysargs
-
Override sys.argv.
coverage_packages=['module1.foo', 'module2.bar']
List of packages that should be included in coverage report.(2) Code-Level python单元测试
以下代码来源nodelet的test/test_loader.py
import roslib; roslib.load_manifest('test_nodelet')
import rospy
from std_msgs.msg import Float64
import unittest
import rostest
import random
class PlusTester:
def __init__(self, topic_in, topic_out, delta):
self.pub = rospy.Publisher(topic_in, Float64)
self.sub = rospy.Subscriber(topic_out, Float64, self.callback)
self.expected_delta = delta
self.send_value = random.random()
self.recieved = False
self.result = False
self.delta = delta
def run(self, cycles = 10):
for i in range(1, cycles):
self.pub.publish(Float64(self.send_value))
rospy.loginfo("Sent %f"%self.send_value);
if self.recieved == True:
break
rospy.sleep(1.0)
return self.result
def callback(self, data):
rospy.loginfo(rospy.get_name()+" I heard %s which was a change of %f",data.data, data.data-self.send_value)
if data.data == self.send_value + self.delta:
self.result = True
self.recieved = True
class TestPlus(unittest.TestCase):
def test_param(self):
pb = PlusTester("Plus/in", "Plus/out", 2.1)
self.assertTrue(pb.run())
def test_default_param(self):
pb = PlusTester("Plus2/in", "Plus2/out", 10.0)
self.assertTrue(pb.run())
def test_standalone(self):
pb = PlusTester("Plus2/out", "Plus3/out", 2.5)
self.assertTrue(pb.run())
def test_remap(self):
pb = PlusTester("plus_remap/in", "remapped_output", 2.1)
self.assertTrue(pb.run())
def test_chain(self):
pb = PlusTester("Plus2/in", "Plus3/out", 12.5)
self.assertTrue(pb.run())
if __name__ == '__main__':
rospy.init_node('plus_local')
rostest.unitrun('test_nodelet', 'test_plus', TestPlus)
rosunit.unitrun(package_name, test_name, test_case_class, sysargs=None, coverage_packages=None)
package_name
- Name of ROS package to record these results as. Test results are aggregated by package name.
test_name
- Name to use for test. This name will be used in the filename of the test results as well as in the XML results reporting.
sysargs
-
Override sys.argv.
coverage_packages=['module1.foo', 'module2.bar']
List of packages that should be included in coverage report.rosunit.unitrun是对python 接口封装,创建XML的测试结果。
以下(3)和(4)是对unittest运行方式的,高级补充说明。
(3)Test suite使用
如果一次运行多个case进行测试,可用test suite
test/my_test_cases.py
import unittest
class CaseA(unittest.TestCase):
def runTest(self):
my_var = True
# do some things to my_var which might change its value...
self.assertTrue(my_var)
class CaseB(unittest.TestCase):
def runTest(self):
my_var = True
# do some things to my_var which might change its value...
self.assertTrue(my_var)
class MyTestSuite(unittest.TestSuite):
def __init__(self):
super(MyTestSuite, self).__init__()
self.addTest(CaseA())
self.addTest(CaseB())
以上代码把case A&&B,加入到test suite中,可以像运行指定的test case一样运行指定的suite
test/run_tests.py
# rosunit
rosunit.unitrun('test_package', 'test_name', 'test.my_test_cases.MyTestSuite')
# rostest
rostest.rosrun('test_package', 'test_name', 'test.my_test_cases.MyTestSuite')
(4)通过名字加载tests
test/my_tests.py
class MyTestCase(unittest.TestCase):
def test_a(self):
self.assertTrue(True)
def test_b(self):
self.assertTrue(True)
运行test case(MyTestCase)的2个test
rosunit.unitrun('test_package', 'test_name', 'test.my_tests.MyTestCase.test_a')
rosunit.unitrun('test_package', 'test_name', 'test.my_tests.MyTestCase.test_b')
或者
from my_tests import MyTestCase
rosunit.unitrun('test_package', 'test_name', MyTestCase)
rosunit.unitrun('test_package', 'test_name', 'test.my_tests.MyTestCase') //等效于上一句
2 编写调用test node启动文件
rostest调用test node的文件通常扩展名是.test,有时可能也会扩展名使用.launch,文件中<test>标签指定要运行的test node。
<test>标签与正常node属性基本一致(例外:没有属性 respawn,output;忽略machine属性),必须属性包括test-name,pkg,type,其它都是可选属性。
如:<test test-name="test_1_2" pkg="mypkg" type="test_1_2.py" time-limit="10.0" args="--test1 --test2" />
3 编写CMakeLists.txt文件
3.1 对于gtest
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(tests_mynode test/mynode.test src/test/test_mynode.cpp [more cpp files])
target_link_libraries(tests_mynode ${catkin_LIBRARIES})
endif()
add_rostest_gtest(target_name test_file source_file+)
3.2对于python unittest
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest(test/mytest.test)
endif()
也可以传递参数或者添加依赖
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest(test/my_first_test.test ARGS myarg1:=true myarg2:=false)
catkin_download_test_data(
${PROJECT_NAME}_saloon.bag
http://downloads.foo.com/bags/saloon.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 01603ce158575da859b8afff5b676bf9)
add_rostest(test/my_second_test.test DEPENDENCIES ${PROJECT_NAME}_saloon.bag)
endif()
4 运行测试
4.1 编译和运行一起自动进行
(1)自动运行全部测试
在build文件夹下运行命令make run_tests,完成二进制测试程序生成,同时会自动调用test目录下的所欲.test文件。
(2)运行单个测试
catkin_make run_tests<TAB><TAB>
(3)测试结果总结
catkin_test_results build/test_results
4.2 使用rostest命令手动运行
rostest -h
Usage: rostest [options] [package] <filename>
Options:
-h, --help show this help message and exit
-t, --text Run with stdout output instead of XML output
--pkgdir=PKG_DIR package dir
--package=PACKAGE package
--results-filename=RESULTS_FILENAME
results_filename
--results-base-dir=RESULTS_BASE_DIR
The base directory of the test results. The test
result file is created in a subfolder name PKG_DIR.
-r, --reuse-master Connect to an existing ROS master instead of spawning
a new ROS master on a custom port
-c, --clear Clear all parameters when connecting to an existing
ROS master (only works with --reuse-master)
如: rostest test_rospy rospy.test
===================以下rostest使用流程的一些辅助内容===================
5 使用现成的Test Nodes进行测试
5.1 hztest test node
rostest package自带test node,用于测试某个node的发布rate,如果topic匹配了specification则成功,默认测试的平均rate。
可用参数~check_intervals (bool, default: False)Check message-to-message interval, instead of just average overall publication rate.
<launch>
<node name="talker" pkg="test_ros" type="talker.py" />
<param name="hztest1/topic" value="chatter" />
<param name="hztest1/hz" value="10.0" />
<param name="hztest1/hzerror" value="0.5" />
<param name="hztest1/test_duration" value="5.0" />
<test test-name="hztest_test" pkg="rostest" type="hztest" name="hztest1" />
</launch>
5.2 paramtest
测试注册到参数服务器的参数是否符合规范。
<!-- Test if a specific parameter is not empty. -->
<test pkg="rostest" type="paramtest" name="paramtest_nonempty"
test-name="paramtest_nonempty">
<param name="param_name_target" value="param_nonempty" />
<param name="test_duration" value="5.0" />
<param name="wait_time" value="20.0" />
</test>
<!-- Test if a specific parameter is empty. -->
<test pkg="rostest" type="paramtest" name="paramtest_empty"
test-name="paramtest_empty">
<param name="param_name_target" value="param_empty" />
<param name="test_duration" value="5.0" />
<param name="wait_time" value="30.0" />
</test>
<!-- Test if a specific parameter carries a specific value. -->
<test pkg="rostest" type="paramtest" name="paramtest_value_specific_correct"
test-name="paramtest_value_specific_correct">
<param name="param_name_target" value="param_value_specific" />
<param name="param_value_expected" value="Opensource Robotics is forever." />
<param name="test_duration" value="5.0" />
<param name="wait_time" value="30.0" />
</test>