Merge remote-tracking branch 'upstream/master'

.travis.yml

@@ -1,5 +1,9 @@
 sudo: false
 language: c++
+cache:
+  apt: true
+  directories:
+    - $HOME/apt-cacher-ng
 python:
   - "2.7"
 compiler:
@@ -17,12 +21,14 @@ env:
     - ROSWS=wstool
     - BUILDER=catkin
     - CATKIN_PARALLEL_TEST_JOBS="-j1 -p1"
-    - ROS_PARALLEL_TEST_JOBS="-j1"
     - USE_DOCKER=true
   matrix:
-    - ROS_DISTRO=indigo  USE_DEB=true EXTRA_DEB="ros-indigo-pr2-gazebo ros-indigo-pr2-arm-kinematics"
-    - ROS_DISTRO=kinetic USE_DEB=true
-    - ROS_DISTRO=melodic USE_DEB=true
+    - ROS_DISTRO=indigo  USE_DEB=true EXTRA_DEB="ros-indigo-pr2-gazebo ros-indigo-pr2-arm-kinematics" TEST_PKGS="pr2eus" NOT_TEST_INSTALL=true
+    - ROS_DISTRO=indigo  USE_DEB=true EXTRA_DEB="ros-indigo-pr2-gazebo ros-indigo-pr2-arm-kinematics" TEST_PKGS="pr2eus_moveit pr2eus_tutorials" NOT_TEST_INSTALL=true
+    - ROS_DISTRO=kinetic USE_DEB=true TEST_PKGS="pr2eus"
+    - ROS_DISTRO=kinetic USE_DEB=true TEST_PKGS="pr2eus_moveit pr2eus_tutorials"
+    - ROS_DISTRO=melodic USE_DEB=true TEST_PKGS="pr2eus"
+    - ROS_DISTRO=melodic USE_DEB=true TEST_PKGS="pr2eus_moveit pr2eus_tutorials"
 matrix:
   fast_finish: true
   # allow_failures:

README.md

@@ -1,9 +1,21 @@
-jsk_pr2eus [![Build Status](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus.png?branch=master)](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus)
+# jsk_pr2eus
+[![Build Status](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus.png?branch=master)](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus)
 
-==========
+[//]: # (!!DO NOT EDIT !!)
 
-| Package            | Indigo (Saucy)                                                                                                                                                                               | Indigo (Trusty)                                                                                                                                                                                | Jade (Trusty)                                                                                                                                                                                  | Jade (Vivid)                                                                                                                                                                                 | Kinetic (Wily)                                                                                                                                                                             | Kinetic (Xenial)                                                                                                                                                                                 |
-|:-------------------|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| jsk_pr2eus (armhf) | [![Build Status](http://build.ros.org/job/Ibin_arm_uShf__jsk_pr2eus__ubuntu_saucy_armhf__binary/badge/icon)](http://build.ros.org/job/Ibin_arm_uShf__jsk_pr2eus__ubuntu_saucy_armhf__binary) | [![Build Status](http://build.ros.org/job/Ibin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary/badge/icon)](http://build.ros.org/job/Ibin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary) | [![Build Status](http://build.ros.org/job/Jbin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary/badge/icon)](http://build.ros.org/job/Jbin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary) | [![Build Status](http://build.ros.org/job/Jbin_arm_uVhf__jsk_pr2eus__ubuntu_vivid_armhf__binary/badge/icon)](http://build.ros.org/job/Jbin_arm_uVhf__jsk_pr2eus__ubuntu_vivid_armhf__binary) | [![Build Status](http://build.ros.org/job/Kbin_arm_uWhf__jsk_pr2eus__ubuntu_wily_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_arm_uWhf__jsk_pr2eus__ubuntu_wily_armhf__binary) | [![Build Status](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary) |
-| jsk_pr2eus (i386)  | [![Build Status](http://build.ros.org/job/Ibin_uS32__jsk_pr2eus__ubuntu_saucy_i386__binary/badge/icon)](http://build.ros.org/job/Ibin_uS32__jsk_pr2eus__ubuntu_saucy_i386__binary)           | [![Build Status](http://build.ros.org/job/Ibin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary/badge/icon)](http://build.ros.org/job/Ibin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary)           | [![Build Status](http://build.ros.org/job/Jbin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary/badge/icon)](http://build.ros.org/job/Jbin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary)           | [![Build Status](http://build.ros.org/job/Jbin_uV32__jsk_pr2eus__ubuntu_vivid_i386__binary/badge/icon)](http://build.ros.org/job/Jbin_uV32__jsk_pr2eus__ubuntu_vivid_i386__binary)           | [![Build Status](http://build.ros.org/job/Kbin_uW32__jsk_pr2eus__ubuntu_wily_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uW32__jsk_pr2eus__ubuntu_wily_i386__binary)           | [![Build Status](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary)             |
-| jsk_pr2eus (amd64) | [![Build Status](http://build.ros.org/job/Ibin_uS64__jsk_pr2eus__ubuntu_saucy_amd64__binary/badge/icon)](http://build.ros.org/job/Ibin_uS64__jsk_pr2eus__ubuntu_saucy_amd64__binary)         | [![Build Status](http://build.ros.org/job/Ibin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary/badge/icon)](http://build.ros.org/job/Ibin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary)         | [![Build Status](http://build.ros.org/job/Jbin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary/badge/icon)](http://build.ros.org/job/Jbin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary)         | [![Build Status](http://build.ros.org/job/Jbin_uV64__jsk_pr2eus__ubuntu_vivid_amd64__binary/badge/icon)](http://build.ros.org/job/Jbin_uV64__jsk_pr2eus__ubuntu_vivid_amd64__binary)         | [![Build Status](http://build.ros.org/job/Kbin_uW64__jsk_pr2eus__ubuntu_wily_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uW64__jsk_pr2eus__ubuntu_wily_amd64__binary)         | [![Build Status](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary)           |
+[//]: # (THIS SECTION IS AUTOMATICALLY GENERATED BY)
+
+[//]: # (rosrun jsk_tools generate_deb_status_table.py jsk_pr2eus)
+
+
+| Package            | Kinetic (Xenial)                                                                                                                                                                                 | Melodic (Bionic)                                                                                                                                                                                 |
+|:-------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| jsk_pr2eus (arm64) | [![Build Status](http://build.ros.org/job/Kbin_uxv8_uXv8__jsk_pr2eus__ubuntu_xenial_arm64__binary/badge/icon)](http://build.ros.org/job/Kbin_uxv8_uXv8__jsk_pr2eus__ubuntu_xenial_arm64__binary) | [![Build Status](http://build.ros.org/job/Mbin_ubv8_uBv8__jsk_pr2eus__ubuntu_bionic_arm64__binary/badge/icon)](http://build.ros.org/job/Mbin_ubv8_uBv8__jsk_pr2eus__ubuntu_bionic_arm64__binary) |
+| jsk_pr2eus (armhf) | [![Build Status](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary) | [![Build Status](http://build.ros.org/job/Mbin_ubhf_uBhf__jsk_pr2eus__ubuntu_bionic_armhf__binary/badge/icon)](http://build.ros.org/job/Mbin_ubhf_uBhf__jsk_pr2eus__ubuntu_bionic_armhf__binary) |
+| jsk_pr2eus (i386)  | [![Build Status](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary)             | ---                                                                                                                                                                                              |
+| jsk_pr2eus (amd64) | [![Build Status](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary)           | [![Build Status](http://build.ros.org/job/Mbin_uB64__jsk_pr2eus__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros.org/job/Mbin_uB64__jsk_pr2eus__ubuntu_bionic_amd64__binary)           |
+
+[//]: #
+
+
+[PR2_joints.pdf](./PR2_joints.pdf) shows the joint names and joint limits for PR2.

pr2eus/package.xml

@@ -36,9 +36,14 @@
   <run_depend>sound_play</run_depend>
 
   <!-- Dependencies needed only for running tests. -->
+  <test_depend>fake_localization</test_depend>
+  <test_depend>map_server</test_depend>
   <test_depend>rostest</test_depend>
   <test_depend>pr2_gazebo</test_depend>
+  <test_depend>pr2_navigation_slam</test_depend>
+  <test_depend>pr2_navigation_config</test_depend>
   <test_depend>robot_state_publisher</test_depend>
+  <test_depend>dwa_local_planner</test_depend>
 
   <export>
     <pr2eus robot-name="pr2" interface-file="${prefix}/pr2-interface.l" />

pr2eus/pr2-interface.l

@@ -267,7 +267,8 @@ Example: (send self :gripper :rarm :position) => 0.00"
    (send self :move-gripper arm 0.0 :effort (* 2000 g) :wait wait)
    (unless joint-action-enable
      (dolist (a (if (eq arm :arms) '(:larm :rarm) (list arm)))
-       (let ((grasp-convex
+       (when objs
+         (let ((grasp-convex
 	      (convex-hull-3d
 	       (flatten
 		(mapcar
@@ -280,7 +281,7 @@ Example: (send self :gripper :rarm :position) => 0.00"
                (and (find-method obj :faces)
                     (not (= (pqp-collision-check grasp-convex obj) 0))))
 	     (if (send obj :parent) (send (send obj :parent) :dissoc obj))
-	     (send robot a :end-coords :assoc obj)))))
+	     (send robot a :end-coords :assoc obj))))))
      ;; (send self :update-robot-state) ;; update state of 'robot' for real robot
      (return-from :start-grasp
        (case arm
@@ -349,9 +350,9 @@ Example: (send self :gripper :rarm :position) => 0.00"
      ))
 
   (:stop-grasp
-   (&optional (arm :arms) &key (wait nil))
+   (&optional (arm :arms) &key ((:gain g) 0.03) (wait nil))
    (prog1
-       (send self :move-gripper arm 0.09 :wait wait)
+       (send self :move-gripper arm 0.09 :effort (* 2000 g) :wait wait)
      (unless joint-action-enable
        (dolist (a (if (eq arm :arms) '(:larm :rarm) (list arm)))
          (dolist (obj (send robot a :end-coords :descendants))
@@ -741,5 +742,5 @@ Example: (send self :gripper :rarm :position) => 0.00"
 (defun use-tilt-laser-obstacle-cloud (enable)
   ;; check if we have service call
   (let ((req (instance topic_tools::MuxSelectRequest :init
-                       :topic (if enable "ground_object_cloud" "empty_cloud"))))
+                       :topic (if enable "tilt_scan_filtered" "empty_cloud"))))
     (ros::service-call "/tilt_laser_mux/select" req)))