[WIP] Merge Fetch15 branch

jsk_fetch_robot/README.md

@@ -33,29 +33,45 @@
 
 ## How to Run
 
+### Setup Environment (For Remote PC)
 
-### Setup Environment
-
-First, you need to install ros. For ros indigo, please refer to install guide like [here](http://wiki.ros.org/indigo/Installation/Ubuntu)
+First, you need to install ROS. For ROS melodic, please refer to install guide like [here](http://wiki.ros.org/melodic/Installation/Ubuntu).
+Please make sure your ROS Distribution is indigo, kinetic or melodic.
 
 ```bash
 mkdir -p catkin_ws/src
 cd  catkin_ws/src
 wstool init .
 wstool set --git jsk-ros-pkg/jsk_robot https://github.com/jsk-ros-pkg/jsk_robot.git -y
-if [[ $ROS_DISTRO =~ ^(indigo|kinetic|melodic)$ ]]; then
-  wstool merge -t . https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_fetch_robot/jsk_fetch_user.rosinstall.$ROS_DISTRO
-else
-  echo "Your ROS distribution $ROS_DISTRO is not supported."
-fi
+wstool merge -t . https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_fetch_robot/jsk_fetch_user.rosinstall.$ROS_DISTRO
 wstool update -t .
+# To use eus10, furuschev script is required.
+wget https://raw.githubusercontent.com/jsk-ros-pkg/jsk_roseus/master/setup_upstream.sh -O /tmp/setup_upstream.sh
+bash /tmp/setup_upstream.sh -w ../ -p jsk-ros-pkg/geneus -p euslisp/jskeus
 source /opt/ros/$ROS_DISTRO/setup.bash
 rosdep install -y -r --from-paths . --ignore-src
 cd ../
 catkin build fetcheus jsk_fetch_startup
 source devel/setup.bash
 ```
 
+#### Setup Environment (For Robot Internal PC, only for advanced developer)
+
+```bash
+mkdir -p catkin_ws/src
+cd  catkin_ws/src
+wstool init .
+wstool set --git jsk-ros-pkg/jsk_robot https://github.com/knorth55/jsk_robot.git -v fetch15 -y
+wstool update -t .
+wstool merge -t . jsk-ros-pkg/jsk_robot/jsk_fetch_robot/jsk_fetch.rosinstall.$ROS_DISTRO
+wstool update -t .
+source /opt/ros/$ROS_DISTRO/setup.bash
+rosdep install -y -r --from-paths . --ignore-src
+cd ../
+catkin build
+source devel/setup.bash
+```
+
 ### Connecting to Fetch
 
 You need to install `ros-indigo-jsk-tools` to use `rosset*` tools, otherwise use setenv command

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -1,18 +1,19 @@
 (require "package://fetcheus/fetch-utils.l")
 (require "package://pr2eus/robot-interface.l")
 (require "package://pr2eus_moveit/euslisp/robot-moveit.l")
+(require "package://roseus_resume/euslisp/interruption-handler.l")
 
 (ros::load-ros-package "fetcheus")
 (ros::load-ros-package "fetch_driver_msgs")
 (ros::load-ros-package "robot_controllers_msgs")
 
 (defclass fetch-interface
   :super robot-move-base-interface
-  :slots (gripper-action moveit-robot fetch-controller-action)
+  :slots (gripper-action moveit-robot fetch-controller-action point-head-action)
   )
 
 (defmethod fetch-interface
-  (:init (&key (default-collision-object t) &rest args)
+  (:init (&rest args &key (default-collision-object t) &allow-other-keys)
    (prog1 (send-super* :init :robot fetch-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args)
      (send self :add-controller :arm-controller)
      (send self :add-controller :torso-controller)
@@ -29,17 +30,24 @@
      (setq moveit-robot (instance fetch-robot :init))
      (send self :set-moveit-environment (instance fetch-moveit-environment :init :robot moveit-robot))
      (when (and (boundp '*co*) default-collision-object)
-       (send self :delete-headbox-collision-object)
        (send self :delete-keepout-collision-object)
        (send self :delete-ground-collision-object)
-       (send self :add-headbox-collision-object)
        (send self :add-keepout-collision-object)
        (send self :add-ground-collision-object))
      (setq fetch-controller-action
            (instance ros::simple-action-client :init
                      "/query_controller_states"
                      robot_controllers_msgs::QueryControllerStatesAction))
-     ))
+     (setq point-head-action
+           (instance ros::simple-action-client :init
+                     "/head_controller/point_head"
+                     control_msgs::PointHeadAction))
+
+     (roseus_resume:install-interruption-handler self
+         gripper-action
+         move-base-action
+         move-base-trajectory-action)
+     (roseus_resume:install-default-intervention self)))
   (:state (&rest args)
    "We do not have :wait-until-update option for :state :worldcoords.
 In other cases, :state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency.
@@ -117,8 +125,8 @@ Example usage:
                             (subseq args (+ (position :use-torso args) 2))))))
      (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
    ;;
-   (when (not (numberp tm))
-     (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args"))
+   (when (and (not (numberp tm)) (not (eql tm :fast)))
+     (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args~%"))
    (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm
                :start-offset-time (if start-offset-time start-offset-time start-time)
                :clear-velocities clear-velocities :use-torso use-torso args)))
@@ -191,6 +199,17 @@ Example usage:
   (:stop-grasp
     (&rest args &key &allow-other-keys)
     (send* self :go-grasp :pos 0.1 args))
+  (:point-head (pos &key (frame-id "base_link") (wait t))
+    (let ((goal (instance control_msgs::PointHeadGoal :init)))
+      (send goal :target :header :stamp (ros::time-now))
+      (send goal :target :header :frame_id frame-id)
+      (send goal :target :point (ros::pos->tf-point pos))
+      (if wait
+          (send point-head-action :send-goal goal)
+        (send point-head-action :send-goal-and-wait goal)
+        )
+      )
+    )
   (:go-grasp
     (&key (pos 0) (effort 50) (wait t))
     (when (send self :simulation-modep)
@@ -243,18 +262,6 @@ Example: (send self :gripper :position) => 0.00"
   (:delete-workspace ()
     (send *co* :delete-attached-object-by-id "workspace")
     (send *co* :delete-object-by-id "workspace"))
-  (:add-headbox-collision-object ()
-    (let ()
-      ;; fetch must be :reset-pose when we run this method
-      (setq *fetch-headbox* (make-cube 100 201 120))
-      (send *fetch-headbox* :move-coords (send robot :head_pan_link_lk :worldcoords)
-                                         (send robot :base_link_lk :worldcoords))
-      (send *fetch-headbox* :translate #f(33.75 0 150) (send robot :head_pan_link_lk :worldcoords))
-      (send *co* :add-attached-object *fetch-headbox* "head_pan_link"
-                 :frame_id "head_pan_link"
-                 :object_id "fetchheadbox")))
-  (:delete-headbox-collision-object ()
-    (send *co* :delete-attached-object-by-id "fetchheadbox"))
   (:add-keepout-collision-object ()
     (let ((cube (make-cube 200 350 10))
           (keepout (make-cylinder 300 10)))

jsk_fetch_robot/fetcheus/fetch-utils.l

@@ -5,16 +5,40 @@
 
 (defmethod fetch-robot
   (:inverse-kinematics
-   (target-coords &rest args &key link-list move-arm (use-torso t) move-target &allow-other-keys)
+   (target-coords &rest args &key link-list move-arm (use-torso t)
+                  use-base (start-coords (send self :copy-worldcoords))
+                  (base-range (list :min #f(-30 -30 -30)
+                                    :max #f( 30  30  30)))
+                  move-target &allow-other-keys)
    (unless move-arm (setq move-arm :rarm))
    (unless move-target (setq move-target (send self :rarm :end-coords)))
    (unless link-list
      (setq link-list (send self :link-list (send move-target :parent)
                            (unless use-torso (car (send self :rarm))))))
-   (send-super* :inverse-kinematics target-coords
-                :move-target move-target
-                :link-list link-list
-                args))
+   (cond
+     (use-base
+      (let ((diff-pos-rot
+             (concatenate float-vector
+                          (send start-coords :difference-position self)
+                          (send start-coords :difference-rotation self))))
+        (send self :move-to start-coords :world)
+        (with-append-root-joint
+            (ll self link-list
+                :joint-class omniwheel-joint
+                :joint-args base-range)
+          (send (caar ll) :joint :joint-angle
+                (float-vector (elt diff-pos-rot 0)
+                              (elt diff-pos-rot 1)
+                              (rad2deg (elt diff-pos-rot 5))))
+          (send-super* :inverse-kinematics target-coords
+                       :move-target move-target
+                       :link-list ll  ;; link-list
+                       args))))
+     (t
+      (send-super* :inverse-kinematics target-coords
+                   :move-target move-target
+                   :link-list link-list
+                   args))))
   (:go-grasp
     (&key (pos 0)) ;; pos is between 0.0 and 0.1
       (send self :l_gripper_finger_joint :joint-angle (/ (* pos 1000) 2)) ;; m -> mm

jsk_fetch_robot/jsk_fetch.rosinstall.indigo

@@ -4,29 +4,32 @@
 - git:
     local-name: PR2/app_manager
     uri: https://github.com/PR2/app_manager.git
-    version: 1.1.0
-# For fetch to use twitter
+    version: kinetic-devel
 - git:
-    local-name: furushchev/image_pipeline
-    uri: https://github.com/furushchev/image_pipeline.git
-    version: develop
-# To send lifelog data to musca
+    local-name: RobotWebTools/rosbridge_suite
+    uri: https://github.com/RobotWebTools/rosbridge_suite.git
+    version: 0.11.9
 - git:
-    local-name: strands-project/mongodb_store
-    uri: https://github.com/strands-project/mongodb_store.git
-    version: 0.4.4
-# to install jsk_robot_startup/lifelog/common_logger.launch
-# remove after current master is released
+    local-name: RoboticMaterials/FA-I-sensor
+    uri: https://github.com/RoboticMaterials/FA-I-sensor.git
+- git:
+    local-name: fetchrobotics/fetch_ros
+    uri: https://github.com/fetchrobotics/fetch_ros.git
+    version: indigo-devel
 - git:
-    local-name: jsk-ros-pkg/jsk_robot
-    uri: https://github.com/jsk-ros-pkg/jsk_robot.git
+    local-name: fetchrobotics/robot_controllers
+    uri: https://github.com/fetchrobotics/robot_controllers.git
+    version: indigo-devel
+- git:
+    local-name: jsk-ros-pkg/jsk_3rdparty
+    uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
     version: master
 # to pass build of jsk_robot
-# remove after 2.2.10 is released
+# 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
+    version: 799fd309c1519801fcb3a37c9094814004d78594
 # to avoid volume 0 problem
 # remove after 0.3.14 (https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/41183fe3401d742bbec0edd13b67cb909a6968bd) is released
 - git:
@@ -39,7 +42,12 @@
 - git:
     local-name: jsk-ros-pkg/jsk_demos
     uri: https://github.com/jsk-ros-pkg/jsk_demos.git
-    version: 810acc7
+    version: 7c429715d0adf12c5dd34459ceb4b0a5b11dec6c
+# need to build from source because of nodelet version
+- git:
+    local-name: jsk-ros-pkg/jsk_recognition
+    uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
+    version: acb7e7dc5b549f8663dec403abdb80cbac36edf1
 # jsk_topic_tools requires nodelet gte 1.9.11
 # remove after 1.9.11 is released by apt
 - git:
@@ -51,20 +59,20 @@
 - git:
     local-name: ros-drivers/audio_common
     uri: https://github.com/ros-drivers/audio_common.git
-    version: 0.3.3
+    version: master
 # 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
 # Use joy/joy_remap.py
 - git:
     local-name: ros-drivers/joystick_drivers
@@ -103,3 +111,12 @@
     local-name: fetchrobotics/fetch_open_auto_dock
     uri: https://github.com/fetchrobotics/fetch_open_auto_dock.git
     version: 0.1.2
+# 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
+- git:
+    local-name: locusrobotics/catkin_virtualenv
+    uri: https://github.com/locusrobotics/catkin_virtualenv.git
+    version: 0.5.0