[NO NOT MERGE FOREVER] https://github.com/start-jsk/jsk_apc/pull/2235

.gitmodules

@@ -2,3 +2,7 @@
 	path = jsk_apc2016_common/python/jsk_apc2016_common/rbo_segmentation
 	url = https://github.com/yuyu2172/rbo-apc-object-segmentation
  	branch = jsk_apc
+[submodule ".travis"]
+	path = .travis
+	url = https://github.com/wkentaro/jsk_travis.git
+	version = install_patch

.travis.rosinstall

@@ -1,18 +1,16 @@
-- git:
-    local-name: jsk-ros-pkg/jsk_common
-    uri: https://github.com/jsk-ros-pkg/jsk_common.git
-    version: master
-- git:
-    local-name: jsk-ros-pkg/jsk_robot
-    uri: https://github.com/jsk-ros-pkg/jsk_robot.git
-    version: master
+# - git:
+#     local-name: jsk-ros-pkg/jsk_robot
+#     uri: https://github.com/jsk-ros-pkg/jsk_robot.git
+#     version: c978f3da06261c77e0dafea2f2e4df072323b555
 - git:
     local-name: FA-I-sensor/force_proximity_ros
     uri: https://github.com/knorth55/FA-I-sensor.git
     version: jsk_apc
-
-# waiting for release of https://github.com/jsk-ros-pkg/jsk_recognition/pull/2118
 - git:
-    local-name: jsk-ros-pkg/jsk_recognition
-    uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
-    version: 4320a60bc06e324c98bc92ed9a9f90491e4d5e5e
+    local-name: FOR_SIMULATION/baxter_common
+    uri: https://github.com/knorth55/baxter_common.git
+    version: jsk_apc
+# - git:
+#     local-name: jsk-ros-pkg/jsk_recognition
+#     uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
+#     version: d774854420af82fe9ea16319d743a2b465d63d78

.travis.yml

@@ -2,35 +2,32 @@ sudo: required
 
 dist: trusty
 
-language: generic
+language: c++ # to avoid using /opt/python from travis
 
-cache:
-  directories:
-    - $HOME/.ccache
-    - $HOME/.cache/pip
-    - $HOME/.ros/data
+# cache:
+#   directories:
+#     - $HOME/.ccache
+#     - $HOME/.cache/pip
+#     - $HOME/.ros/data
 
 env:
   global:
-    - CATKIN_TOOLS_CONFIG_OPTIONS="--blacklist jsk_perception jsk_pcl_ros_utils jsk_pcl_ros resized_image_transport"
+    - CATKIN_TOOLS_CONFIG_OPTIONS="--blacklist imagesift jsk_recognition_msgs jsk_perception jsk_pcl_ros_utils jsk_pcl_ros resized_image_transport checkerboard_detector fetcheus naoqieus jsk_fetch_startup jsk_nao_startup roseus_remote jsk_robot_startup jsk_robot_utils jsk_pr2_calibration pr2_base_trajectory_action jsk_baxter_web peppereus naoeus jsk_baxter_desktop jsk_pepper_startup jsk_pr2_startup jsk_pr2_desktop"
     - NOT_TEST_INSTALL=true
-    - ROS_PARALLEL_JOBS="-j2"
+    - ROS_PARALLEL_JOBS="-j4"
     - ROSDEP_ADDITIONAL_OPTIONS="-n -q --ignore-src --skip-keys=jsk_smart_gui --skip-keys=ros3djs --skip-keys=pr2_calibration_launch --skip-keys=jsk_android_gui_api9 --skip-keys=ros2djs --skip-keys=face_recognition --skip-keys=roslibjs --skip-keys=force_proximity_ros --skip-keys=safe_teleop_base"
     - USE_DEB=false
-    - USE_TRAVIS=true
+    - NUMBER_OF_LOGS_TO_KEEP=10 # default is 3
+    # - USE_TRAVIS=true
+    - DOCKER_IMAGE_JENKINS="jsk-apc-indigo"  # built on Jenkins
   matrix:
     - ROS_DISTRO=indigo
 
-install:
-  - git clone https://github.com/jsk-ros-pkg/jsk_travis.git .travis
-  - (cd .travis && git checkout 40640d45c709bdc7d74a1f8983d51d5df553dbc1)
-
 script:
   # build & test ROS packages
   - source .travis/travis.sh
   # build doc
   - cd $TRAVIS_BUILD_DIR/doc
-  - unset PYTHONPATH  # cleanup for virtualenv
   - source setup.sh
   - make html
 

jsk_arc2017_baxter/euslisp/lib/arc-interface.l

@@ -260,13 +260,16 @@
   (:pick-object-with-movable-region
     (arm movable-region &key (n-trial 1) (n-trial-same-pos 1)
          (do-stop-grasp nil) (grasp-style :suction))
+    (send *ri* :calib-proximity-threshold arm)
     (let (graspingp avs object-index obj-pos obj-cube pinch-yaw)
       ;; TODO: object-index is set randomly
       (setq object-index (random (length (gethash arm object-boxes-))))
       (setq obj-pos
             (send self :get-object-position arm movable-region :object-index object-index))
       (setq obj-cube (send self :bbox->cube (elt (gethash arm object-boxes-) object-index)))
-      (if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) (setq pinch-yaw pi/2) (setq pinch-yaw 0))
+      (if (> (x-of-cube obj-cube) (y-of-cube obj-cube))
+        (setq pinch-yaw (+ pi/2 (caar (send (send obj-cube :worldcoords) :rpy-angle))))
+        (setq pinch-yaw (caar (send (send obj-cube :worldcoords) :rpy-angle))))
       (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm)
       (send *ri* :gripper-servo-on arm)
       ;; Setup arm for picking

jsk_arc2017_baxter/euslisp/lib/baxter-interface.l

@@ -15,11 +15,14 @@
   :super baxter-interface
   :slots (rarm-pressure-threshold-
           larm-pressure-threshold-
+          proximity-threshold-
           right-hand-action-
           pressure-
           finger-flex-
           finger-load-
-          prismatic-load-))
+          prismatic-load-
+          proximities-
+          proximity-init-values-))
 
 (defmethod jsk_arc2017_baxter::baxter-interface
   (:init
@@ -72,6 +75,8 @@
     (setq finger-flex- (make-hash-table))
     (setq prismatic-load- (make-hash-table))
     (setq prismatic-vel- (make-hash-table))
+    (setq proximities- (make-hash-table))
+    (setq proximity-init-values- (make-hash-table))
     (dolist (arm (list :rarm :larm))
       (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
                       std_msgs::Float64
@@ -87,7 +92,13 @@
                       dynamixel_msgs::JointState
                       #'send self :set-prismatic-state arm
                       :groupname groupname)
+      (ros::subscribe (format nil "/gripper_front/limb/~a/proximity_array"
+                              (arm2str arm))
+                      force_proximity_ros::ProximityArray
+                      #'send self :set-proximities arm
+                      :groupname groupname)
       (sethash arm finger-flex- (make-hash-table))
+      (sethash arm proximity-init-values- (make-hash-table))
       (dolist (side (list :right :left))
         (ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state"
                                 (arm2str arm) (symbol2str side))
@@ -153,6 +164,9 @@
   (:set-finger-flex
     (arm side msg)
     (sethash side (gethash arm finger-flex-) (send msg :data)))
+  (:set-proximities
+    (arm msg)
+    (sethash arm proximities- (send msg :proximities)))
 
   ;; Hand interface
   ;; based on naoqi-interface and fetch-interface
@@ -184,6 +198,10 @@
           res)
         nil)
       nil))
+  (:cancel-move-hand()
+    (send right-hand-action- :cancel-goal))
+  (:hand-interpolatingp ()
+    (eq (send right-hand-action- :get-state) ros::*simple-goal-state-active*))
   (:get-finger-flex (arm side)
     (send self :spin-once)
     (gethash side (gethash arm finger-flex-)))
@@ -196,6 +214,22 @@
   (:get-prismatic-vel (arm)
     (send self :spin-once)
     (gethash arm prismatic-vel-))
+  (:get-proximity (arm side &key (raw nil))
+    (send self :spin-once)
+    (let ((rl (gethash :left (gethash arm proximity-init-values-)))
+          (rr (gethash :right (gethash arm proximity-init-values-)))
+          ;;(rm (gethash :middle (gethash arm proximity-init-values-)))
+          (proximities (gethash arm proximities-)))
+      (if (null raw)
+        (cond
+          ;; for sparkfun proximity sensor
+          ((eq side :left)
+           (/ (- (send (elt proximities 0) :average) rl) (expt (/ rl 2500.0) 1.5)))
+          ((eq side :right)
+           (/ (- (send (elt proximities 1) :average) rr) (expt (/ rr 2500.0) 1.5))))
+        (cond
+          ((eq side :left) (send (elt proximities 0) :average))
+          ((eq side :right) (send (elt proximities 1) :average))))))
   (:get-real-finger-av (arm)
     (send self :update-robot-state :wait-until-update t)
     (float-vector
@@ -225,9 +259,55 @@
             ((eq type :pinch)
              (send self :update-robot-state :wait-until-update t)
              (let ((finger-av (send self :get-real-finger-av l/r))
-                   (prev-av (send robot :angle-vector)) avs)
+                   prev-av av avs hand-interpolatingp)
+               ;; rotate arm using proximity sensor
                (setf (aref finger-av 1) 180)
-               (send self :move-hand l/r finger-av 1000)
+               (send self :move-hand l/r finger-av 2000 :wait nil)
+               (dotimes (x 100)
+                 (if (send self :interpolatingp) (return))
+                 (unix::usleep 1000))
+               (while (and
+                        (< (max (send self :get-proximity l/r :right)
+                                (send self :get-proximity l/r :left))
+                           proximity-threshold-)
+                        (setq hand-interpolatingp (send self :hand-interpolatingp)))
+                 (unix::usleep 1000))
+               (when hand-interpolatingp
+                 (send self :cancel-move-hand)
+                 (send self :update-robot-state :wait-until-update t)
+                 (setq prev-av (send robot :angle-vector))
+                 (if (< (send self :get-proximity l/r :right) (send self :get-proximity l/r :left))
+                   (progn
+                     (send robot l/r :wrist-r :joint-angle 45 :relative t)
+                     (setq av (send robot :angle-vector))
+                     (send robot :angle-vector prev-av)
+                     (send self :angle-vector-raw av 2000)
+                     (send self :move-hand l/r finger-av 4000 :wait nil)
+                     (dotimes (x 100)
+                       (if (send self :interpolatingp) (return))
+                       (unix::usleep 1000))
+                     (while (and (< (send self :get-proximity l/r :right)
+                                    (send self :get-proximity l/r :left))
+                                 (send self :interpolatingp))
+                       (unix::usleep 1000)))
+                   (progn
+                     (send robot l/r :wrist-r :joint-angle -45 :relative t)
+                     (setq av (send robot :angle-vector))
+                     (send robot :angle-vector prev-av)
+                     (send self :angle-vector-raw av 2000)
+                     (send self :move-hand l/r finger-av 4000 :wait nil)
+                     (dotimes (x 100)
+                       (if (send self :interpolatingp) (return))
+                       (unix::usleep 1000))
+                     (while (and (> (send self :get-proximity l/r :right)
+                                    (send self :get-proximity l/r :left))
+                                 (send self :interpolatingp))
+                       (unix::uleep 1000)))
+                 (send self :cancel-move-hand)
+                 (send self :cancel-angle-vector)
+                 (send self :update-robot-state :wait-until-update t)
+                 (send self :move-hand l/r finger-av 1000))
+               (setq prev-av (send robot :angle-vector))
                (when (> (aref finger-av 0) 45)
                  ;; if cylindrical and spherical grasp, move other gripper joints
                  (send robot l/r :gripper-p :joint-angle -90)
@@ -236,7 +316,7 @@
                  (pushback (send robot :angle-vector) avs)
                  (send robot :angle-vector prev-av)
                  (send self :angle-vector-sequence-raw avs)
-                 (send self :wait-interpolation)))))))
+                 (send self :wait-interpolation))))))))
   (:stop-grasp
     (&optional (arm :arms) (type :suction))
     (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
@@ -392,7 +472,17 @@
                (setq rarm-pressure-threshold- (- min-pressure 15))))))
     (send self :stop-grasp arm)
     (ros::ros-info "[:calib-pressure-threshold] Threshold r: ~a l: ~a"
-                   rarm-pressure-threshold- larm-pressure-threshold-)))
+                   rarm-pressure-threshold- larm-pressure-threshold-))
+  (:calib-proximity-threshold
+    (&optional (arm :arms))
+    (send self :spin-once)
+    (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
+      (let ((hash-table (make-hash-table)))
+        (sethash :left hash-table (send self :get-proximity l/r :left :raw t))
+        (sethash :right hash-table (send self :get-proximity l/r :right :raw t))
+        (sethash :middle hash-table (send self :get-proximity l/r :middle :raw t))
+        (sethash l/r proximity-init-values- hash-table))
+      (setq proximity-threshold- 100)))) ;; TODO decide proper threshold 100 -> ???
 
 
 (defclass jsk_arc2017_baxter::baxter-moveit-environment

jsk_arc2017_baxter/euslisp/lib/baxter.l

@@ -109,31 +109,32 @@
     (case (car args)
       ((:angle-vector)
        (let ((av (cadr args)) (joints (gethash arm hand-joints-)))
-         (when av
-           (dotimes (i (length av))
-             (send self (elt joints i) :joint-angle (elt av i))
-             (send self (elt joints (+ i (length av))) :joint-angle (elt av i))))
+         (if (and (null (eq (length av) 0)) (null (eq (length av) 2)))
+           (progn (ros::ros-error "length of angle-vector must be 0 or 2.~%") (exit)))
+         (dotimes (i (length av))
+           (send self (elt joints i) :joint-angle (elt av i))
+           (send self (elt joints (+ i (length av))) :joint-angle (elt av i)))
          (mapcar
            #'(lambda (j) (send self j :joint-angle))
            (subseq joints 0 (/ (length joints) 2)))
          ))
-      (t (error ":hand first arg is invalid. args: ~A~%" args))
+      (t (ros::ros-error ":hand first arg is invalid. args: ~A~%" args) (exit))
       ))
   (:hand-grasp-pre-pose
     (arm style)
     (case style
       (:opposed (send self :hand arm :angle-vector #f(0 0)))
       (:spherical (send self :hand arm :angle-vector #f(30 0)))
       (:cylindrical (send self :hand arm :angle-vector #f(90 0)))
-      (t (error ":hand-grasp-pre-pose no such style ~A~%" style))
+      (t (ros::ros-error ":hand-grasp-pre-pose no such style ~A~%" style) (exit))
     ))
   (:hand-grasp-pose
     (arm style &key (angle 180))
     (case style
       (:opposed (send self :hand arm :angle-vector (float-vector 0 angle)))
       (:spherical (send self :hand arm :angle-vector (float-vector 30 angle)))
       (:cylindrical (send self :hand arm :angle-vector (float-vector 90 angle)))
-      (t (error ":hand-grasp-pose no such style ~A~%" style))
+      (t (ros::ros-error ":hand-grasp-pose no such style ~A~%" style) (exit))
     ))
   (:avoid-shelf-pose
     (arm bin)