Add test for :go-pos and :go-pos-unsafe

jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt

@@ -14,6 +14,7 @@ if(CATKIN_ENABLE_TESTING)
     find_package(catkin REQUIRED COMPONENTS rostest roslaunch)
     add_rostest(test/jsk_fetch_gazebo_demo.test)
     add_rostest(test/grasping_objects.test)
+    add_rostest(test/go_pos.test)
     roslaunch_add_file_check(launch/demo.launch)
   endif()
 endif()

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/go_pos.test

@@ -0,0 +1,8 @@
+<launch>
+  <include file="$(find jsk_fetch_gazebo_demo)/launch/demo.launch" >
+    <arg name="run_demo_script" value="false" />
+    <arg name="rviz" value="false" />
+  </include>
+
+  <test test-name="demo_test" pkg="roseus" type="roseus" name="demo_test" args="$(find jsk_fetch_gazebo_demo)/test/test-go-pos.l" time-limit="600"/>
+</launch>

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-go-pos.l

@@ -0,0 +1,36 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+(init-unit-test)
+
+(load "package://fetcheus/fetch-interface.l")
+(fetch-init)
+
+(deftest test-go-pos ()
+  (let ((diff #f(1000 0 0)) pos-now pos-goal)
+    (setq pos-now (send (send *ri* :state :worldcoords "map") :worldpos))
+    (setq pos-goal (v+ pos-now diff))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos-goal (norm diff))
+    ;; go-pos
+    (send *ri* :go-pos (/ (elt diff 0) 1000.0) (/ (elt diff 1) 1000.0) 0)
+    (setq pos-now (send (send *ri* :state :worldcoords "map") :worldpos))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos-goal (norm (v- pos-goal pos-now)))
+    (assert (< (norm (v- pos-goal pos-now)) 500)
+            (format nil "go-pos moves incorrectly : ~A" (norm (v- pos-goal pos-now))))
+    ))
+
+(deftest test-go-pos-unsafe
+  (let ((diff #f(1000 0 0)) pos-now pos-goal)
+    (setq pos-now (send (send *ri* :state :worldcoords "map") :worldpos))
+    (setq pos-goal (v+ pos-now diff))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos-goal (norm diff))
+    ;; go-pos-unsafe
+    (send *ri* :go-pos-unsafe (/ (elt diff 0) 1000.0) (/ (elt diff 1) 1000.0) 0)
+    (setq pos-now (send (send *ri* :state :worldcoords "map") :worldpos))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos-goal (norm (v- pos-goal pos-now)))
+    (assert (< (norm (v- pos-goal pos-now)) 500)
+            (format nil "go-pos moves incorrectly : ~A" (norm (v- pos-goal pos-now))))
+    ))
+
+(run-all-tests)
+(exit)