From aee7175f5131db55fb16737ea32b41a9bf991010 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Mon, 8 Jan 2024 16:38:52 +0900
Subject: [PATCH] Add

---
 ros/kxr_controller/CMakeLists.txt           | 15 ++++++++
 ros/kxr_controller/package.xml              |  3 ++
 ros/kxr_controller/test/kxr_controller.test | 42 +++++++++++++++++++++
 3 files changed, 60 insertions(+)

diff --git a/ros/kxr_controller/CMakeLists.txt b/ros/kxr_controller/CMakeLists.txt
index 12a6b88e..98efd395 100644
--- a/ros/kxr_controller/CMakeLists.txt
+++ b/ros/kxr_controller/CMakeLists.txt
@@ -61,3 +61,18 @@ target_link_libraries(kxr_controller
 install(TARGETS kxr_controller
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  find_package(roslaunch REQUIRED)
+  if(roslaunch_VERSION VERSION_LESS "1.11.1")
+    message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
+  else()
+    file(GLOB LAUNCH_FILES launch/*.launch)
+    foreach(LAUNCH_FILE ${LAUNCH_FILES})
+      roslaunch_add_file_check(${LAUNCH_FILE})
+    endforeach()
+  endif()
+  add_rostest(test/kxr_controller.test ARGS namespace:="")
+  add_rostest(test/kxr_controller.test ARGS namespace:="robot_a")
+endif()
diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml
index c3e705f8..1ee1b1fa 100644
--- a/ros/kxr_controller/package.xml
+++ b/ros/kxr_controller/package.xml
@@ -31,6 +31,9 @@
   <exec_depend>cmake_modules</exec_depend>
   <exec_depend>kxr_models</exec_depend>
 
+  <test_depend>roslaunch</test_depend>
+  <test_depend>rostest</test_depend>
+
   <export>
   </export>
 </package>
diff --git a/ros/kxr_controller/test/kxr_controller.test b/ros/kxr_controller/test/kxr_controller.test
index 9aac0eba..1e89f261 100644
--- a/ros/kxr_controller/test/kxr_controller.test
+++ b/ros/kxr_controller/test/kxr_controller.test
@@ -1,9 +1,51 @@
 <launch>
 
+  <arg name="namespace" default="" />
+
   <include file="$(find kxr_controller)/launch/kxr_controller.launch">
     <arg name="urdf_path" value="$(find kxr_models)/urdf/kxr_test_head.urdf" />
     <arg name="servo_config_path" value="$(find kxr_models)/config/kxr_test_head_servo_config.yaml" />
     <arg name="namespace" value="robot_a" />
   </include>
 
+  <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
+    <test test-name="test_ros_topics"
+          name="test_ros_topics"
+          pkg="jsk_tools" type="test_topic_published.py"
+          retry="3" >
+      <rosparam>
+        topic_0: current_joint_states
+        timeout_0: 30
+        topic_1: joint_states
+        timeout_1: 30
+        topic_2: kxr_fullbody_controller/follow_joint_trajectory/status
+        timeout_2: 30
+        topic_3: kxr_fullbody_controller/servo_on_off_real_interface/status
+        timeout_3: 30
+        topic_4: kxr_fullbody_controller/servo_on_off/status
+        timeout_4: 30
+      </rosparam>
+    </test>
+  </group>
+
+  <group unless="$(eval len(arg('namespace')) > 0)">
+    <test test-name="test_ros_topics"
+          name="test_ros_topics"
+          pkg="jsk_tools" type="test_topic_published.py"
+          retry="3" >
+      <rosparam>
+        topic_0: current_joint_states
+        timeout_0: 30
+        topic_1: joint_states
+        timeout_1: 30
+        topic_2: kxr_fullbody_controller/follow_joint_trajectory/status
+        timeout_2: 30
+        topic_3: kxr_fullbody_controller/servo_on_off_real_interface/status
+        timeout_3: 30
+        topic_4: kxr_fullbody_controller/servo_on_off/status
+        timeout_4: 30
+      </rosparam>
+    </test>
+  </group>
+
 </launch>