ROS-----rostest tool使用

    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）简单函数调用方式

   例如： 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 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>