[jsk_fetch_startup] Fix description report-light-on

jsk_fetch_robot/README.md

@@ -33,26 +33,49 @@
 
 ## 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 set --git jsk-ros-pkg/jsk_robot https://github.com/jsk-ros-pkg/jsk_robot.git -v develop/fetch -y
+wstool merge -t . https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_fetch_robot/jsk_fetch_user.rosinstall.$ROS_DISTRO
+
+# (optional): the two lines below are necessary when you want to use roseus_resume
+wstool merge -t . https://gist.githubusercontent.com/Affonso-Gui/25518fef9dc7af0051147bdd2a94b116/raw/e3fcbf4027c876329801a25e32f4a4746200ddae/guiga_system.rosinstall
 wstool update -t .
+
+# (optional): the two lines below are necessary when you want to use eus10
+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 ../
+# (optional): if you want to use roseus_resume, build roseus_resume, too.
 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/jsk-ros-pkg/jsk_robot.git -v develop/fetch -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
 ```
 

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -1,18 +1,27 @@
 (require "package://fetcheus/fetch-utils.l")
 (require "package://pr2eus/robot-interface.l")
 (require "package://pr2eus_moveit/euslisp/robot-moveit.l")
+(defvar *enable-roseus-resume* (ros::rospack-find "roseus_resume"))
+(if *enable-roseus-resume*
+  (progn
+    (require :roseus_resume "package://roseus_resume/euslisp/interruption-handler.l")
+    (warning-message 2 "roseus_resume is enabled.~%"))
+  (progn
+    ;; make dummy package to avoid symbol search error
+    (unless (find-package "ROSEUS_RESUME") (make-package "ROSEUS_RESUME"))
+    (warning-message 3 "roseus_resume is disabled.~%")))
 
 (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,16 +38,28 @@
      (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))
+
+     (if *enable-roseus-resume*
+       (progn
+         (warning-message 3 "Installing interruption handler...~%")
+         (roseus_resume::install-interruption-handler self
+             gripper-action
+             move-base-action
+             move-base-trajectory-action)
+         (roseus_resume::install-default-intervention self)
+         (warning-message 3 "...done.~%")))
      ))
   (:state (&rest args)
    "We do not have :wait-until-update option for :state :worldcoords.
@@ -117,8 +138,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 +212,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 +275,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.melodic

@@ -65,11 +65,12 @@
     local-name: jsk-ros-pkg/jsk_pr2eus
     uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
     version: master
-# we need to use the development branch until the next release.
+# we need this PR for fisheye vital check
+# https://github.com/jsk-ros-pkg/jsk_recognition/pull/2734
 - git:
     local-name: jsk-ros-pkg/jsk_recognition
-    uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
-    version: master
+    uri: https://github.com/knorth55/jsk_recognition.git
+    version: fisheye-vital-check
 # we need to use the development branch (fetch15 branch in knorth55's fork)
 # until it is merged to master
 - git: