[WIP] Merge Fetch15 branch to master

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -32,7 +32,10 @@
      ))
   (:state (&rest args)
    ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency"
-   (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t)))))
+   (send-super* :state
+                (if (or (eq (car args) :worldcoords)
+                        (member :wait-until-update args))
+                    args (append args (list :wait-until-update t)))))
   (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged
    (diff-av)
    (let ((i 0) add-new-trajectory-point)
@@ -256,12 +259,13 @@ Example: (send self :gripper :position) => 0.00"
              (forward-p (>= x 0))
              (d0 (if forward-p (atan2 y x) (atan2 (- y) (- x))))
              (d1 (distance (float-vector 0 0) (float-vector x y)))
-             (d2 (shortest-angle (- d d0) 0))
+             (d2 (shortest-angle (- (deg2rad d) d0) 0))
              org-cds cur-cds diffrot diffpos err)
         (send self :remprop :go-pos-unsafe-no-wait-goal)
 
         ;; yaw-x-yaw approach
-        (ros::ros-info "go-pos-unsafe: (yaw, x, yaw) = (~A ~A ~A)" d0 d1 d2)
+        (ros::ros-info "go-pos-unsafe: (yaw, x, yaw) = (~A ~A ~A)"
+                       d0 (* (if forward-p 1.0 -1.0) d1) d2)
 
         ;; yaw
         (send self :spin-once)
@@ -329,6 +333,10 @@ Example: (send self :gripper :position) => 0.00"
                 d-vel (* (if (>= d-vel 0) 1.0 -1.0) min-rotation-abs-vel))))
           (ros::sleep))
         t)))
+  (:move-to-wait (&rest args)
+    (send-super* :move-to-wait
+                 (if (member :correction args) args
+                     (append args (list :correction nil)))))
 ) ;; fetch-interface (simple base actions)
 
 (defun fetch-init (&optional (create-viewer))

jsk_fetch_robot/jsk_fetch.rosinstall

@@ -6,11 +6,12 @@
     local-name: PR2/app_manager
     uri: https://github.com/PR2/app_manager.git
     version: 1.1.0
-# For fetch to use twitter
+# serious bug in 1.12.21
+# need to build from source because of nodelet version
 - git:
-    local-name: furushchev/image_pipeline
-    uri: https://github.com/furushchev/image_pipeline.git
-    version: develop
+    local-name: ros-perception/image_pipeline
+    uri: https://github.com/ros-perception/image_pipeline.git
+    version: 1.12.23
 # To send lifelog data to musca
 - git:
     local-name: strands-project/mongodb_store
@@ -23,46 +24,66 @@
     uri: https://github.com/jsk-ros-pkg/jsk_robot.git
     version: master
 # to pass build of jsk_robot
-# remove after 2.2.10 is released
+# to build with nodelet 1.9.10, we need https://github.com/jsk-ros-pkg/jsk_common/pull/1647
+# remove after 2.2.11 is released
 - git:
     local-name: jsk-ros-pkg/jsk_common
-    uri: https://github.com/jsk-ros-pkg/jsk_common.git
-    version: 2.2.10
-# to avoid volume 0 problem
+    uri: https://github.com/knorth55/jsk_common.git
+    version: 80be0920e2518a4fd49b7da1e96e69802cc49312
+# use latest version to avoid several problems
 # remove after 0.3.14 (https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/41183fe3401d742bbec0edd13b67cb909a6968bd) is released
 - git:
     local-name: jsk-ros-pkg/jsk_pr2eus
     uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
-    version: 0.3.14
+    version: 4102b8d3a542b9e93b4115065b2bea2494092abc
 # to use fetch's dock position for several demos
 # remove after https://github.com/jsk-ros-pkg/jsk_demos/commit/810acc7cc24a4792d455a7e9e8c8c50e5e07e21b (0.0.5) is released
 # Now, we set CATKIN_IGNORE to all packages except jsk_maps because we need only jsk_maps in fetch PC
 - git:
     local-name: jsk-ros-pkg/jsk_demos
     uri: https://github.com/jsk-ros-pkg/jsk_demos.git
-    version: 810acc7
-# jsk_topic_tools requires nodelet gte 1.9.11
-# remove after 1.9.11 is released by apt
+    version: 1f81e61c890e0642251ce0ec12952eb0828aadca
+# need to build from source because of nodelet version
 - git:
-    local-name: ros/nodelet_core
-    uri: https://github.com/ros/nodelet_core.git
-    version: 1.9.11
-# 0.3.x is needed to set volume in pr2eus/speak.l
-# remove after 0.3.x is released by apt
+    local-name: jsk-ros-pkg/jsk_recognition
+    uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
+    version: ccdb666ed109e565e89bd35ef7dfa14999550da0
+# 0.3.4 is needed to set volume in pr2eus/speak.l
+# remove after 0.3.4 is released by apt
 - git:
     local-name: ros-drivers/audio_common
     uri: https://github.com/ros-drivers/audio_common.git
-    version: 0.3.3
+    version: 0.3.4
 # to install nodelet_plugins.xml
 # remove after 1.3.10 is released by apt
 - git:
     local-name: ros-perception/slam_gmapping
     uri: https://github.com/ros-perception/slam_gmapping.git
     version: 1.3.10
-# indigo is already EOL and 2.1.13 is never released.
+# indigo is already EOL and 2.1.15 is never released.
 # set the same version as https://github.com/jsk-ros-pkg/jsk_robot/blob/master/.travis.rosinstall.indigo#L7-L11
-# change to 2.1.14 when it is released.
+# change to 2.1.15 when it is released.
 - git:
     local-name: jsk-ros-pkg/jsk_3rdparty
     uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
-    version: 82e897dcbdcd6aa0cbd126fa122d4dbdc9df67c9
+    version: ffbd7b0592d7dd2f830f185636a1dd1695d8cb44
+# indigo is already EOL and fetch_ros is never released.
+- git:
+    local-name: fetchrobotics/fetch_ros
+    uri: https://github.com/fetchrobotics/fetch_ros.git
+    version: c152f9349b26c60d3dde1b4973073bfa187920fe
+# indigo is already EOL and visualization_msgs is never released
+- tar:
+    local-name: ros/common_msgs/visualization_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/visualization_msgs/1.12.7-0.tar.gz
+    version: common_msgs-release-release-kinetic-visualization_msgs-1.12.7-0
+# catkin_virtualenv is required in jsk_3rdparty
+- git:
+    local-name: locusrobotics/catkin_virtualenv
+    uri: https://github.com/locusrobotics/catkin_virtualenv.git
+    version: 0.5.0
+# use latest robot_calibration
+- git:
+    local-name: mikeferguson/robot_calibration
+    uri: https://github.com/mikeferguson/robot_calibration.git
+    version: 0.5.5

jsk_fetch_robot/jsk_fetch_accessories/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_fetch_accessories)
+find_package(catkin)
+catkin_package()