Merge branch 'master' into develop/fetch

jsk_naoqi_robot/README.md

@@ -57,6 +57,8 @@ If this happens, please connect Pepper to network with DHCP and exit Setting.
 For ros kinetic, please refer to install guide like [here](http://wiki.ros.org/kinetic/Installation).
 For ros melodic, please refer to install guide like [here](http://wiki.ros.org/melodic/Installation).
 
+% As mentioned in https://github.com/ros-naoqi/naoqi_driver#launch, naoqi_driver for ROS melodic and greater have to be used for robots running NAOqi 2.9 and greater. Using ROS melodic on Pepper running NAOqi OS 2.5 has some known issue like this: https://github.com/ros-naoqi/naoqi_driver/issues/96
+
 1. Install ``Python NAOqi SDK``
 You can download it (version = 2.5.5) from [here](https://drive.google.com/file/d/1xHuYREDa78xGiikEpsjxfZQ7Gfvo1E9D/view?usp=sharing).  
 Please unzip the downloaded file.  
@@ -124,6 +126,8 @@ source devel/setup.bash
 
 Confirm that you get the following output when you type `wstool info`
 
+% UID of jsk_robot may change from below, but there is no problem. That is because this package is in active developpment.
+
 ```
 $ cd ~/catkin_ws/src
 $ wstool info
@@ -132,13 +136,13 @@ workspace: /home/leus/catkin_ws/src
  Localname         S SCM Version (Spec)    UID  (Spec)  URI  (Spec) [http(s)://...]
  ---------         - --- --------------    -----------  ---------------------------
  pepper_robot        git master  (-)       efad3979b374 github.com/ros-naoqi/pepper_robot
- naoqi_driver      M git kochigami-develop 98c0b678286a github.com/kochigami/naoqi_driver
- naoqi_dashboard     git kochigami-develop 33962f146c77 github.com/kochigami/naoqi_dashboard
+ naoqi_driver        git kochigami-develop 98c0b678286a github.com/kochigami/naoqi_driver
+ naoqi_dashboard     git kochigami-develop 7f32005e08e0 github.com/kochigami/naoqi_dashboard
  naoqi_bridge_msgs   git kochigami-develop d7417613690e github.com/kochigami/naoqi_bridge_msgs
  naoqi_bridge        git kochigami-develop c28b727e1e9b github.com/kochigami/naoqi_bridge
  nao_robot           git master  (-)       67476469a137 github.com/ros-naoqi/nao_robot
  nao_interaction     git master  (-)       f97ad12f3896 github.com/ros-naoqi/nao_interaction
- jsk_robot           git master            1f44c97c7855 github.com/jsk-ros-pkg/jsk_robot.git
+ jsk_robot           git master            d551865511c3 github.com/jsk-ros-pkg/jsk_robot.git
 ```
 
 ## Interface when controlling NAO and Pepper via roseus

jsk_naoqi_robot/cross/build_user.sh

@@ -88,6 +88,7 @@ docker run -it --rm \
 cp -a ${PWD}/startup_scripts/user_setup.bash ${SOURCE_ROOT}/
 cp -a ${PWD}/startup_scripts/start.sh ${SOURCE_ROOT}/
 cp -a ${PWD}/startup_scripts/screenrc ${SOURCE_ROOT}/
+cp -a ${PWD}/startup_scripts/attach.sh ${SOURCE_ROOT}/
 
 echo "
 

jsk_naoqi_robot/cross/startup_scripts/attach.sh

@@ -0,0 +1,2 @@
+source User/user_setup.bash
+screen -c User/screenrc -r

jsk_naoqi_robot/jsk_pepper_startup/apps/meeting/meeting.app

@@ -2,4 +2,4 @@ display: Meeting
 platform: pepper
 launch: jsk_pepper_startup/meeting.xml
 interface: jsk_pepper_startup/meeting.interface
-icon: jsk_pepper_startup/meeting.png
+icon: jsk_pepper_startup/meeting.jpg

jsk_naoqi_robot/jsk_pepper_startup/apps/meeting/meeting.xml

@@ -1,7 +1,4 @@
 <launch>
-  <include file="$(find naoqi_apps)/launch/tablet.launch" />
-  <include file="$(find naoqi_apps)/launch/basic_awareness.launch" />
-  <include file="$(find naoqi_apps)/launch/background_movement.launch" />
   <node pkg="jsk_pepper_startup" name="meeting"
         type="meeting.l" args="'(main)'" />
 </launch>

jsk_naoqi_robot/jsk_pepper_startup/apps/meeting/scripts/meeting.l

@@ -53,11 +53,10 @@
 (defvar *pose6* (list #f(2.0 -2.0 -5.0 110.0 35.0 -40.0 -60.0 -40.0 110.0 -35.0 40.0 60.0 40.0 0.0 15.0) #f(2.0 -2.0 -5.0 110.0 35.0 -40.0 -60.0 -40.0 110.0 -35.0 40.0 60.0 40.0 0.0 15.0) #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0)))
 (defvar *time6* (list 2000 3000 1000))
 
-;;
+;; poses for byebye
 (defvar *reset* #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0))
-(defvar *a* #f(2.0 -2.0 -5.0 0.0 10.0 -100.0 -70.0 60.0 0.0 -10.0 100.0 70.0 -60.0 0.0 0.0))
-(defvar *d* #f(2.0 -2.0 -5.0 0.0 10.0 -110.0 -70.0 60.0 0.0 -10.0 110.0 70.0 -60.0 0.0 -10.0))
-(defvar *e* #f(2.0 -2.0 -5.0 0.0 10.0 -110.0 -70.0 60.0 0.0 -10.0 110.0 70.0 -60.0 0.0 -10.0))
+(defvar *byebye-close* #f(2.0 -2.0 -5.0 0.0 10.0 -100.0 -70.0 60.0 0.0 -10.0 100.0 70.0 -60.0 0.0 0.0))
+(defvar *byebye-open* #f(2.0 -2.0 -5.0 0.0 10.0 -110.0 -70.0 60.0 0.0 -10.0 110.0 70.0 -60.0 0.0 -10.0))
 
 (defvar *img-list*
   (list
@@ -86,15 +85,7 @@
 
 (ros::roseus "online-meeting-enhancement-main")
 
-(ros::subscribe "joy" sensor_msgs::Joy
-                #'(lambda (msg)
-                    (let ((button (send msg :buttons)))
-                      (ros::ros-info "received joy button ~A" button)
-                      (when (eq (elt button 3) 1)
-                        (send *ri* :speak "はい")
-                        (setq *continue* nil)))))
-
-(defun start-func (args)
+(defun start-func (&rest args)
   (let ()
     (setq *last-tm* (ros::time-now))
     ;;
@@ -114,14 +105,16 @@
     (set-alist 'description "" args)
     :started))
 
-(defun end-func (args)
+(defun end-func (&rest args)
   (let ()
     (send *ri* :show-image "golden-saying-by-pepper-7.png")
+    (send *ri* :set-basic-awareness-enabled nil)
+    (send *ri* :set-background-movement-enabled nil)
     (send *ri* :angle-vector-sequence (list #f(2.0 -2.0 -5.0 85.0 20.0 -70.0 -20.0 -40.0 85.0 -20.0 70.0 20.0 40.0 30.0 -10.0) #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0) #f(2.0 -2.0 -5.0 85.0 20.0 -70.0 -20.0 -40.0 85.0 -20.0 70.0 20.0 40.0 -30.0 -10.0) #f(2.0 -2.0 -5.0 85.0 10.0 -70.0 -20.0 -40.0 85.0 -10.0 70.0 20.0 40.0 0.0 0.0)) (list 1000 1000 1000 1000))
     (send *ri* :speak "\\vct=120\\\\rspd=80\\みなさん")
     (send *ri* :wait-interpolation)
 
-    (send *ri* :angle-vector-sequence (list *a* *d* *a* *d* *a* *d* *a* *d* *a* *e* *reset*) (list 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 3000))
+    (send *ri* :angle-vector-sequence (list *byebye-close* *byebye-open* *byebye-close* *byebye-open* *byebye-close* *byebye-open* *byebye-close* *byebye-open* *byebye-close* *byebye-open* *reset*) (list 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 3000))
     (send *ri* :speak "\\vct=120\\\\rspd=80\\おつかれさまでしたッ！\\rspd=100\\")
     (send *ri* :wait-interpolation)
     (send *ri* :hide-image)
@@ -130,7 +123,7 @@
     (set-alist 'description "End Meeting" args)
     :finished))
 
-(defun comment-func (args)
+(defun comment-func (&rest args)
   (let (count str (wait 600) tm)
     ;; cleanup description/image
     (set-alist 'description "" args)

jsk_naoqi_robot/jsk_pepper_startup/launch/jsk_pepper_startup.launch

@@ -15,6 +15,11 @@
     <arg name="network_interface" value="$(arg network_interface)"/>
   </include>
 
+  <!-- Please clone https://github.com/kochigami/naoqi_bridge/tree/kochigami-develop -->
+  <include file="$(find naoqi_apps)/launch/tablet.launch" />
+  <include file="$(find naoqi_apps)/launch/basic_awareness.launch" />
+  <include file="$(find naoqi_apps)/launch/background_movement.launch" />
+
   <node pkg="jsk_pepper_startup" name="take_wakeup_pose" type="take_wakeup_pose.py" if="$(arg autonomous_wakeup)" />
 
   <!-- node to publish compressed image -->

jsk_panda_robot/jsk_panda_teleop/package.xml

@@ -1,4 +1,5 @@
-<package>
+<?xml version="1.0"?>
+<package format="3">
   <name>jsk_panda_teleop</name>
   <version>1.1.0</version>
   <description>Teleopration of dual-franka using TouchUSB</description>
@@ -11,29 +12,31 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>message_generation</build_depend>
 
-  <run_depend>message_runtime</run_depend>
-  <run_depend>rospy</run_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rospy</exec_depend>
 
-  <run_depend>franka_description</run_depend>
-  <run_depend>franka_msgs</run_depend>
-  <run_depend>franka_gripper</run_depend>
-  <run_depend>franka_control</run_depend>
-  <run_depend>franka_example_controllers</run_depend>
-  <run_depend>franka_visualization</run_depend>
+  <exec_depend>franka_description</exec_depend>
+  <exec_depend>franka_msgs</exec_depend>
+  <exec_depend>franka_gripper</exec_depend>
+  <exec_depend>franka_control</exec_depend>
+  <exec_depend>franka_example_controllers</exec_depend>
+  <exec_depend>franka_visualization</exec_depend>
 
-  <run_depend>omni_common</run_depend>
-  <run_depend>omni_msgs</run_depend>
+  <exec_depend>omni_common</exec_depend>
+  <exec_depend>omni_msgs</exec_depend>
 
-  <run_depend>python-scipy</run_depend>
-  <run_depend>python-numpy</run_depend>
-  <run_depend>python-absl-py-pip</run_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</exec_depend>
+  <exec_depend>python-absl-py-pip</exec_depend>
 
-  <run_depend>jsk_panda_startup</run_depend>
-  <run_depend>jsk_teleop_joy</run_depend>
+  <exec_depend>jsk_panda_startup</exec_depend>
+  <exec_depend>jsk_teleop_joy</exec_depend>
 
-  <run_depend>audio_capture</run_depend>
-  <run_depend>audio_play</run_depend>
-  <run_depend>audio_common_msgs</run_depend>
-  <run_depend>sound_classification</run_depend>
+  <exec_depend>audio_capture</exec_depend>
+  <exec_depend>audio_play</exec_depend>
+  <exec_depend>audio_common_msgs</exec_depend>
+  <exec_depend>sound_classification</exec_depend>
 
 </package>

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/check_openni_node.py

@@ -31,6 +31,7 @@ def speak(self, speak_str):
             msg.sound = SoundRequest.SAY
             msg.command = SoundRequest.PLAY_ONCE
             msg.arg = speak_str
+            msg.volume = 1.0
             self.speak_pub.publish(msg)
 
     def restart_service_callback(self, req):
@@ -76,14 +77,15 @@ def restart_openni_node(self):
             # 1. kill nodelet manager
             self.speak("something wrong with kinect, I'll restart it, killing nodelet manager")
             retcode = subprocess.call('rosnode kill /%s/%s_nodelet_manager' % (self.camera, self.camera), shell=True)
-            time.sleep(10)
-            # 2. pkill
-            self.speak("killing child processes")
             retcode = subprocess.call('pkill -f %s_nodelet_manager' % self.camera, shell=True)
-            time.sleep(10)
+            # time.sleep(10)
+            # 2. pkill
+            # self.speak("killing child processes")
+            # retcode = subprocess.call('pkill -f %s_nodelet_manager' % self.camera, shell=True)
+            # time.sleep(10)
             # 3 restarting
-            self.speak("restarting processes")
-            retcode = subprocess.call('roslaunch openni_launch openni.launch camera:=%s publish_tf:=false depth_registration:=true rgb_processing:=false ir_processing:=false depth_processing:=false depth_registered_processing:=false disparity_processing:=false disparity_registered_processing:=false hw_registered_processing:=true sw_registered_processing:=false rgb_frame_id:=/head_mount_kinect_rgb_optical_frame depth_frame_id:=/head_mount_kinect_ir_optical_frame' % self.camera, shell=True)
+            # self.speak("restarting processes")
+            # retcode = subprocess.call('roslaunch openni_launch openni.launch camera:=%s publish_tf:=false depth_registration:=true rgb_processing:=false ir_processing:=false depth_processing:=false depth_registered_processing:=false disparity_processing:=false disparity_registered_processing:=false hw_registered_processing:=true sw_registered_processing:=false rgb_frame_id:=/head_mount_kinect_rgb_optical_frame depth_frame_id:=/head_mount_kinect_ir_optical_frame' % self.camera, shell=True)
         except Exception as e:
             rospy.logerr('[%s] Unable to kill kinect node, caught exception:\n%s', self.__class__.__name__, traceback.format_exc())
 

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/kinect_head.launch

@@ -1,5 +1,6 @@
 <launch>
   <arg name="load_driver" default="true" doc="launch driver set to false on gazebo"/>
+  <arg name="manager_respawn" default="true" />
   <arg name="respawn" default="true" />
   <arg name="use_c2" default="true" doc="Launch nodes for registration on C2"/>
   <arg name="deprecated_relay" default="true" doc="advertise to deprecated kinect_head_c2 ns"/>
@@ -26,12 +27,13 @@
   <param name="/kinect_head/driver/depth_ir_offset_x" value="4.5" />
   <param name="/kinect_head/driver/depth_ir_offset_y" value="2.5" />
 
-  <include file="$(find openni_launch)/launch/openni.launch"
+  <include file="$(find jsk_pr2_startup)/jsk_pr2_sensors/openni.launch"
            if="$(arg load_driver)">
     <arg name="camera" value="$(arg camera)"/>
     <arg name="num_worker_threads" value="$(arg num_worker_threads)" />
     <arg name="rgb_frame_id" value="head_mount_kinect_rgb_optical_frame" />
     <arg name="depth_frame_id" value="head_mount_kinect_ir_optical_frame" />
+    <arg name="manager_respawn" value="$(arg manager_respawn)" />
     <arg name="respawn" value="$(arg respawn)" />
     <arg name="publish_tf" value="false"/>
     <arg name="depth_registration" value="true"/>

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/manager.launch.xml

@@ -0,0 +1,31 @@
+<!-- this file is copied and modified from $(find rgbd_launch)/launch/includes/manager.launch.xml -->
+<!-- this file is modified from original to add respawn arguments -->
+<!-- see https://github.com/ros-drivers/rgbd_launch/pull/40 -->
+
+<!-- Start nodelet manager -->
+<launch>
+
+  <!-- Name here should NOT be globally qualified (node names cannot contain /) -->
+  <arg name="name" />
+
+  <!-- Optionally launch manager in GDB, for debugging -->
+  <arg name="debug" default="false" />
+  <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <!-- Worker threads -->
+  <arg name="num_worker_threads" />
+  <!-- respawn -->
+  <arg name="respawn" />
+
+  <!-- Also globally disable bond heartbeat timeout in debug mode, so everything
+       doesn't die when you hit a break point -->
+  <param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />
+
+  <!-- Nodelet manager -->
+  <node pkg="nodelet" type="nodelet" name="$(arg name)" args="manager"
+        output="screen" launch-prefix="$(arg launch_prefix)"
+        respawn="$(arg respawn)">
+     <param name="num_worker_threads" value="$(arg num_worker_threads)" />
+  </node>
+
+</launch>