fix hrpsys_gazebo_atlas test not to depend on hrpsys_ros_bridge_tutor…

…ials

.travis.yml

@@ -19,11 +19,6 @@ env:
   global:
     - ROS_PARALLEL_JOBS="-j8 -l1"
     - CATKIN_PARALLEL_TEST_JOBS="-p1 -j8"
-    # hrpsys_gazebo_atlas is compiled only. not tested.
-    # Tests for hrpsys_gazebo_atlas depend on rtmros_tutorials/hrpsys_ros_bridge_tutorials.
-    # Since rtmros_tutorials depends on rtmros_gazebo, these tests are executed in rtmros_tutorials instead of rtmros_gazebo.
-    - BUILD_PKGS=""
-    - TEST_PKGS="eusgazebo hrp2jsk_moveit_config hrp2jsknt_moveit_config hrp2jsknts_moveit_config hrp2w_moveit_config hrpsys_gazebo_msgs hrpsys_gazebo_general samplerobot_moveit_config staro_moveit_config"
   matrix:
     #- ROS_DISTRO=groovy ROSWS=rosws BUILDER=rosbuild USE_DEB=true
     #- ROS_DISTRO=groovy ROSWS=rosws BUILDER=rosbuild USE_DEB=false

hrpsys_gazebo_atlas/test/atlas-hrpsys-ros-bridge-test.l

@@ -1,6 +1,19 @@
 #!/usr/bin/env roseus
 
-(load "package://hrpsys_ros_bridge_tutorials/test/hrpsys-ros-bridge-test-suits.l")
+;; (load "package://hrpsys_ros_bridge_tutorials/test/hrpsys-ros-bridge-test-suits.l")
+
+;; test for :angle-vector
+(defun tmp-test-seq-angle-vector
+  (rob &optional (func))
+  (mapcar #'(lambda (av)
+              (let ((newav (send rob :angle-vector av)))
+                (send *ri* :angle-vector (send rob :angle-vector) 1000)
+                (send *ri* :wait-interpolation)
+                (if func (setq newav (funcall func)))
+                (eps= (distance (send *ri* :state :potentio-vector) newav) 0.0)))
+          (list (instantiate float-vector (length (send rob :angle-vector)))
+                (send rob :reset-pose)))
+  )
 
 (deftest test-seq-angle-vector
   (assert
@@ -19,8 +32,27 @@
 ;; (deftest test-impedance-spring
 ;;   (assert (every #'(lambda (x) (< (abs x) 1.0)) (tmp-test-impedance-spring *atlas* (float-vector 350 -550 900)))))
 
+;; test for simple walking of AutoBalancer.rtc by checking whether the robot tumbled or not
+(defun tmp-test-walk-simple
+  (rob &key (pose-func
+             #'(lambda ()
+                 (send rob :reset-pose)
+                 (send rob :fix-leg-to-coords (make-coords)))))
+  (funcall pose-func)
+  (send *ri* :angle-vector (send rob :angle-vector) 2000)
+  (send *ri* :wait-interpolation)
+  (send *ri* :start-auto-balancer)
+  (send *ri* :go-pos 0.3 0.1 5)
+  (send *ri* :go-velocity 0 0 0)
+  (send *ri* :go-stop)
+  (send *ri* :stop-auto-balancer)
+  (let ((ypr (car (send (send *ri* :state :imucoords) :rpy-angle))))
+    (and (< (rad2deg (abs (elt ypr 1))) 10)
+         (< (rad2deg (abs (elt ypr 2))) 10))
+    ))
+
 (deftest test-walk-simple
   (assert (tmp-test-walk-simple *atlas* :pose-func #'(lambda () (send *atlas* :walk-pose)))))
 
 (warn ";; (run-hrpsys-ros-bridge-test \"atlas\" :robot-model-pkg-name \"hrpsys_gazebo_atlas\") ;; run-all-tests~%")
-(warn ";; (hrpsys-ros-bridge-test-debug-mode \"atlas\" :robot-model-pkg-name \"hrpsys_gazebo_atlas\") ;; setup *ri* and robot without error exit~%")
+(warn ";; (hrpsys-ros-bridge-test-debug-mode \"atlas\" :robot-model-pkg-name \"hrpsys_gazebo_atlas\") ;; setup *ri* and robot without error exit~%")