Add publisher test

.travis.yml

@@ -42,7 +42,8 @@ script:
   # Run the tests, ensuring the path is set correctly.
   - source devel/setup.bash
   # Run tests
-  - catkin_make run_tests && catkin_test_results
+  # NOTE: GAZEBO TESTS FAIL IN PARALLEL
+  - catkin_make run_tests -j1 && catkin_test_results
   # Lint package files ONLY for create_autonomy
   - sudo apt-get install python-catkin-lint
   - catkin lint -W2 --strict --explain $CREATE_AUTONOMY_SRC --ignore plugin_missing_install --ignore target_name_collision

ca_gazebo/CMakeLists.txt

@@ -229,5 +229,7 @@ roslint_cpp()
 
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
+  # Tests list
   add_rostest(test/hz.test)
+  add_rostest(test/publish.test)
 endif()

ca_gazebo/launch/create_maze.launch

@@ -1,5 +1,7 @@
 <!-- https://github.com/tu-darmstadt-ros-pkg/hector_nist_arenas_gazebo -->
 <launch>
+  <arg name="gui" default="$(optenv GUI true)" doc="Start gzclient (Gazebo's GUI)"/>
+
   <param name="create1/amcl/initial_pose_x" value="5"/>
   <param name="create1/amcl/initial_pose_y" value="5"/>
   <param name="create1/amcl/initial_pose_a" value="1.5708"/>
@@ -10,5 +12,6 @@
 
 	<include file="$(find ca_gazebo)/launch/create_empty_world.launch">
     <arg name="env" value="maze"/>
+    <arg name="gui" value="$(arg gui)"/>
 	</include>
-</launch>
+</launch>

ca_gazebo/test/publish.test

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="gui" default="false"/>
+
+  <include file="$(find ca_gazebo)/launch/create_maze.launch">
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+
+  <include file="$(find ca_node)/launch/state_machine.launch"/>
+
+  <test test-name="publishtest" name="publishtest"
+        type="publish.py" pkg="ca_gazebo"
+        time-limit="100.0"> <!-- time-limit >= timeout_s * number of topics to test -->
+    <param name="timeout_s" value="30.0"/>
+  </test>
+</launch>

ca_gazebo/test/scripts/publish.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+import rospy
+import rostest
+import unittest
+import sys
+
+from ca_msgs.msg import Bumper, Cliff
+
+__author__ = 'Emiliano Borghi'
+
+PKG = 'ca_gazebo'
+NAME = 'plugins'
+
+class PluginsTesterClass(unittest.TestCase):
+
+  def __init__(self, *args):
+    # Init ROS and params
+    rospy.init_node(NAME, anonymous=True)
+    self.timeout = rospy.get_param("~timeout_s", default=10.0)
+    rospy.loginfo("Timeout set to {} seconds".format(self.timeout))
+    # Call TestCase class
+    super(PluginsTesterClass, self).__init__(*args)
+
+  def check(self, topic, msg_type, callback):
+    self.message_triggered = False
+    sub = rospy.Subscriber(topic, msg_type, callback)
+
+    rate_hz = rospy.Rate(20)  # 20 Hz
+    timeout_t = rospy.get_time() + self.timeout
+    while (not rospy.is_shutdown()) and \
+          (rospy.get_time() <= timeout_t) and \
+          (not self.message_triggered):
+      rate_hz.sleep()
+    # Unregister subscriber
+    sub.unregister()
+
+    return self.message_triggered
+
+  def test_bumper(self): # test names must start with 'test_'
+    res = self.check('/create1/bumper', Bumper, self.bumper_cb)
+
+    self.assertTrue(res, msg="Bumper message not received")
+
+  def bumper_cb(self, msg):
+    self.message_triggered |= (msg.is_left_pressed or msg.is_right_pressed)
+
+  def test_cliff(self):
+    res = self.check('/create1/cliff', Cliff, self.cliff_cb)
+
+    self.assertFalse(res, msg="Cliff message not received")
+
+  def cliff_cb(self, msg):
+    self.message_triggered |= \
+        (msg.is_cliff_left or msg.is_cliff_front_left or \
+         msg.is_cliff_front_right or msg.is_cliff_right)
+
+if __name__ == '__main__':
+  rostest.rosrun(PKG, NAME, PluginsTesterClass, sys.argv)