Merge pull request #1328 from knorth55/fetch-melodic-support

[merge after #1208 and #1371] Fetch melodic and new hardware support

jsk_fetch_robot/jsk_fetch.rosinstall.melodic

@@ -1,11 +1,10 @@
 # This is rosinstall file for melodic PC inside fetch.
 # $ ln -s $(rospack find jsk_fetch_startup)/../jsk_fetch.rosinstall.$ROS_DISTRO $HOME/ros/$ROS_DISTRO/src/.rosinstall
 
-# we need https://github.com/PR2/app_manager/pull/31 to handle timeout
 - git:
     local-name: PR2/app_manager
-    uri: https://github.com/knorth55/app_manager.git
-    version: fetch15
+    uri: https://github.com/PR2/app_manager.git
+    version: kinetic-devel
 - git:
     local-name: RobotWebTools/rosbridge_suite
     uri: https://github.com/RobotWebTools/rosbridge_suite.git
@@ -111,7 +110,8 @@
     local-name: fetchrobotics/fetch_open_auto_dock
     uri: https://github.com/fetchrobotics/fetch_open_auto_dock.git
     version: melodic-devel
+# Use https://github.com/ros-planning/navigation/pull/839 based on 7f22997e6804d9d7249b8a1d789bf27343b26f75
 - git:
     local-name: ros-planning/navigation
-    uri: https://github.com/ros-planning/navigation.git
-    version: 7f22997e6804d9d7249b8a1d789bf27343b26f75
+    uri: https://github.com/708yamaguchi/navigation.git
+    version: fetch15

jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt

@@ -64,12 +64,17 @@ if(CATKIN_ENABLE_TESTING)
   # https://github.com/ros/ros_comm/pull/998 respect if/unless for roslaunch-check (1.13.1)
   # https://github.com/ros/ros_comm/pull/1455 make roslaunch-check respect arg remappings with command line argument (1.14.4)
   set(roslaunch_check_script ${PROJECT_SOURCE_DIR}/scripts/roslaunch-check)
-  # xacro parser error in kinetic and below
-  # xacro tries finding /etc/$ROS_DISTRO/calibrated_latest.urdf even if use_fetch_description is true.
-  if($ENV{ROS_DISTRO} STRGREATER "kinetic")
+  # test fetch_bringup.launch only in indigo, melodic and above, because kinetic fetch is not released.
+  # disable launch_insta360 because jsk_perception with insta360 launch is not released yet.
+  if("$ENV{ROS_DISTRO}" STREQUAL "indigo")
+    # disable launch_teleop because fetch_auto_dock package exists only in the fetch's PC
+    roslaunch_add_file_check(launch/fetch_bringup.launch launch_teleop:=false launch_insta360:=false)
+  elseif("$ENV{ROS_DISTRO}" STRGREATER "kinetic")
+    # xacro parser error in kinetic and below
+    # xacro tries finding /etc/$ROS_DISTRO/calibrated_latest.urdf even if use_fetch_description is true.
     roslaunch_add_file_check(launch/fetch.launch use_fetch_description:=true)
+    roslaunch_add_file_check(launch/fetch_bringup.launch launch_insta360:=false)
   endif()
-  roslaunch_add_file_check(launch/fetch_bringup.launch launch_teleop:=false) # disable launch_teleop because fetch_auto_dock package exists only in the fetch's PC
   roslaunch_add_file_check(launch/rviz.launch)
   roslaunch_add_file_check(launch/fetch_gazebo_bringup.launch)
   set(roslaunch_check_script ${roslaunch_DIR}/../scripts/roslaunch-check)

jsk_fetch_robot/jsk_fetch_startup/README.md

@@ -115,6 +115,13 @@ Logging script is initialized at:
 /etc/init/jsk-log-wifi.conf
 ```
 
+### Show all logs
+
+
+```bash
+tmuxinator log
+```
+
 ## Administration
 - 2016/10/26 add `allow 133.11.216/8` to /etc/chrony/chrony.conf
 - 2018/08/26 add `0 10 * * 1-5 /home/fetch/ros/indigo_robot/devel/env.sh rosservice call /fetch15/start_app "name: 'jsk_fetch_startup/go_to_kitchen'"` to crontab

jsk_fetch_robot/jsk_fetch_startup/config/install_supervisor.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+jsk_fetch_startup=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`"/.. > /dev/null && pwd)
+
+IFS=':' read -r -a prefix_paths <<< "$CMAKE_PREFIX_PATH"
+current_prefix_path="${prefix_paths[0]}"
+
+set -x
+
+cd $jsk_fetch_startup/supervisor_scripts
+for file in $(ls ./*.conf); do
+    sudo cp $file /etc/supervisor/conf.d/
+    sudo chown root:root /etc/supervisor/conf.d/$file
+    sudo chmod 644 /etc/supervisor/conf.d/$file
+    echo "copied $file to /etc/supervisor/conf.d"
+done
+
+set +x

jsk_fetch_robot/jsk_fetch_startup/config/install_tmuxinator.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+jsk_fetch_startup=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`"/.. > /dev/null && pwd)
+
+set -x
+
+if [ ! command -v tmuxinator &> /dev/null ]; then
+  sudo apt update
+  sudo apt install tmuxinator
+fi
+if [ ! -d "$HOME/.tmuxinator" ]; then
+  mkdir $HOME/.tmuxinator
+fi
+ln -sf $jsk_fetch_startup/tmuxinator_yaml/log.yml $HOME/.tmuxinator/log.yml
+
+set +x

jsk_fetch_robot/jsk_fetch_startup/config/install_udev.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+jsk_fetch_startup=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`"/.. > /dev/null && pwd)
+
+IFS=':' read -r -a prefix_paths <<< "$CMAKE_PREFIX_PATH"
+current_prefix_path="${prefix_paths[0]}"
+
+set -x
+
+cd $jsk_fetch_startup/udev_rules
+for file in $(ls ./*.rules); do
+    sudo cp $file /etc/udev/rules.d/
+    sudo chown root:root /etc/udev/rules.d/$file
+    sudo chmod 644 /etc/udev/rules.d/$file
+    echo "copied $file to /etc/udev/rules.d/"
+done
+
+set +x

jsk_fetch_robot/jsk_fetch_startup/config/install_upstart.sh

@@ -5,34 +5,14 @@ jsk_fetch_startup=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`"/.. > /dev/null &&
 IFS=':' read -r -a prefix_paths <<< "$CMAKE_PREFIX_PATH"
 current_prefix_path="${prefix_paths[0]}"
 
-sudo bash -c "cat <<EOF > /etc/init/jsk-fetch-startup.conf
-description \"spawn jsk bringup nodes for fetch\"
-# use default log dir: yamaguchi & s-kitagawa (2019/04/18)
-# env ROS_LOG_DIR=/var/log/ros
+set -x
 
-start on roscore_is_up
-stop on roscore_is_down
+cd $jsk_fetch_startup/upstart_scripts
+for file in $(ls ./*.conf); do
+    sudo cp $file /etc/init/
+    sudo chown root:root /etc/init/$file
+    sudo chmod 644 /etc/init/$file
+    echo "copied $file to /etc/init"
+done
 
-respawn
-
-# add ROSCONSOLE_FORMAT: s-kitagawa (2019/10/03)
-env AUDIO_DEVICE=alsa_output.usb-1130_USB_AUDIO-00-AUDIO.analog-stereo
-env ROSCONSOLE_FORMAT='[${severity}] [${time}] [${node}:${logger}]: ${message}'
-
-# enable usb speaker if available
-pre-start script
-    # launch by fetch user: yamaguchi & s-kitagawa (2019/04/18)
-    # exec su ros -c 'pactl set-default-sink $AUDIO_DEVICE || true'
-    exec su fetch -c 'pactl set-default-sink $AUDIO_DEVICE || true'
-end script
-
-script
-    # launch by fetch user: yamaguchi & s-kitagawa (2019/04/18)
-    # exec su ros -c \". ${current_prefix_path}/setup.bash && roslaunch ${jsk_fetch_startup}/launch/fetch_bringup.launch boot_sound:=true\"
-    exec su fetch -c \". ${current_prefix_path}/setup.bash && roslaunch ${jsk_fetch_startup}/launch/fetch_bringup.launch boot_sound:=true\"
-end script
-
-EOF"
-echo "== cat /etc/init/jsk-fetch-startup.conf =="
-cat /etc/init/jsk-fetch-startup.conf
-echo "== usage: sudo service jsk-fetch-startup restart =="
+set +x

jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch

@@ -1,8 +1,8 @@
 <launch>
   <!-- add arg launch_teleop: yamaguchi & s-kitagawa (2019/04/19) -->
   <arg name="launch_teleop" default="true" />
-  <arg name="base_camera_mount" default="true" />
-  <arg name="head_box" default="true" />
+  <arg name="use_base_camera_mount" default="true" />
+  <arg name="use_head_box" default="true" />
   <arg name="use_realsense" default="true" />
   <arg name="use_fetch_description" default="false" />
 
@@ -120,7 +120,7 @@
   <param name="robot_description"
          command="$(find xacro)/xacro $(find jsk_fetch_startup)/robots/jsk_fetch.urdf.xacro
                   ros_distro:=$(env ROS_DISTRO) use_fetch_description:=$(arg use_fetch_description)
-                  base_camera_mount:=$(arg base_camera_mount) head_box:=$(arg head_box)" />
+                  base_camera_mount:=$(arg use_base_camera_mount) head_box:=$(arg use_head_box)" />
 
   <!-- Drivers for Base -->
   <node name="robot_driver" launch-prefix="$(arg launch_prefix)" pkg="fetch_drivers" type="robot_driver" output="screen">

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -3,6 +3,8 @@
   <arg name="launch_teleop" default="true" />
   <arg name="launch_move_base" default="true" />
   <arg name="launch_sound_play" default="true" />
+  <arg name="launch_insta360" default="true" />
+  <arg name="use_voice_text" default="true" />
   <arg name="boot_sound" default="false" />
   <arg name="map_frame" default="eng2" />
   <arg name="map_file" default="$(find jsk_maps)/raw_maps/eng2-7f-0.05.yaml"/>
@@ -43,14 +45,22 @@
     <remap from="~input/people_pose_array" to="/edgetpu_human_pose_estimator/output/poses"/>
   </node>
 
+  <!-- insta360 air images -->
+  <include file="$(find jsk_fetch_startup)/launch/fetch_insta360_$(env ROS_DISTRO).launch"
+           if="$(arg launch_insta360)" />
+
   <!-- english speach node -->
   <!-- disable sound_play in julius.launch and place it in fetch_bringup.launch -->
   <!-- see: https://github.com/jsk-ros-pkg/jsk_robot/pull/1140 -->
   <node name="sound_play" pkg="sound_play" type="soundplay_node.py"
         respawn="true" if="$(arg launch_sound_play)" />
 
   <!-- japanese speech node -->
-  <include file="$(find voice_text)/launch/voice_text.launch">
+  <include if="$(arg use_voice_text)" file="$(find voice_text)/launch/voice_text.launch">
+    <arg name="launch_sound_play" value="$(arg launch_sound_play)" />
+    <arg name="sound_play_respawn" value="true" />
+  </include>
+  <include unless="$(arg use_voice_text)" file="$(find aques_talk)/launch/aques_talk.launch">
     <arg name="launch_sound_play" value="$(arg launch_sound_play)" />
     <arg name="sound_play_respawn" value="true" />
   </include>
@@ -99,12 +109,13 @@
   </include>
 
   <!-- include fetch moveit -->
-  <include file="$(find fetch_moveit_config)/launch/move_group.launch"
-           if="$(arg launch_moveit)" />
-  <!-- overwrite fetch.srdf -->
-  <!-- The semantic description that corresponds to the URDF -->
-  <param name="robot_description_semantic"
-         command="rosrun xacro xacro --inorder $(find jsk_fetch_startup)/launch/moveit/fetch.srdf.xacro" />
+  <group if="$(arg launch_moveit)">
+    <include file="$(find fetch_moveit_config)/launch/move_group.launch" />
+    <!-- overwrite fetch.srdf -->
+    <!-- The semantic description that corresponds to the URDF -->
+    <param name="robot_description_semantic"
+           command="rosrun xacro xacro --inorder $(find jsk_fetch_startup)/launch/moveit/fetch.srdf.xacro" />
+  </group>
 
   <!-- teleop -->
   <include file="$(find jsk_fetch_startup)/launch/fetch_teleop.xml"
@@ -187,70 +198,16 @@
       <remap from="scan" to="base_scan"/>
     </node>
 
-    <rosparam ns="amcl">
-      update_min_a: 0.01 <!-- update filter every 0.01[m] translation -->
-      update_min_d: 0.01 <!-- update filter every 0.01[rad] rotation -->
-      odom_alpha1: 0.2  # rotation noise per rotation
-      odom_alpha2: 0.2  # rotation noise per translation
-      odom_alpha3: 0.2  # translation noise per translation
-      odom_alpha4: 0.2  # translation noise per rotation
-    </rosparam>
-
-    <rosparam ns="move_base">
-      controller_frequency: 10.0
-    </rosparam>
-
-    <rosparam ns="move_base/global_costmap">
-      inflater:
-        inflation_radius: 0.7 # 0.7
-        cost_scaling_factor: 5.0 # 10.0
-      obstacles:
-        min_obstacle_height: 0.05
-      footprint_padding: 0.05
-    </rosparam>
-    <rosparam ns="move_base/local_costmap">
-      inflater:
-        inflation_radius: 0.7 # 0.7
-        cost_scaling_factor: 5.0 # 25.0 default 10, increasing factor decrease the cost value
-      obstacles:
-        min_obstacle_height: 0.05
-      # default 5 (http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide)
-      update_frequency: 10.0
-      footprint_padding: 0.05
-    </rosparam>
-    <rosparam ns="move_base">
-      base_local_planner: base_local_planner/TrajectoryPlannerROS
-      TrajectoryPlannerROS:
-        min_in_place_vel_theta: 1.0
-        escape_vel: -0.1 # -0.1
-        vx_samples: 10
-        meter_scoring: true
-        pdist_scale: 5.0
-        gdist_scale: 3.2
-        occdist_scale: 0.1
-        dwa: true
-      recovery_behavior_enabled: true
-      recovery_behaviors:
-        - name: "conservative_reset"
-          type: "clear_costmap_recovery/ClearCostmapRecovery"
-        - name: "rotate_recovery"
-          type: "rotate_recovery/RotateRecovery"
-          frequency: 20.0
-          sim_granularity: 0.017
-        - name: "aggressive_reset"
-          type: "clear_costmap_recovery/ClearCostmapRecovery"
-      conservative_reset:
-        reset_distance: 1.0 # 3.0
-      aggressive_reset:
-        reset_distance: 0.2 # 0.5
-      move_slow_and_clear:
-        clearing_distance: 0.5
-        limited_distance: 0.3
-        limited_rot_speed: 0.45
-        limited_trans_speed: 0.25
-        max_trans_param_name: max_vel_x
-        max_rot_param_name: max_vel_theta
-    </rosparam>
+    <!-- load amcl params -->
+    <rosparam command="load"
+              file="$(find jsk_fetch_startup)/launch/navigation/fetch_amcl_common_params.yaml" />
+    <rosparam command="load"
+              file="$(find jsk_fetch_startup)/launch/navigation/fetch_amcl_$(env ROS_DISTRO)_params.yaml" />
+    <!-- load move_base params -->
+    <rosparam command="load"
+              file="$(find jsk_fetch_startup)/launch/navigation/fetch_move_base_common_params.yaml" />
+    <rosparam command="load"
+              file="$(find jsk_fetch_startup)/launch/navigation/fetch_move_base_$(env ROS_DISTRO)_params.yaml" />
   </group>
 
   <!-- /imu has no frame_id information and there is no bug fix release in indigo. -->

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_gazebo_bringup.launch

@@ -38,5 +38,6 @@
     <arg name="map_frame" value="$(arg map_frame)" />
     <arg name="boot_sound" value="false" />
     <arg name="launch_teleop" value="false" /> <!-- duplicates cmd_vel_mux nodes -->
+    <arg name="launch_insta360" value="false" />
   </include>
 </launch>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_insta360_indigo.launch

@@ -0,0 +1,14 @@
+<launch>
+  <!-- Insta360 images -->
+  <include file="$(find jsk_perception)/sample/sample_insta360_air.launch">
+    <arg name="gui" value="false" />
+    <!-- Reduce CPU load -->
+    <!-- To check the valid formats, -->
+    <!-- $ sudo apt-get install v4l-utils -->
+    <!-- $ v4l2-ctl -d /dev/insta360 -\-list-formats-ext -->
+    <arg name="height" value="736" />
+    <arg name="width" value="1472" />
+    <!-- for indigo -->
+    <arg name="use_usb_cam" value="true" />
+  </include>
+</launch>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_insta360_melodic.launch

@@ -0,0 +1,14 @@
+<launch>
+  <!-- Insta360 images -->
+  <include file="$(find jsk_perception)/sample/sample_insta360_air.launch">
+    <arg name="gui" value="false" />
+    <!-- Reduce CPU load -->
+    <!-- To check the valid formats, -->
+    <!-- $ sudo apt-get install v4l-utils -->
+    <!-- $ v4l2-ctl -d /dev/insta360 -\-list-formats-ext -->
+    <arg name="height" value="736" />
+    <arg name="width" value="1472" />
+    <!-- for melodic -->
+    <arg name="use_usb_cam" value="false" />
+  </include>
+</launch>

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch_amcl_common_params.yaml

@@ -0,0 +1,7 @@
+amcl:
+  update_min_a: 0.05 # update filter every 0.05 [rad] rotation
+  update_min_d: 0.1  # update filter every 0.1 [m] translation
+  odom_alpha1: 0.1   # rotation noise per rotation
+  odom_alpha2: 0.05  # rotation noise per translation
+  odom_alpha3: 0.05  # translation noise per translation
+  odom_alpha4: 0.1   # translation noise per rotation

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch_amcl_indigo_params.yaml

@@ -0,0 +1,8 @@
+amcl:
+  kld_err: 0.1
+  kld_z: 0.5
+  laser_z_hit: 0.9
+  laser_z_rand: 0.5
+  laser_sigma_hit: 0.1
+  laser_likelihood_max_dist: 4.0
+  resample_interval: 1

jsk_fetch_robot/jsk_fetch_startup/launch/navigation/fetch_amcl_melodic_params.yaml

@@ -0,0 +1,7 @@
+amcl:
+  update_min_a: 0.05 # update filter every 0.05 [rad] rotation
+  update_min_d: 0.1  # update filter every 0.1 [m] translation
+  odom_alpha1: 0.01   # rotation noise per rotation
+  odom_alpha2: 0.01  # rotation noise per translation
+  odom_alpha3: 0.01  # translation noise per translation
+  odom_alpha4: 0.01   # translation noise per rotation