Merge branch 'master' into add_result_cb

jsk-ros-pkg · Oct 25, 2022 · 46d738c · 46d738c
2 parents dbbb3d8 + b9fd4fb
commit 46d738c
Show file tree

Hide file tree

Showing 9 changed files with 122 additions and 89 deletions.
diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml
@@ -50,14 +50,16 @@ jobs:
             CONTAINER: ubuntu:18.04
             USE_DEB : true
             TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
-          # - ROS_DISTRO: noetic
-          #   CONTAINER: ubuntu:20.04
-          #   USE_DEB : true
-          #   TEST_PKGS: "pr2eus"
-          # - ROS_DISTRO: noetic
-          #   CONTAINER: ubuntu:20.04
-          #   USE_DEB : true
-          #   TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
+          - ROS_DISTRO: noetic
+            CONTAINER: ubuntu:20.04
+            USE_DEB : true
+            BUILD_PKGS: "pr2eus"
+            TEST_PKGS: "pr2eus"
+          - ROS_DISTRO: noetic
+            CONTAINER: ubuntu:20.04
+            USE_DEB : true
+            BUILD_PKGS: "pr2eus_moveit pr2eus_tutorials"
+            TEST_PKGS: "pr2eus_moveit pr2eus_tutorials"
           - ROS_DISTRO: noetic
             CONTAINER: ubuntu:20.04
             USE_DEB : false

diff --git a/pr2eus/test/pr2-ri-test-base.launch b/pr2eus/test/pr2-ri-test-base.launch
@@ -1,9 +1,12 @@
 <launch>
   <arg name="gui" default="false"/>
+  <env name="DISPLAY" value=":0.0" if="$(arg gui)"/>
+  <env name="DISPLAY" value="" unless="$(arg gui)"/>
+
   <include file="$(find pr2eus)/test/pr2-ri-test-bringup.launch"
            pass_all_args="true" />
 
   <!-- start test -->
-  <test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="1"
+  <test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="3"
         args="$(find pr2eus)/test/pr2-ri-test-base.l" time-limit="800" />
 </launch>
diff --git a/pr2eus/test/pr2-ri-test-bringup.launch b/pr2eus/test/pr2-ri-test-bringup.launch
@@ -63,7 +63,7 @@
     <param name="sensor_timeout" value="1.0"/>
     <param name="publish_tf" value="true"/>
     <param name="odom_used" value="true"/>
-    <param name="imu_used" value="true"/>
+    <param name="imu_used" value="false"/>
     <param name="vo_used" value="false"/>
     <remap from="odom" to="base_odometry/odom" />
     <remap from="imu_data" to="torso_lift_imu/data" />

diff --git a/pr2eus/test/pr2-ri-test-simple.l → ...us/test/pr2-ri-test-simple-angle-vector.l b/pr2eus/test/pr2-ri-test-simple.l → ...us/test/pr2-ri-test-simple-angle-vector.l
@@ -49,31 +49,6 @@
     (assert (eps= tm 1.0))
     ))
 
-;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/165#discussion_r37421484
-(deftest test-go-pos
-  (let ()
-    (setq *ri* (instance pr2-interface :init))
-    (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position
-    (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)")
-    (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)")
-    (ros::sleep 1) ;; wait for 1 sec to activate goal status...
-    (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)")
-    (assert (send *ri* :go-wait) "(send *ri* :go-wait)")
-    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
-    (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)")
-    ))
-
-(deftest test-move-to
-  (let ()
-    (setq *ri* (instance pr2-interface :init))
-    (assert (send *ri* :move-to (make-coords :pos #f(1000 0 0))) "(send *ri* :move-to (make-coords :pos #f(1000 0 0)))") ;; default is world and wait
-    (send *ri* :move-to (make-coords :pos #f(1000 1000 0) :rpy (float-vector pi/2 0 0)))
-    (assert (send *ri* :move-to (make-coords) :no-wait t) "(send *ri* :move-to (make-coords) :no-wait t)") ;; no-wait t means not wait so need to call wait
-    (assert (send *ri* :move-to-wait) "(send *ri* :move-to-wait)") ;; wait move-to
-    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
-    ))
-
-
 ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/143
 (defclass pr2-interface-wrong
   :super pr2-interface)
@@ -158,55 +133,6 @@
       )
     ))
 
-;; pr2 / pr2-sensor-robot test
-(deftest instantiate-pr2-sensor-robot-test
-  (let (rgb-camera-pr1012 rgb-camera-pr1040)
-    ;; call from function
-    (pr2)
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-    (setq rgb-camera-1012 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
-
-    (pr2 :pr1040)
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
-    (setq rgb-camera-1040 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
-    (defun m= (m1 m2) (v= (array-entity m1) (array-entity m2)))
-    (when (m= rgb-camera-1012 rgb-camera-1040)
-      (warning-message 1 "each robot should have different camera param~%~A~%~A~%"
-                       rgb-camera-1040 rgb-camera-1040))
-
-    ;; use instance
-    (setq *pr2* (instance pr2-robot :init))
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (null (assoc 'name (send *pr2* :slots)))) ;; but it does not have slot 'name
-
-    (setq *pr2* (instance pr2-sensor-robot :init))
-    (assert *pr2*) ;; generate *pr2*
-    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-
-    (setq *pr2* (instance pr2-sensor-robot :init :pr1012))
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
-
-    (setq *pr2* (instance pr2-sensor-robot :init :pr1040))
-    (assert *pr2*) ;; generated *pr2*
-    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
-    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
-    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
-    ))
 
 (run-all-tests)
 (exit)
-
-
diff --git a/pr2eus/test/pr2-ri-test-simple-go-pos.l b/pr2eus/test/pr2-ri-test-simple-go-pos.l
@@ -0,0 +1,33 @@
+(require :unittest "lib/llib/unittest.l")
+(load "package://pr2eus/pr2-interface.l")
+
+(init-unit-test)
+
+(pr2-init)
+
+;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/165#discussion_r37421484
+(deftest test-go-pos
+  (let ()
+    (setq *ri* (instance pr2-interface :init))
+    (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position
+    (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)")
+    (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)")
+    (ros::sleep 1) ;; wait for 1 sec to activate goal status...
+    (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)")
+    (assert (send *ri* :go-wait) "(send *ri* :go-wait)")
+    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
+    (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)")
+    ))
+
+(deftest test-move-to
+  (let ()
+    (setq *ri* (instance pr2-interface :init))
+    (assert (send *ri* :move-to (make-coords :pos #f(1000 0 0))) "(send *ri* :move-to (make-coords :pos #f(1000 0 0)))") ;; default is world and wait
+    (send *ri* :move-to (make-coords :pos #f(1000 1000 0) :rpy (float-vector pi/2 0 0)))
+    (assert (send *ri* :move-to (make-coords) :no-wait t) "(send *ri* :move-to (make-coords) :no-wait t)") ;; no-wait t means not wait so need to call wait
+    (assert (send *ri* :move-to-wait) "(send *ri* :move-to-wait)") ;; wait move-to
+    (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
+    ))
+
+(run-all-tests)
+(exit)
diff --git a/pr2eus/test/pr2-ri-test-simple-sensor-model.l b/pr2eus/test/pr2-ri-test-simple-sensor-model.l
@@ -0,0 +1,57 @@
+(require :unittest "lib/llib/unittest.l")
+(load "package://pr2eus/pr2-interface.l")
+
+(init-unit-test)
+
+(pr2-init)
+
+;; pr2 / pr2-sensor-robot test
+(deftest instantiate-pr2-sensor-robot-test
+  (let (rgb-camera-pr1012 rgb-camera-pr1040)
+    ;; call from function
+    (pr2)
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+    (setq rgb-camera-1012 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
+
+    (pr2 :pr1040)
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
+    (setq rgb-camera-1040 (send (send *pr2* :camera :kinect_head/rgb) :viewing :projection))
+    (defun m= (m1 m2) (v= (array-entity m1) (array-entity m2)))
+    (when (m= rgb-camera-1012 rgb-camera-1040)
+      (warning-message 1 "each robot should have different camera param~%~A~%~A~%"
+                       rgb-camera-1040 rgb-camera-1040))
+
+    ;; use instance
+    (setq *pr2* (instance pr2-robot :init))
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (null (assoc 'name (send *pr2* :slots)))) ;; but it does not have slot 'name
+
+    (setq *pr2* (instance pr2-sensor-robot :init))
+    (assert *pr2*) ;; generate *pr2*
+    (assert (derivedp *pr2* pr2-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+
+    (setq *pr2* (instance pr2-sensor-robot :init :pr1012))
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1012)) ;; but we can know serial number
+
+    (setq *pr2* (instance pr2-sensor-robot :init :pr1040))
+    (assert *pr2*) ;; generated *pr2*
+    (assert (derivedp *pr2* pr2-sensor-robot)) ;; pr2 derived from pr2-sensor-robot
+    (assert (equal (send *pr2* :name) "pr2")) ;; robot name is pr2
+    (assert (equal (*pr2* . name) :pr1040)) ;; but we can know serial number
+    ))
+
+(run-all-tests)
+(exit)
diff --git a/pr2eus/test/pr2-ri-test-simple.launch b/pr2eus/test/pr2-ri-test-simple.launch
@@ -1,5 +1,13 @@
 <launch>
+  <arg name="gui" default="false"/>
+  <env name="DISPLAY" value=":0.0" if="$(arg gui)"/>
+  <env name="DISPLAY" value="" unless="$(arg gui)"/>
+
   <!-- start test -->
-  <test test-name="pr2_ri_test_simple_no_gazebo" pkg="roseus" type="roseus"
-	args="$(find pr2eus)/test/pr2-ri-test-simple.l" time-limit="1800" />
+  <test test-name="pr2_ri_test_simple_no_gazebo_angle_vector" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-angle-vector.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple_no_gazebo_go_pos" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-go-pos.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple_no_gazebo_sensor_model" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-sensor-model.l" time-limit="800" />
 </launch>
diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch
@@ -4,6 +4,10 @@
            pass_all_args="true" />
 
   <!-- start test -->
-  <test test-name="pr2_ri_test_simple" pkg="roseus" type="roseus" retry="1"
-        args="$(find pr2eus)/test/pr2-ri-test-simple.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple_angle_vector" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-angle-vector.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple_go_pos" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-go-pos.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple_sensor_model" pkg="roseus" type="roseus" retry="3"
+        args="$(find pr2eus)/test/pr2-ri-test-simple-sensor-model.l" time-limit="800" />
 </launch>
diff --git a/pr2eus_moveit/test/test-pr2eus-moveit.l b/pr2eus_moveit/test/test-pr2eus-moveit.l
@@ -290,7 +290,7 @@
     (send co :add-object r)
     ;; move left arm between blocks
     (send *ri* :move-end-coords-plan (make-coords :pos #f(700 0 700)) :move-arm :larm)
-    (ros::rate 10)
+    (ros::rate 1)
     (while (not (send *ri* :interpolatingp))
       (send *ri* :spin-once)
       (ros::sleep))