add test code for #461

pr2eus/robot-interface.l

@@ -374,7 +374,7 @@
    (let* ((prev-av (send robot :angle-vector)))
      (send-all (gethash ctype controller-table) :push-angle-vector-simulation av tm prev-av)))
   (:angle-vector
-   (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil))
+   (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil) &allow-other-keys)
    "Send joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops.
 - av : joint angle vector [deg]
 - tm : (time to goal in [msec])
@@ -448,7 +448,7 @@
         cacts (send self ctype))))
    av)
   (:angle-vector-sequence
-   (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil))
+   (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil) &allow-other-keys)
    "Send sequence of joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops.
 - avs: sequence of joint angles(float-vector) [deg],  (list av0 av1 ... avn)
 - tms: sequence of duration(float) from previous angle-vector to next goal [msec],  (list tm0 tm1 ... tmn)

pr2eus_moveit/CMakeLists.txt

@@ -31,6 +31,7 @@ if(CATKIN_ENABLE_TESTING)
   find_package(pr2_moveit_config QUIET)
   if(pr2_gazebo_FOUND AND pr2_moveit_config_FOUND)
     add_rostest(test/test-pr2eus-moveit.test)
+    add_rostest(test/test-pr2eus-moveit-sim.test)
   else()
     message(WARNING "pr2_gazebo is not found. Skipping pr2eus_moveit tests")
   endif()

pr2eus_moveit/euslisp/robot-moveit.l

@@ -569,6 +569,11 @@
 - use-send-angle-vector : whether to use :angle-vector in robot-interface for sending trajectory
 - scale : if :fast is specified as total-time, it will use 'scale' times of the planned time
 "
+   (if (send self :simulation-modep)
+     (return-from :angle-vector-motion-plan
+                  (if (listp av)
+                    (send* self :angle-vector-sequence av total-time ctype start-offset-time args)
+                    (send* self :angle-vector av total-time ctype start-offset-time args))))
    (let (traj ret orig-total-time sent-controllers other-joints)
      (setq ctype (or ctype controller-type))
      (unless (gethash ctype controller-table)
@@ -774,6 +779,11 @@
    (av &rest args &key (ctype controller-type) (start-offset-time 0) (total-time) (use-base)
        (move-arm :larm) (reset-total-time 5000.0) (use-send-angle-vector) (scale 1.0)
        &allow-other-keys)
+   (if (send self :simulation-modep)
+     (return-from :angle-vector-motion-plan
+                  (if (listp av)
+                    (send* self :angle-vector-sequence av total-time ctype start-offset-time args)
+                    (send* self :angle-vector av total-time ctype start-offset-time args))))
    (let ((ret (send-super* :angle-vector-motion-plan av
                            :ctype ctype
                            :start-offset-time start-offset-time

pr2eus_moveit/test/test-pr2eus-moveit-sim.l

@@ -0,0 +1,18 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+(init-unit-test)
+
+(ros::load-ros-package "pr2eus_moveit")
+(load "package://pr2eus/pr2-interface.l")
+(load "package://pr2eus_moveit/euslisp/pr2eus-moveit.l")
+
+(pr2-init)
+(deftest test-angle-vector-motion-plan
+  (let (ret)
+    (setq ret (send *ri* :angle-vector-motion-plan (send *pr2* :reset-manip-pose)))
+    (assert ret "angle-vector-motion-plan returns something")
+    ))
+
+(run-all-tests)
+(exit)

pr2eus_moveit/test/test-pr2eus-moveit-sim.test

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="DISPLAY" default="" />
+
+  <env name="DISPLAY" value="$(arg DISPLAY)" />
+
+  <env name="ROSCONSOLE_CONFIG_FILE"
+       value="$(find pr2eus_moveit)/test/rosconsole.conf"/>
+
+  <test pkg="pr2eus_moveit" type="test-pr2eus-moveit-sim.l"
+        test-name="test_pr2eus_moveit_sim"  />
+</launch>