[WIP] Merge develop/fetch branch

.github/labeler_conf/develop_fetch_labeler.yml

@@ -0,0 +1,2 @@
+develop/fetch:
+    - '**'

.github/workflows/develop_fetch_labeler.yml

@@ -0,0 +1,16 @@
+name: "Pull Request Labeler for develop/fetch"
+on:
+  pull_request_target:
+    branches:
+      - develop/fetch
+
+jobs:
+  triage:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/[email protected]
+      with:
+        configuration-path: ".github/labeler_conf/develop_fetch_labeler.yml"
+        repo-token: "${{ secrets.GITHUB_TOKEN }}"
+        sync-labels: true
+        dot: true

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

@@ -22,11 +22,11 @@
 
 (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)
@@ -43,16 +43,18 @@
      (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))
      ))
   (:state (&rest args)
    "We do not have :wait-until-update option for :state :worldcoords.
@@ -131,8 +133,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)))
@@ -205,6 +207,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)
@@ -257,18 +270,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

@@ -5,10 +5,12 @@
 # https://github.com/PR2/app_manager/pull/50
 # In order to run multiple app_managers in one master, we need this PR
 # https://github.com/PR2/app_manager/pull/54
+# This PR add run_name entry to specif node name when we use run entry
+# https://github.com/PR2/app_manager/pull/64
 - git:
     local-name: PR2/app_manager
-    uri: https://github.com/knorth55/app_manager.git
-    version: fetch15
+    uri: https://github.com/tkmtnt7000/app_manager.git
+    version: add-run-name-entry-fetch15
 # we need this for proximity sensors
 - git:
     local-name: RoboticMaterials/FA-I-sensor
@@ -70,7 +72,7 @@
     local-name: jsk-ros-pkg/jsk_recognition
     uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
     version: master
-# we need to use the development branch (fetch15 branch in knorth55's fork)
+# we need to use the development branch (develop/fetch branch in jsk-ros-pkg)
 # until it is merged to master
 - git:
     local-name: jsk-ros-pkg/jsk_robot
@@ -185,3 +187,15 @@
     local-name: BehaviorTree/BehaviorTree.ROS
     uri: https://github.com/BehaviorTree/BehaviorTree.ROS
     version: master
+# we need this for smart device ros system
+- git:
+    local-name: sktometometo/smart_device_protocol
+    uri: https://github.com/sktometometo/smart_device_protocol.git
+    version: v0.5.1
+# melodic version of robot_localization causes TF_REPEATED_DATA Errors to noetic client
+# So we have to adopt https://github.com/cra-ros-pkg/robot_localization/pull/595 (Using noetic-devel causes some localization trouble)
+# After https://github.com/cra-ros-pkg/robot_localization/pull/840 has been merged, we can use upstream melodic-devel branch
+- git:
+    local-name: cra-ros-pkg/robot_localization
+    uri: https://github.com/mqcmd196/robot_localization.git
+    version: 2.6.9-suppress

jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt

@@ -10,16 +10,47 @@ endif()
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin)
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  roscpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  tf
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  tf2_eigen
+  tf2_msgs
+)
+find_package(Boost REQUIRED COMPONENTS)
+
+generate_dynamic_reconfigure_options(
+  cfg/TimeSignal.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
 ###################################
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS dynamic_reconfigure
+)
 
 catkin_add_env_hooks(99.jsk_fetch_startup SHELLS bash zsh sh
   DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
 
+###########
+## Build ##
+###########
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+add_executable(trashbins_pose_estimator src/trashbins_pose_estimator.cpp)
+target_link_libraries(trashbins_pose_estimator
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############
@@ -51,8 +82,38 @@ macro(configure_icon_files icol iname)
   endif()
 endmacro(configure_icon_files)
 
+macro(configure_dialogflow_hotword_yaml dname)
+  set(FETCH_NAME ${dname})
+  if (${FETCH_NAME} STREQUAL "fetch15")
+    set(ROBOT_NICKNAME_KANA "ヘンゼル")
+    set(ROBOT_NICKNAME_HIRA "へんぜる")
+    configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config/dialogflow_hotword.yaml.in
+                   ${CMAKE_CURRENT_SOURCE_DIR}/config/${FETCH_NAME}_dialogflow_hotword.yaml)
+  elseif (${FETCH_NAME} STREQUAL "fetch1075")
+    set(ROBOT_NICKNAME_KANA "グレーテル")
+    set(ROBOT_NICKNAME_HIRA "ぐれーてる")
+    configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config/dialogflow_hotword.yaml.in
+                   ${CMAKE_CURRENT_SOURCE_DIR}/config/${FETCH_NAME}_dialogflow_hotword.yaml)
+  endif()
+endmacro(configure_dialogflow_hotword_yaml)
+
+macro(configure_email_topic_yaml rname)
+  cmake_host_system_information(RESULT FETCH_NAME QUERY HOSTNAME)
+  if (${FETCH_NAME} MATCHES ^${rname})
+    set(RECEIVER_MAIL ${rname})
+    set(EMAIL_SENDER_ADDRESS ${FETCH_NAME}@jsk.imi.i.u-tokyo.ac.jp)
+    set(EMAIL_RECEIVER_ADDRESS ${RECEIVER_MAIL}@jsk.imi.i.u-tokyo.ac.jp)
+    configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config/email_topic.yaml.in
+                   ${CMAKE_CURRENT_SOURCE_DIR}/config/${FETCH_NAME}_email_topic.yaml)
+  endif()
+endmacro(configure_email_topic_yaml)
+
+
 configure_icon_files(blue fetch15)
 configure_icon_files(red fetch1075)
+configure_dialogflow_hotword_yaml(fetch15)
+configure_dialogflow_hotword_yaml(fetch1075)
+configure_email_topic_yaml(fetch)
 
 
 #############