add tests in jsk_fetch_gazebo_demo

jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt

@@ -8,3 +8,12 @@ install(DIRECTORY euslisp launch config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
 )
+
+if(CATKIN_ENABLE_TESTING)
+  if ("$ENV{ROS_DISTRO}" STRGREATER "lunar")  # >= melodic
+    find_package(catkin REQUIRED COMPONENTS rostest roslaunch)
+    add_rostest(test/jsk_fetch_gazebo_demo.test)
+    add_rostest(test/grasping_objects.test)
+    roslaunch_add_file_check(launch/demo.launch)
+  endif()
+endif()

jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l

@@ -4,6 +4,10 @@
 ;; https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/fetch_gazebo_demo/scripts/demo.py
 
 (require "package://fetcheus/fetch-interface.l")
+;; hotfix for https://github.com/jsk-ros-pkg/jsk_robot/pull/1213#issuecomment-612976152, see https://github.com/jsk-ros-pkg/jsk_roseus/issues/574
+(make-package "GRASPING_MSGS")
+(load "package://roseus/ros/grasping_msgs/msg/Object.l")
+;;
 (ros::load-ros-manifest "grasping_msgs")
 
 ;; Remove these functions after https://github.com/jsk-ros-pkg/jsk_roseus/pull/640 is merged
@@ -51,8 +55,8 @@
   (setq *co* (instance collision-object-publisher :init)))
 
 (defmethod grasping-client
-  (:init ()
-    (let ((find-topic "basic_grasping_perception/find_objects"))
+  (:init (&key (find-topic "basic_grasping_perception/find_objects"))
+    (let ()
       (ros::ros-info (format nil "Waiting for ~A ..." find-topic))
       (setq find-client (instance ros::simple-action-client :init
                                   find-topic

jsk_fetch_robot/jsk_fetch_gazebo_demo/launch/demo.launch

@@ -1,4 +1,5 @@
 <launch>
+  <arg name="run_demo_script" default="true" doc="run euslisp demo script" />
 
   <arg name="rviz" default="true" />
   <include file="$(find fetch_gazebo)/launch/playground.launch" />
@@ -20,7 +21,7 @@
   <!-- Drive to the table, pick stuff up -->
   <!-- <node name="demo" pkg="fetch_gazebo_demo" type="demo.py" output="screen" /> -->
   <node name="gazebo_demo" pkg="jsk_fetch_gazebo_demo" type="demo.l" output="screen"
-        args="&quot;(main)&quot;" />
+        args="&quot;(main)&quot;" if="$(arg run_demo_script)" />
 
   <!-- set rosparams to increase demo success rate -->
   <rosparam>

jsk_fetch_robot/jsk_fetch_gazebo_demo/package.xml

@@ -12,4 +12,15 @@
 
   <exec_depend>fetch_gazebo_demo</exec_depend>
 
+  <!-- to avoid roslaunch-check, this should be include in fetch_gazebo -->
+  <exec_depend>robot_state_pubclisher</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+  <exec_depend>simple_grasping</exec_depend>
+  <exec_depend>fetch_gazebo</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>fetch_moveit_config</exec_depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>roslaunch</test_depend>
+
 </package>

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.test

@@ -0,0 +1,9 @@
+<launch>
+  <group ns="grasp_objects_test" >
+    <node pkg="rosbag" type="play" name="play_grasping_objects_msg"
+          args="-l $(find jsk_fetch_gazebo_demo)/test/grasping_objects.bag" />
+  </group>
+
+  <test test-name="demo_test" pkg="roseus" type="roseus" name="demo_test" args="$(find jsk_fetch_gazebo_demo)/test/test-grasping-objects.l" />
+</launch>
+

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/jsk_fetch_gazebo_demo.test

@@ -0,0 +1,20 @@
+<launch>
+  <include file="$(find jsk_fetch_gazebo_demo)/launch/demo.launch" >
+    <arg name="run_demo_script" value="false" />
+    <arg name="rviz" value="false" />
+  </include>
+
+  <group ns="grasp_objects_test" >
+    <node pkg="rosbag" type="play" name="play_grasping_objects_msg"
+          args="-l $(find jsk_fetch_gazebo_demo)/test/grasping_objects.bag" />
+  </group>
+
+  <param name="joint_states_hz_test/topic" value="joint_states" />
+  <param name="joint_states_hz_test/hz" value="100.0" />
+  <param name="joint_states_hz_test/hzerror" value="25.0" />
+  <param name="joint_states_hz_test/test_duration" value="5.0" />
+  <atest test-name="joint_states_test" pkg="rostest" type="hztest" name="joint_states_hz_test" />
+
+  <test test-name="demo_test" pkg="roseus" type="roseus" name="demo_test" args="$(find jsk_fetch_gazebo_demo)/test/test-demo.l" />
+</launch>
+

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-demo.l

@@ -0,0 +1,35 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+(init-unit-test)
+
+(load "package://jsk_fetch_gazebo_demo/euslisp/demo.l")
+
+(deftest init-and-move ()
+  (let ((pos0 #f(2250 3118 0)) pos1)
+    ;; Create *ri* to use fetch with MoveIt! and Navigation stack
+    (fetch-init)
+
+    ;; Move the base to be in front of the table
+    ;; Demonstrates the use of the navigation stack
+    (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0)))
+    (ros::ros-warn "Moving to table...")
+    ;; first time
+    (send *ri* :move-to (make-coords :pos pos0) :frame-id "map")
+    (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0)))
+    (assert (< (norm  (v- pos1 pos0)) 500)
+            (format nil "go-pos moves incorrectly : ~A" (norm  (v- pos0 pos1))))
+    ;; second time
+    (send *ri* :move-to (make-coords :pos pos0) :frame-id "map")
+    (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos))
+    (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0)))
+    (assert (< (norm  (v- pos1 pos0)) 500)
+            (format nil "go-pos moves incorrectly : ~A" (norm  (v- pos0 pos1))))
+    ))
+
+(run-all-tests)
+(exit)
+
+

jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-grasping-objects.l

@@ -0,0 +1,49 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+(init-unit-test)
+
+;; force set *co* to aovid loading (instance collision-object-publisher :init))) that wait for '/apply_planning_scene' serice
+(defclass dummy-collision-object-publisher :super propertied-object)
+(defmethod dummy-collision-object-publisher
+  (:init () self)
+  (:wipe-all () t)
+  )
+(setq *co* (instance dummy-collision-object-publisher :init))
+;; fake action goal/result id, skip wait-for-server
+(in-package "ROS")
+(defmethod ros::simple-action-client
+  (:wait-for-server (&optional (timeout nil)) t)
+(defmethod actionlib-comm-state
+  (:update-result
+   (msg)
+   (setq latest-goal-status (send msg :status))
+   (setq latest-result msg)
+   (ros::ros-info "[~A] update-result goal id: ~A ~A~%" (send self :name) (send msg :status :goal_id :id) state)
+   (if action-goal (send msg :status :goal_id :id (send action-goal :goal_id :id)))
+   (ros::ros-info "[~A]      replaced goal id: ~A ~A~%" (send self :name) (send msg :status :goal_id :id) state)
+   (send self :update-status
+         (instance actionlib_msgs::GoalStatusArray :init :status_list (list (send msg :status))))
+   (setq state 'ros::*comm-state-done*))
+   (ros::ros-info "[~A] update-result state: ~A ~A" (send self :name) (send msg :status :goal_id :id) state)
+   ))
+(in-package "USER")
+;;
+(load "package://jsk_fetch_gazebo_demo/euslisp/demo.l")
+
+(deftest grasp-client ()
+  (let (ret cube grasps)
+    (setq grasping-client (instance grasping-client :init
+                                    :find-topic "grasp_objects_test/basic_grasping_perception/find_objects"))
+    (ros::ros-warn "Picking object...")
+    (send grasping-client :update-scene)
+    (setq ret (send grasping-client :get-graspable-cube))
+    (ros::ros-warn "Graspable object results: ~A" ret)
+    (setq cube (car ret))
+    (setq grasps (cadr ret))
+    (ros::ros-warn "Graspable cube ~A / grasps" cube grasps)
+    ))
+
+(run-all-tests)
+(exit)
+