例如： ros_controllers / diff_drive_controller / test / diff_drive_test.cpp

#include "test_common.h"
#include <tf/transform_listener.h>// TEST CASES
TEST_F(DiffDriveControllerTest, testForward)
{// wait for ROSwhile(!isControllerAlive()){ros::Duration(0.1).sleep();}// zero everything before testgeometry_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 odomnav_msgs::Odometry old_odom = getLastOdom();// send a velocity command of 0.1 m/scmd_vel.linear.x = 0.1;publish(cmd_vel);// wait for 10sros::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 ~~0const 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 waydouble 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 ROSwhile(!isControllerAlive()){ros::Duration(0.1).sleep();}// zero everything before testgeometry_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 odomnav_msgs::Odometry old_odom = getLastOdom();// send a velocity commandcmd_vel.angular.z = M_PI/10.0;publish(cmd_vel);// wait for 10sros::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 ~~0EXPECT_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 waydouble 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 ROSwhile(!isControllerAlive()){ros::Duration(0.1).sleep();}// set up tf listenertf::TransformListener listener;ros::Duration(2.0).sleep();// check the odom frame existEXPECT_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 pythonimport sys, unittest, time
import rospy, rostest
from std_msgs.msg import Stringclass TestTalker(unittest.TestCase):def __init__(self, *args):super(TestTalker, self).__init__(*args)self.success = Falsedef 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.0while (not rospy.is_shutdown() andnot 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 randomclass 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 = deltaself.send_value = random.random()self.recieved = Falseself.result = Falseself.delta = deltadef 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:breakrospy.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 = Trueself.recieved = Trueclass 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 unittestclass 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 MyTestCaserosunit.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.baghttp://downloads.foo.com/bags/saloon.bagDESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/testMD5 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>