Test with Gazebo

jsk_kinova_robot/kinovaeus/CMakeLists.txt

@@ -62,7 +62,7 @@ if(CATKIN_ENABLE_TESTING)
   # Test with Kinematic Simulator
   add_rostest(test/test-gen3.test)
   add_rostest(test/test-gen3-lite.test)
-  # TODO: Test with Gazebo
-  # add_rostest(test/test-gen3-gazebo.test)
-  # add_rostest(test/test-gen3-lite-gazebo.test)
+  # Test with Gazebo and MoveIt!
+  add_rostest(test/test-gen3-moveit.test)
+  add_rostest(test/test-gen3-lite-moveit.test)
 endif()

jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.l

@@ -0,0 +1,31 @@
+#!/usr/bin/env roseus
+(require :unittest "lib/llib/unittest.l")
+(require "package://kinovaeus/gen3-lite-utils.l")
+(require "package://kinovaeus/gen3-lite-interface.l")
+
+(init-unit-test)
+
+(gen3-lite-init)
+
+(deftest test-gen3-lite-move-moveit
+
+  (send *gen3-lite* :reset-pose)
+  (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *ri* :start-grasp :wait t)
+
+  (send *gen3-lite* :angle-vector #f(90.0 0.0 25.0 0.0 0.0 0.0 0.0))
+  (assert (send *ri* :angle-vector-raw (send *gen3-lite* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *gen3-lite* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi  0 pi)) :debug-view t)
+  (assert (send *ri* :angle-vector (send *gen3-lite* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *ri* :stop-grasp :wait t)
+  )
+
+
+(run-all-tests)
+(exit)

jsk_kinova_robot/kinovaeus/test/test-gen3-lite-gazebo.test → jsk_kinova_robot/kinovaeus/test/test-gen3-lite-moveit.test

@@ -4,7 +4,10 @@
     <arg name="gripper" value="gen3_lite_2f" />
     <arg name="gazebo_gui" value="false" />
     <arg name="start_rviz" value="false" />
+    <!-- TODO: Wait for https://github.com/Kinovarobotics/ros_kortex/pull/141 -->
+    <arg name="enable_test" value="false" />
   </include>
 
-  <test pkg="kinovaeus" type="test-gen3-lite.l" test-name="gen3_lite_lisp" time-limit="300" />
+  <test pkg="kinovaeus" type="test-gen3-lite-moveit.l" test-name="gen3_lite_move_moveit"
+        time-limit="300" retry="2" />
 </launch>

jsk_kinova_robot/kinovaeus/test/test-gen3-lite.test

@@ -1,3 +1,3 @@
 <launch>
-  <test pkg="kinovaeus" type="test-gen3-lite.l" test-name="gen3_lite_lisp" time-limit="300" />
+  <test pkg="kinovaeus" type="test-gen3-lite.l" test-name="gen3_lite_move" time-limit="100" />
 </launch>

jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.l

@@ -0,0 +1,31 @@
+#!/usr/bin/env roseus
+(require :unittest "lib/llib/unittest.l")
+(require "package://kinovaeus/gen3-utils.l")
+(require "package://kinovaeus/gen3-interface.l")
+
+(init-unit-test)
+
+(gen3-init)
+
+(deftest test-gen3-move-moveit
+
+  (send *gen3* :reset-pose)
+  (assert (send *ri* :angle-vector (send *gen3* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *ri* :start-grasp :wait t)
+
+  (send *gen3* :angle-vector #f(90.0 0.0 25.0 0.0 0.0 0.0 0.0))
+  (assert (send *ri* :angle-vector-raw (send *gen3* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *gen3* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector pi  0 pi)) :debug-view t)
+  (assert (send *ri* :angle-vector (send *gen3* :angle-vector)))
+  (send *ri* :wait-interpolation)
+
+  (send *ri* :stop-grasp :wait t)
+  )
+
+
+(run-all-tests)
+(exit)

jsk_kinova_robot/kinovaeus/test/test-gen3-gazebo.test → jsk_kinova_robot/kinovaeus/test/test-gen3-moveit.test

@@ -4,7 +4,10 @@
     <arg name="gripper" value="robotiq_2f_85" />
     <arg name="gazebo_gui" value="false" />
     <arg name="start_rviz" value="false" />
+    <!-- TODO: Wait for https://github.com/Kinovarobotics/ros_kortex/pull/141 -->
+    <arg name="enable_test" value="false" />
   </include>
 
-  <test pkg="kinovaeus" type="test-gen3.l" test-name="gen3_lisp" time-limit="300" />
+  <test pkg="kinovaeus" type="test-gen3-moveit.l" test-name="gen3_move_moveit"
+        time-limit="300" retry="2" />
 </launch>

jsk_kinova_robot/kinovaeus/test/test-gen3.test

@@ -1,3 +1,3 @@
 <launch>
-  <test pkg="kinovaeus" type="test-gen3.l" test-name="gen3_lisp" time-limit="300" />
+  <test pkg="kinovaeus" type="test-gen3.l" test-name="gen3_move" time-limit="100" />
 </launch>