Melodic support for jsk fetch

jsk_fetch_robot/jsk_fetch.rosinstall.indigo

@@ -1,7 +1,6 @@
-# This file is for work space in fetch PC.
+# This is rosinstall file for indigo PC inside fetch.
+# $ ln -s $(rospack find jsk_fetch_startup)/../jsk_fetch.rosinstall.$ROS_DISTRO $HOME/ros/$ROS_DISTRO/src/.rosinstall
 
-# To install app_manager.launch
-# remove after 1.1.0 (https://github.com/PR2/app_manager/commit/7fa0929b6608246073a0fb89eed013edd6b934ca) is released
 - git:
     local-name: PR2/app_manager
     uri: https://github.com/PR2/app_manager.git
@@ -78,4 +77,34 @@
     local-name: fetchrobotics/fetch_ros
     uri: https://github.com/fetchrobotics/fetch_ros.git
     version: 4680118106ba8ffa99a803122bf1a9e383edf3df
-
+- git:
+    local-name: strands-project/mongodb_store
+    uri: https://github.com/strands-project/mongodb_store.git
+    version: 0.4.4
+- git:
+    local-name: tork-a/roswww
+    uri: https://github.com/knorth55/roswww.git
+    version: fetch15
+- git:
+    local-name: tork-a/visualization_rwt
+    uri: https://github.com/knorth55/visualization_rwt.git
+    version: fetch15
+- git:
+    local-name: ros-planning/navigation
+    uri: https://github.com/708yamaguchi/navigation.git
+    version: fetch15
+# Visualize visualization_msgs/MarkerArray >= 1.12.7 in RViz
+- git:
+    local-name: ros-visualization/rviz
+    uri: https://github.com/ros-visualization/rviz.git
+    version: indigo-devel
+# fetch_open_auto_dock is not released in indigo
+- git:
+    local-name: fetchrobotics/fetch_open_auto_dock
+    uri: https://github.com/fetchrobotics/fetch_open_auto_dock.git
+    version: 0.1.2
+# Use joy/joy_remap.py
+- git:
+    local-name: ros-drivers/joystick_drivers
+    uri: https://github.com/ros-drivers/joystick_drivers.git
+    version: 1.14.0

jsk_fetch_robot/jsk_fetch_startup/apps/go_to_kitchen/go_to_kitchen.app

@@ -3,3 +3,115 @@ platform: fetch
 launch: jsk_fetch_startup/go_to_kitchen.xml
 interface: jsk_fetch_startup/go_to_kitchen.interface
 icon: jsk_fetch_startup/go_to_kitchen.png
+plugins:
+  - name: head_camera_video_recorder_plugin
+    type: app_recorder/audio_video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: go_to_kitchen_head_camera.avi
+      audio_topic_name: /audio
+      audio_channels: 1
+      audio_sample_rate: 16000
+      audio_format: wave
+      audio_sample_format: S16LE
+      video_topic_name: /head_camera/rgb/image_rect_color
+      video_height: 480
+      video_width: 640
+      video_framerate: 30
+      video_encoding: RGB
+  - name: object_detection_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: go_to_kitchen_object_detection.avi
+      video_topic_name: /edgetpu_object_detector_visualization/output
+      video_fps: 5.0
+  - name: respeaker_audio_recorder_plugin
+    type: app_recorder/audio_recorder_plugin
+    launch_args:
+      audio_path: /tmp
+      audio_title: go_to_kitchen_audio.wav
+      audio_topic_name: /audio
+      audio_format: wave
+  - name: rosbag_recorder_plugin
+    type: app_recorder/rosbag_recorder_plugin
+    launch_args:
+      rosbag_path: /tmp
+      rosbag_title: go_to_kitchen_rosbag.bag
+      compress: true
+      rosbag_topic_names:
+        - /rosout
+        - /tf
+        - /tf_static
+        - /joint_states
+        - /map
+        - /odom
+        - /odom_combined
+        - /cmd_vel
+        - /move_base/NavFnROS/plan
+        - /move_base/TrajectoryPlannerROS/global_plan
+        - /move_base/TrajectoryPlannerROS/local_plan
+        - /move_base/global_costmap/footprint
+        - /spots_marker_array
+        - /spots_pictogram
+        - /safe_teleop_base/local_costmap/costmap
+        - /move_base/local_costmap/costmap
+        - /move_base/global_costmap/costmap
+        - /particlecloud
+        - /base_scan/throttled
+        - /head_camera/rgb/throttled/camera_info
+        - /head_camera/depth_registered/throttled/camera_info
+        - /head_camera/rgb/throttled/image_rect_color/compressed
+        - /head_camera/depth_registered/throttled/image_rect/compressedDepth
+        - /audio
+  - name: result_recorder_plugin
+    type: app_recorder/result_recorder_plugin
+    plugin_args:
+      result_path: /tmp
+      result_title: go_to_kitchen_result.yaml
+  - name: gdrive_uploader_plugin
+    type: app_uploader/gdrive_uploader_plugin
+    plugin_args:
+      upload_file_paths:
+        - /tmp/go_to_kitchen_result.yaml
+        - /tmp/go_to_kitchen_head_camera.avi
+        - /tmp/go_to_kitchen_object_detection.avi
+        - /tmp/go_to_kitchen_audio.wav
+        - /tmp/go_to_kitchen_rosbag.bag
+      upload_file_titles:
+        - go_to_kitchen_result.yaml
+        - go_to_kitchen_head_camera.avi
+        - go_to_kitchen_object_detection.avi
+        - go_to_kitchen_audio.wav
+        - go_to_kitchen_rosbag.bag
+      upload_parents_path: fetch_go_to_kitchen
+      upload_server_name: /gdrive_server
+  - name: speech_notifier_plugin
+    type: app_notifier/speech_notifier_plugin
+    plugin_args:
+      client_name: /sound_play
+  - name: mail_notifier_plugin
+    type: app_notifier/mail_notifier_plugin
+    plugin_args:
+      mail_title: Fetch kitchen patrol demo
+      use_timestamp_title: true
+    plugin_arg_yaml: /var/lib/robot/fetch_mail_notifier_plugin.yaml
+plugin_order:
+  start_plugin_order:
+    - head_camera_video_recorder_plugin
+    - object_detection_video_recorder_plugin
+    - respeaker_audio_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin
+  stop_plugin_order:
+    - head_camera_video_recorder_plugin
+    - object_detection_video_recorder_plugin
+    - respeaker_audio_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - speech_notifier_plugin
+    - mail_notifier_plugin

jsk_fetch_robot/jsk_fetch_startup/apps/speak_battery/speak_battery.xml

@@ -1,4 +1,5 @@
 <launch>
   <node name="run_speak_battery" pkg="roseus" type="roseus"
-        args="$(find jsk_fetch_startup)/euslisp/speak-battery.l"/>
+        args="$(find jsk_fetch_startup)/euslisp/speak-battery.l"
+        output="screen" required="true" />
 </launch>

jsk_fetch_robot/jsk_fetch_startup/apps/time_signal/time_signal.app

@@ -3,11 +3,13 @@ platform: fetch
 launch: jsk_fetch_startup/time_signal.xml
 interface: jsk_fetch_startup/time_signal.interface
 icon: jsk_fetch_startup/time_signal.png
+timeout: 10
 plugins:
   - name: user_speech_notifier_plugin
     type: app_notifier/user_speech_notifier_plugin
     plugin_args:
       client_name: /sound_play
+      warning: false
 plugin_order:
   start_plugin_order:
     - user_speech_notifier_plugin

jsk_fetch_robot/jsk_fetch_startup/apps/time_signal/time_signal.xml

@@ -1,3 +1,4 @@
 <launch>
-  <node name="run_time_signal" pkg="jsk_fetch_startup" type="time_signal.py" output="screen"/>
+  <node name="run_time_signal" pkg="jsk_fetch_startup" type="time_signal.py"
+        output="screen" required="true" />
 </launch>

jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l

@@ -11,6 +11,16 @@
 (defparameter *tfl* (instance ros::transform-listener :init))
 
 
+(let ((robot-name (ros::get-param "/robot/name")))
+  (defparameter *dock-spot*
+    (cond
+      ((equal robot-name "fetch15")
+       "/eng2/7f/room73B2-fetch-dock-front")
+      ((equal robot-name "fetch1075")
+       "/eng2/7f/room73B2-fetch-dock-entrance")
+      (t nil))))
+
+
 (defun get-spot-coords (name)
   (unless *spots*
     (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray)))

jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l

@@ -19,3 +19,4 @@
 
 (ros::roseus "speak_battery")
 (speak-battery)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch

@@ -0,0 +1,119 @@
+<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_visual_odom" default="false" />
+
+  <!-- GDB Debug Option -->
+  <arg name="debug" default="false"/>
+  <arg unless="$(arg debug)" name="launch_prefix" value=""/>
+  <arg if="$(arg debug)" name="launch_prefix" value="gdb -x $(find fetch_bringup)/launch/gdb_settings.gdb --ex run --args"/>
+
+  <!-- calibration: s-kitagawa (2019/12/11) -->
+  <!-- Calibration -->
+  <param name="calibration_date" value="2020-12-04 02:08:50"/>
+  <param name="base_controller/track_width" value="0.37476"/>
+  <param name="head_camera/driver/z_offset_mm" value="4"/>
+  <param name="head_camera/driver/z_scaling" value="1.0202112"/>
+  <!-- use symbolic link: s-kitagawa (2020/09/18) -->
+  <arg name="rgb_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>
+  <arg name="depth_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>
+
+  <!-- Odometry -->
+  <param name="base_controller/publish_tf" value="false"/>
+  <!-- stop using graft: s-kitagawa (2019/10/17) -->
+  <!-- <include file="$(find fetch_bringup)/launch/include/graft.launch.xml"/> -->
+
+  <!-- Odometry without visual odom -->
+  <!-- use robot localization ukf -->
+  <node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true" unless="$(arg use_visual_odom)">
+    <remap from="odometry/filtered" to="/odom_combined" />
+    <rosparam>
+      frequency: 50
+      sensor_timeout: 1.0
+      two_d_mode: true
+      publish_tf: true
+      publish_acceleration: false
+      map_frame: map
+      odom_frame: odom
+      base_link_frame: base_link
+      odom0: /odom_corrected
+      odom0_config: [true, true, false,
+                     false, false, true,
+                     true, true, false,
+                     false, false, true,
+                     false, false, false]
+      odom0_nodelay: true
+      odom0_differential: true
+    </rosparam>
+  </node>
+
+  <!-- Use Visual Odom -->
+  <include file="$(find jsk_fetch_startup)/launch/fetch_t265.launch" if="$(arg use_visual_odom)">
+      <arg name="topic_odom_out" value="/odom_visual" />
+      <arg name="odom_frame_id" value="odom" />
+      <arg name="base_link_frame_id" value="base_link" />
+  </include>
+
+  <!-- /imu has no frame_id information and there is no bug fix release in indigo. -->
+  <!-- see https://github.com/fetchrobotics/fetch_ros/issues/70 -->
+  <node name="imu_corrector" pkg="jsk_fetch_startup" type="imu_corrector.py">
+    <remap from="~input" to="/imu" />
+    <remap from="~output" to="/imu_corrected" />
+  </node>
+
+  <!-- /odom has no covariance value. -->
+  <node name="odom_corrector" pkg="jsk_fetch_startup" type="odom_corrector.py">
+    <remap from="~input" to="/odom" />
+    <remap from="~output" to="/odom_corrected" />
+  </node>
+
+  <!-- calibration: s-kitagawa (2019/12/11) -->
+  <!-- testing urdf with head box by shinjo (2020/02/13) -->
+  <!-- URDF -->
+  <param name="robot_description" command="$(find xacro)/xacro $(find jsk_fetch_startup)/robots/jsk_fetch.urdf.xacro ros_distro:=$(env ROS_DISTRO) base_camera_mount:=$(arg base_camera_mount) head_box:=$(arg head_box)" />
+
+  <!-- Drivers for Base -->
+  <node name="robot_driver" launch-prefix="$(arg launch_prefix)" pkg="fetch_drivers" type="robot_driver" output="screen">
+    <param name="firmware_tar_gz" value="$(find fetch_drivers)/firmware.tar.gz"/>
+    <param name="has_base" value="true"/>
+    <param name="has_torso" value="true"/>
+    <param name="has_head" value="true"/>
+    <param name="has_arm" value="true"/>
+  </node>
+
+  <!-- Drivers for Gripper -->
+  <node name="gripper_driver" pkg="fetch_drivers" type="gripper_driver" output="screen">
+    <param name="firmware_tar_gz" value="$(find fetch_drivers)/firmware.tar.gz"/>
+  </node>
+
+  <!-- Controllers -->
+  <rosparam file="$(find fetch_bringup)/config/default_controllers.yaml" command="load"/>
+
+  <!-- Joint State -> TF -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
+
+  <!-- Head Camera -->
+  <include file="$(find fetch_bringup)/launch/include/head_camera.launch.xml">
+    <arg name="rgb_camera_info_url" value="$(arg rgb_camera_info_url)"/>
+    <arg name="depth_camera_info_url" value="$(arg depth_camera_info_url)"/>
+  </include>
+
+  <!-- Laser -->
+  <include file="$(find fetch_bringup)/launch/include/laser.launch.xml"/>
+
+  <!-- add arg launch_teleop: yamaguchi & s-kitagawa (2019/04/19) -->
+  <!-- Teleop -->
+  <include file="$(find fetch_bringup)/launch/include/teleop.launch.xml" if="$(arg launch_teleop)"/>
+
+  <!-- enable software runstop from joy: s-kitagawa (2019/11/09) -->
+  <!-- Software Runstop -->
+  <include file="$(find fetch_bringup)/launch/include/runstop.launch.xml">
+    <arg name="flags" value="-a -b -g -t"/>
+  </include>
+
+  <!-- Diagnostics Aggregator -->
+  <include file="$(find fetch_bringup)/launch/include/aggregator.launch.xml"/>
+
+</launch>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_t265.launch

@@ -0,0 +1,48 @@
+<launch>
+
+  <arg name="topic_odom_in" default="/t265/odom/sample" />
+  <arg name="topic_odom_out" default="odom_visual" />
+  <arg name="odom_frame_id" default="odom_visual" />
+  <arg name="base_link_frame_id" default="base_link_virtual_2" />
+  <arg name="base_link_virtual_frame_id" default="base_link_virtual" />
+  <arg name="publish_tf" default="true" />
+
+  <node
+      pkg="tf"
+      type="static_transform_publisher"
+      name="t265_to_base_link_virtual_joint"
+      args="-0.441 -0.008 -0.225 0 0.524 0 t265_pose_frame $(arg base_link_virtual_frame_id) 10"
+      output="screen"
+      />
+
+  <node
+      pkg="jsk_fetch_startup"
+      type="odometry_transformer.py"
+      name="odometry_transformer"
+      output="screen"
+      >
+      <remap from="~odom_in" to="$(arg topic_odom_in)" />
+      <remap from="~odom_out" to="$(arg topic_odom_out)" />
+
+      <param name="~frame_id_base_link" value="$(arg base_link_frame_id)" />
+      <param name="~frame_id_base_link_virtual" value="$(arg base_link_virtual_frame_id)" />
+      <param name="~frame_id_pose_frame" value="t265_pose_frame" />
+      <param name="~frame_id_odom" value="$(arg odom_frame_id)" />
+      <param name="~publish_tf" value="$(arg publish_tf)" />
+      <param name="~2d_mode" value="true" />
+  </node>
+
+  <rosparam>
+    /t265/tracking_module/enable_mapping: false
+    /t265/tracking_module/enable_relocalization: false
+    /t265/tracking_module/enable_pose_jumping: false
+    /t265/tracking_module/enable_dynamic_calibration: true
+    /t265/tracking_module/enable_map_preservation: false
+  </rosparam>
+
+  <include file="$(find realsense2_camera)/launch/rs_t265.launch" >
+      <arg name="camera" value="t265" />
+      <arg name="publish_odom_tf" value="false" />
+  </include>
+
+</launch>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml

@@ -15,7 +15,7 @@
       <remap from="joy" to="joy_ds3" />
     </node>
 
-    <node name="joy_remap" pkg="joy" type="joy_remap.py" output="screen" >
+    <node name="joy_remap" pkg="joy" type="joy_remap.py" output="screen" respawn="true">
       <remap from="joy_in" to="joy_ds3" />
       <remap from="joy_out" to="joy" />
       <rosparam command="load" file="$(find jsk_fetch_startup)/config/ds3joy_$(env ROS_DISTRO).yaml" />
@@ -94,6 +94,6 @@
   <node pkg="jsk_robot_startup" type="unsafe_warning.l" name="unsafe_warning" respawn="true" />
 
   <!-- auto dock -->
-  <include file="$(find fetch_auto_dock)/launch/auto_dock.launch"
+  <include file="$(find fetch_open_auto_dock)/launch/auto_dock.launch"
 	   if="$(arg enable_auto_dock)"/>
 </launch>