add test code and meta package

gundam_robot/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gundam_robot)
+find_package(catkin REQUIRED)
+catkin_metapackage()

gundam_robot/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gundam_robot</name>
+  <version>0.0.1</version>
+  <description>gundam_robot is a meta package for GUNDAM RX-78 robot controller and simulator</description>
+
+  <maintainer email="kei.okada@gmail.com">Kei Okada</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/gundam_robot</url>
+
+  <author email="kei.okada@gmail.com">Kei Okada</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>gundam_rx78_control</exec_depend>
+  <exec_depend>gundam_rx78_description</exec_depend>
+  <exec_depend>gundam_rx78_gazebo</exec_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+
+</package>

gundam_rx78_control/CMakeLists.txt

@@ -1,13 +1,13 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(gundam_rx78_control)
 
-find_package(catkin REQUIRED COMPONENTS pluginlib roslint)
+find_package(catkin REQUIRED COMPONENTS controller_interface joint_trajectory_controller pluginlib)
 
 ###################################
 ## catkin specific configuration ##
 ###################################
 catkin_package(
-  CATKIN_DEPENDS pluginlib
+  CATKIN_DEPENDS controller_interface joint_trajectory_controller pluginlib
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
   )
@@ -45,7 +45,7 @@ install(FILES joint_trajectory_controller.xml
 ## Testing ##
 #############
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS roslaunch)
+  find_package(catkin REQUIRED COMPONENTS roslaunch roslint)
   file(GLOB LAUNCH_FILES launch/*.launch)
   foreach(LAUNCH_FILE ${LAUNCH_FILES})
     roslaunch_add_file_check(${LAUNCH_FILE})

gundam_rx78_control/package.xml

@@ -23,6 +23,7 @@
 
   <!-- for joint_trajectory_controller -->
   <depend>controller_interface</depend>
+  <depend>joint_trajectory_controller</depend>
   <depend>pluginlib</depend>
 
   <test_depend>roslaunch</test_depend>

gundam_rx78_gazebo/CMakeLists.txt

@@ -20,9 +20,14 @@ install(DIRECTORY launch worlds
 ## Testing ##
 #############
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS roslaunch)
+  find_package(catkin REQUIRED COMPONENTS roslaunch roslint rostest)
   file(GLOB LAUNCH_FILES launch/*.launch)
   foreach(LAUNCH_FILE ${LAUNCH_FILES})
     roslaunch_add_file_check(${LAUNCH_FILE})
   endforeach()
+
+  add_rostest(test/test_gundam_spawn.launch)
+  set(ROSLINT_PYTHON_OPTS --max-line-length=180)
+  roslint_python()
+  roslint_add_test()
 endif()

gundam_rx78_gazebo/package.xml

@@ -22,6 +22,8 @@
   <exec_depend>gundam_rx78_description</exec_depend>
 
   <test_depend>roslaunch</test_depend>
+  <test_depend>roslint</test_depend>
+  <test_depend>rostest</test_depend>
 
   <export/>
 

gundam_rx78_gazebo/test/check_initial_pose.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Kei Okada
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the Rethink Robotics nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import unittest
+import rospy
+import time
+from nav_msgs.msg import Odometry
+from std_srvs.srv import Empty
+from tf.transformations import euler_from_quaternion
+
+
+class TestInitialPose(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(self):
+        rospy.init_node('test_initial_pose', anonymous=True)
+        self.base_success = False
+
+    def base_link_cb(self, msg):
+        p = msg.pose.pose.position
+        r = msg.pose.pose.orientation
+        pos = [p.x, p.y, p.z]
+        rot = euler_from_quaternion([r.x, r.y, r.z, r.w])
+
+        rospy.loginfo("pos: {:6.3f} {:6.3f} {:6.3f} - ".format(pos[0], pos[1], pos[2]) +
+                      "rot: {:6.3f} {:6.3f} {:6.3f}".format(rot[0], rot[1], rot[2]))
+
+        # check position
+        if (abs(pos[0]) < 1.0 and abs(pos[1]) < 1.0 and abs(pos[2] - 10) < 2.0 and
+                abs(rot[0]) < 1.0 and abs(rot[1]) < 1.0 and abs(rot[2]) < 0.5):
+            self.base_success = True
+
+    def test_initial_pose(self):
+        rospy.Subscriber("/base_link_ground_truth", Odometry, self.base_link_cb)
+        timeout_t = time.time() + 15
+        while not rospy.is_shutdown() and not self.base_success and time.time() < timeout_t:
+            time.sleep(1.0)
+        self.assertTrue(self.base_success)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun('gundam_rx78_gazebo', 'test_initial_pose', TestInitialPose)

gundam_rx78_gazebo/test/start_simulator.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Kei Okada
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the Rethink Robotics nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import time
+import sys
+from std_srvs.srv import Empty
+from gazebo_msgs.srv import GetPhysicsProperties, GetPhysicsPropertiesResponse
+
+
+class StartGazeboSimulator:
+
+    def __init__(self):
+        self.physics_properties = GetPhysicsPropertiesResponse()
+        self.physics_properties.pause = True
+        rospy.loginfo(self.physics_properties)
+        rospy.wait_for_service('/gazebo/get_physics_properties')
+        self.get_physics_properties_service = rospy.ServiceProxy('/gazebo/get_physics_properties', GetPhysicsProperties)
+        rospy.wait_for_service('/gazebo/unpause_physics')
+        self.start_simulation = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+
+    def call_start_simulation(self):
+        rospy.logwarn("start gazebo simulation")
+        self.start_simulation()
+        time.sleep(1.0)  # use wall clock just in case
+        self.physics_properties = self.get_physics_properties_service()
+        rospy.loginfo(self.physics_properties)
+
+
+if __name__ == '__main__':
+    rospy.init_node('start_simulator', anonymous=True)
+    rospy.logwarn("wait for gazebo startup")
+    time.sleep(5.0)  # wait for gazebo startup
+    start_gazebo_simulator = StartGazeboSimulator()
+
+    # start simulator when pouse is true
+    while not rospy.is_shutdown() and start_gazebo_simulator.physics_properties.pause is True:
+        try:
+            start_gazebo_simulator.call_start_simulation()
+        except Exception as e:
+            print("Unexpected error:", e)
+            pass

gundam_rx78_gazebo/test/test_gundam_spawn.launch

@@ -0,0 +1,23 @@
+<launch>
+
+  <!-- start gazebo with an empty plane -->
+  <include file="$(find gundam_rx78_gazebo)/launch/gundam_rx78_world.launch" >
+    <arg name="gui" default="false"/>
+    <arg name="headless" default="true"/>
+  </include>
+
+  <node name="start_simulator" pkg="gundam_rx78_gazebo" type="start_simulator.py" />
+
+  <!-- test joint state -->
+  <param name="joint_states_hz_test/topic" value="joint_states" />
+  <param name="joint_states_hz_test/hz" value="50.0" />
+  <param name="joint_states_hz_test/hzerror" value="15.0" />
+  <param name="joint_states_hz_test/test_duration" value="5.0" />
+  <test test-name="joint_states_test" pkg="rostest" type="hztest" name="joint_states_hz_test" />
+
+  <!-- test initial pose -->
+  <test test-name="check_initial_pose" pkg="gundam_rx78_gazebo"
+        type="check_initial_pose.py" time-limit="20" />
+
+</launch>
+