From 632b506b5dbde0b2be9b93428d48ca6dd951e820 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Thu, 16 Apr 2020 14:14:52 +0900
Subject: [PATCH] Add test for :go-pos and :go-pos-unsafe

---
 .../jsk_fetch_gazebo_demo/CMakeLists.txt      |  1 +
 .../jsk_fetch_gazebo_demo/test/go_pos.test    |  8 +++++
 .../jsk_fetch_gazebo_demo/test/test-go-pos.l  | 36 +++++++++++++++++++
 3 files changed, 45 insertions(+)
 create mode 100644 jsk_fetch_robot/jsk_fetch_gazebo_demo/test/go_pos.test
 create mode 100644 jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-go-pos.l

diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt
index f8ab04eead8..37c7ae3d703 100644
--- a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt
+++ b/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()
diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/go_pos.test b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/go_pos.test
new file mode 100644
index 00000000000..d784f9e9be6
--- /dev/null
+++ b/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>
diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-go-pos.l b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-go-pos.l
new file mode 100644
index 00000000000..b94646c9033
--- /dev/null
+++ b/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)