Merge branch 'master' into kinova-gen3

.travis.rosinstall

@@ -1 +1,8 @@
-# see .travis.rosinstall.ROS_DISTRO
+# see also .travis.rosinstall.ROS_DISTRO
+# waiting for jsk_maps release
+# add keepout map for entire eng2 building
+# https://github.com/jsk-ros-pkg/jsk_demos/pull/1300
+- git:
+    local-name: jsk-ros-pkg/jsk_demos
+    uri: https://github.com/jsk-ros-pkg/jsk_demos.git
+    version: master

.travis.rosinstall.kinetic

@@ -21,10 +21,6 @@
 - git:
    uri: https://github.com/ros-naoqi/nao_interaction.git
    local-name: ros-naoqi/nao_interaction
-- git:
-   uri: https://github.com/tork-a/jsk_demos-release.git
-   local-name: jsk_maps
-   version: release/kinetic/jsk_maps
 # jsk_fetch_startup requires fetch_ros with following PRs.
 # https://github.com/fetchrobotics/fetch_ros/pull/144
 # https://github.com/fetchrobotics/fetch_ros/pull/146

.travis.rosinstall.melodic

@@ -19,10 +19,6 @@
    uri: https://github.com/ros-planning/moveit_robots.git
    local-name: ros-planning/moveit_robots
    version: kinetic-devel
-- git: # wait until releasing
-   uri: https://github.com/tork-a/jsk_demos-release.git
-   local-name: jsk_maps
-   version: release/kinetic/jsk_maps
 - git: # wait for melodic release https://github.com/ros-naoqi/pepper_robot/issues/50
    uri: https://github.com/ros-naoqi/pepper_robot.git
    local-name: pepper_robot
@@ -40,3 +36,9 @@
     local-name: fetchrobotics/fetch_ros
     uri: https://github.com/fetchrobotics/fetch_ros.git
     version: ba4bafdb802e474487d1e0d893aa25c9535dc55f
+# for drc_task_common in jsk_demos
+# jsk_control is not released in melodic
+- tar:
+    local-name: jsk_control/jsk_ik_server
+    uri: https://github.com/tork-a/jsk_control-release/archive/release/kinetic/jsk_ik_server/0.1.14-0.tar.gz
+    version: jsk_control-release-release-kinetic-jsk_ik_server-0.1.14-0

.travis.yml

@@ -1,14 +1,16 @@
-language: c++
+sudo: true
+dist: bionic
+language: python
+services:
+  - docker
 cache:
   apt: true
   pip: true
   directories:
     - $HOME/.ccache
+    - $HOME/.cache/pip
     - $HOME/apt-cacher-ng
-sudo: required
-services:
-  - docker
-dist: trusty
+    - $HOME/.ros/data
 notifications:
   email:
     on_success: always

jsk_fetch_robot/jsk_fetch_startup/README.md

@@ -24,9 +24,9 @@ sudo mv rockmongo-1.1.7 /var/www/html/rockmongo
 
 ### Teleoperation
 
-Below is a flow of commands from Joystick sent to base controller:
+For the JSK safe teleop system, please see [data flow diagram of safe_teleop.launch](https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_robot_common/jsk_robot_startup#launchsafe_teleoplaunch)
 
-![safe_teleop_system](https://user-images.githubusercontent.com/19769486/37566992-dab73142-2b03-11e8-94bc-4d1914c967b0.png)
+The numbers assigned to the joystick are as follows.
 
 ![joystick_numbered](https://user-images.githubusercontent.com/19769486/28101905-889e9cc2-6706-11e7-9981-5704cc29f2b3.png)
 ![joystick_numbered2](https://user-images.githubusercontent.com/19769486/28101906-88b5f20a-6706-11e7-987c-d94e64ac2cc1.png)

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -7,15 +7,14 @@
   <arg name="map_frame" default="eng2" />
   <arg name="map_file" default="$(find jsk_maps)/raw_maps/eng2-7f-0.05.yaml"/>
   <arg name="keepout_map_file" default="$(find jsk_maps)/raw_maps/eng2-7f-0.05_keepout.yaml" />
+  <arg name="use_keepout" default="true" />
 
   <param name="robot/type" value="fetch" />
   <param name="robot/name" command='bash -c "hostname | xargs echo -n"' />
 
   <include file="$(find jsk_fetch_startup)/jsk_fetch.machine" />
 
   <!-- add jsk startups -->
-  <node pkg="jsk_fetch_startup" name="warning" type="warning.py" respawn="true" />
-
   <node pkg="jsk_fetch_startup" name="battery_warning" type="battery_warning.py"
         respawn="true" output="screen">
     <rosparam>
@@ -132,6 +131,7 @@
     <!-- jsk_maps -->
     <include file="$(find jsk_maps)/launch/start_map_$(arg map_frame).launch">
       <arg name="launch_map_server" value="true" />
+      <arg name="keepout" value="$(arg use_keepout)" />
     </include>
 
     <!-- dock localization -->
@@ -141,7 +141,7 @@
     <include file="$(find fetch_navigation)/launch/fetch_nav.launch" >
       <arg name="map_file" value="$(arg map_file)" />
       <arg name="map_keepout_file" value="$(arg keepout_map_file)" />
-      <arg name="use_keepout" value="false" />
+      <arg name="use_keepout" value="$(arg use_keepout)" />
       <arg name="use_map_topic" value="true" />
       <arg name="launch_map_server" value="false" />
     </include>

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml

@@ -1,5 +1,6 @@
 <launch>
   <!-- mainly copied from $(find fetch_bringup)/launch/include/teleop.launch.xml -->
+  <arg name="use_safe_teleop" default="true" />
   <arg name="enable_auto_dock" default="true" />
   <arg name="joy_device" default="/dev/ps3joy"/>
   <arg name="launch_fetch_bringup_teleop" default="false" />
@@ -11,13 +12,6 @@
       <param name="autorepeat_rate" value="1"/>
       <param name="dev" value="$(arg joy_device)"/>
     </node>
-
-    <!-- need to launch cmd_vel_mux because of launch_teleop:=false is set at /etc/ros/indigo/robot.launch,
-         see https://github.com/fetchrobotics/fetch_robots/pull/40 -->
-    <node name="cmd_vel_mux" pkg="topic_tools" type="mux" respawn="true" output="screen"
-          args="base_controller/command /cmd_vel /teleop/cmd_vel">
-      <remap from="mux" to="cmd_vel_mux" />
-    </node>
   </group>
 
   <!-- copied for button mapping is changed to standardize with PR2 joy.
@@ -27,13 +21,48 @@
        joystick_teleop need to publish /teleop/cmd_vel/unsafe ,
        see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/README.md -->
   <node name="teleop" pkg="fetch_teleop" type="joystick_teleop" respawn="true">
-    <remap from="teleop/cmd_vel" to="/teleop/cmd_vel/unsafe" />
+    <remap from="teleop/cmd_vel" to="/joy_vel" />
     <param name="arm/button_arm_linear" value="4"/>
     <param name="arm/button_arm_angular" value="6"/>
     <param name="head/button_deadman" value="11"/>
     <param name="base/use_mux" value="false" />
   </node>
 
+  <!-- Mux cmd_vel topics -->
+  <include file="$(find jsk_robot_startup)/launch/safe_teleop.launch" >
+    <arg name="use_safe_teleop" value="$(arg use_safe_teleop)" />
+    <arg name="odom_topic" value="/odom_teleop"/>
+    <arg name="joy_topic" value="/joy_vel"/>
+    <arg name="navigation_topic" value="/navigation/cmd_vel"/>
+    <arg name="input_topic" value="/input_vel"/>
+  </include>
+  <!-- Relay topic names according to system diagram -->
+  <node name="relay_odom"
+        pkg="topic_tools" type="relay"
+        args="/odom_combined /odom_teleop" />
+  <node name="relay_navigation_cmd_vel"
+        pkg="topic_tools" type="relay"
+        args="/cmd_vel /navigation/cmd_vel" />
+  <group ns="safe_teleop_base">
+    <rosparam file="$(find fetch_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find fetch_navigation)/config/fetch/costmap_common.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find fetch_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find fetch_navigation)/config/fetch/costmap_local.yaml" command="load" ns="local_costmap" />
+    <rosparam ns="local_costmap">
+      inflater:
+        inflation_radius: 1.0 # 0.7
+        cost_scaling_factor: 3.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>
+  </group>
+
+  <!-- Do not publish /base_controller/command if fetch is docking -->
+  <node pkg="jsk_fetch_startup" name="warning" type="warning.py" respawn="true" />
+
   <!-- copied for button mapping is changed to standardize with PR2 joy. -->
   <node name="controller_reset" pkg="fetch_bringup" type="controller_reset.py">
     <param name="reset_button" value="7" />
@@ -45,49 +74,6 @@
     <param name="tuck_button" value="5"/>
   </node>
 
-  <!-- safe teleop -->
-  <node name="unsafe_vel_mux" pkg="topic_tools" type="mux" respawn="true"
-	args="/teleop/cmd_vel /teleop/cmd_vel/safe /teleop/cmd_vel/unsafe">
-    <remap from="mux" to="unsafe_vel_mux" />
-  </node>
-
-  <!-- m.button[10]: L1 -->
-  <!-- m.button[9] : R2 -->
-  <!-- safe teleop with L1 -->
-  <node name="cmd_vel_mux_selector" pkg="jsk_robot_startup" type="mux_selector.py"
-	respawn="true"
-	args="/joy 'm.buttons[10]==1' /teleop/cmd_vel /cmd_vel 'True' /cmd_vel">
-    <remap from="mux" to="cmd_vel_mux" />
-    <param name="default_select" value="/cmd_vel" />
-  </node>
-
-  <!-- unsafe teleop with L1 and R2 -->
-  <node name="unsafe_vel_mux_selector" pkg="jsk_robot_startup" type="mux_selector.py"
-	respawn="true"
-	args="/joy 'm.buttons[10]==1 and m.buttons[9]==1' /teleop/cmd_vel/unsafe /joy 'm.buttons[10]==1 and m.buttons[9]==0' /teleop/cmd_vel/safe">
-    <remap from="mux" to="unsafe_vel_mux" />
-    <param name="default_select" value="/teleop/cmd_vel/safe" />
-  </node>
-
-  <node name="safe_teleop_base" pkg="safe_teleop_base" type="safe_teleop_base">
-    <remap from="base_velocity" to="/teleop/cmd_vel/unsafe" />
-    <remap from="~safe_vel"     to="/teleop/cmd_vel/safe" />
-    <rosparam file="$(find fetch_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find fetch_navigation)/config/fetch/costmap_common.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find fetch_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find fetch_navigation)/config/fetch/costmap_local.yaml" command="load" ns="local_costmap" />
-  </node>
-  <rosparam ns="safe_teleop_base/local_costmap">
-    inflater:
-      inflation_radius: 1.0 # 0.7
-      cost_scaling_factor: 3.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>
-
   <node name="safe_tilt_head" pkg="jsk_fetch_startup" type="safe_tilt_head.py" />
 
   <!-- audible warning on unsafe teleop -->

jsk_fetch_robot/jsk_fetch_startup/scripts/warning.py

@@ -89,10 +89,12 @@ def __init__(self):
         self.base_breaker = rospy.ServiceProxy('base_breaker', BreakerCommand)
         #
         self.battery_sub = rospy.Subscriber("battery_state", BatteryState, self.battery_callback, queue_size = 1)
-        self.cmd_vel_sub = rospy.Subscriber("base_controller/command", Twist, self.cmd_vel_callback, queue_size = 1)
+        self.cmd_vel_sub = rospy.Subscriber("base_controller/command_unchecked", Twist, self.cmd_vel_callback, queue_size = 1)
         self.robot_state_sub = rospy.Subscriber("robot_state", RobotState, self.robot_state_callback, queue_size = 1)
         self.diagnostics_status_sub = rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_status_callback, queue_size = 1)
         self.undock_sub = rospy.Subscriber("/undock/status", GoalStatusArray, self.undock_status_callback)
+        #
+        self.cmd_vel_pub = rospy.Publisher("base_controller/command", Twist, queue_size=1)
 
     def undock_status_callback(self, msg):
         for status in msg.status_list:
@@ -131,6 +133,8 @@ def cmd_vel_callback(self, msg):
             sound.play(4) # play builtin sound Boom!
             time.sleep(5)
             self.base_breaker(BreakerCommandRequest(enable=True))
+        else:
+            self.cmd_vel_pub.publish(msg)
         ##
         self.twist_msgs = msg
 

jsk_pr2_robot/README.md

@@ -1,6 +1,10 @@
 jsk_pr2_robot
 =============
 
+## Teleoperation
+
+For the JSK safe teleop system, please see [data flow diagram of safe_teleop.launch](https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_robot_common/jsk_robot_startup#launchsafe_teleoplaunch)
+
 ![teleop_command](images/pr2_teleop_command.png)
 
 

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml

@@ -15,43 +15,31 @@
     <rosparam command="load" file="$(find pr2_base_trajectory_action)/config/pr2_base_link.yaml"/>
     <param name="goal_threshold" value="10.0" /> <!-- ignore goal error -->
     <param name="stopped_velocity_tolerance" value="0.35" /> <!-- ignore goal stopped velocity tolerance -->
-    <remap from="command" to="/navigation/cmd_vel"/>
+    <remap from="command" to="/base_controller/cmd_vel"/>
     <remap from="odom" to="/base_odometry/odom"/>
   </node>
 
-  <node pkg="topic_tools" type="mux" name="unsafe_vel_mux" respawn="true"
-	args="/navigation/unsafe_vel /input_vel /navigation/cmd_vel /teleop/joy_vel" machine="c2">
-    <remap from="mux" to="vel_type_mux" />
-  </node>
-  <!-- args is (topic, condition, select)* -->
-  <node pkg="jsk_robot_startup" type="mux_selector.py" machine="c2"
-	name="vel_type_selector" respawn="true" output="screen"
-	args="/joy 'm.buttons[10]==1' /teleop/joy_vel /navigation/cmd_vel 'True' /navigation/cmd_vel">
-    <remap from="mux" to="vel_type_mux" />
-    <param name="default_select" value="/input_vel"/>
-  </node>
-
-  <node pkg="safe_teleop_base" type="safe_teleop_base" respawn="true"
-        name="safe_teleop_base" machine="c2" clear_params="true">
-    <remap from="/odom" to="/base_odometry/odom"/>
-    <remap from="base_velocity" to="/navigation/unsafe_vel"/>
-    <remap from="~safe_vel" to="/navigation/safe_vel"/>
+  <!-- Mux cmd_vel topics -->
+  <include file="$(find jsk_robot_startup)/launch/safe_teleop.launch">
+    <arg name="machine" value="c2" />
+    <arg name="odom_topic" value="/odom_teleop" />
+    <arg name="joy_topic" value="/joy_vel" />
+    <arg name="navigation_topic" value="/navigation/cmd_vel" />
+    <arg name="input_topic" value="/input_vel" />
+    <arg name="base_controller_topic" value="/base_controller/cmd_vel" />
+  </include>
+  <!-- Relay topic names according to system diagram -->
+  <node name="relay_odom"
+        pkg="topic_tools" type="relay"
+        args="/base_odometry/odom /odom_teleop" />
+  <node name="relay_teleop_cmd_vel"
+        pkg="topic_tools" type="relay"
+        args="/teleop/joy_vel /joy_vel" />
+  <group ns="safe_teleop_base">
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/local_costmap_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/safe_teleop_pr2_params.yaml" command="load" ns="local_costmap"/>
-  </node>
-
-  <!-- switch base velocity, safe or not -->
-  <node pkg="topic_tools" type="mux" name="cmd_vel_mux" respawn="true" machine="c2"
-	args="base_controller/command_unchecked /navigation/unsafe_vel /navigation/safe_vel">
-    <remap from="mux" to="cmd_vel_mux" />
-  </node>
-  <node pkg="jsk_robot_startup" type="mux_selector.py" machine="c2"
-	name="cmd_vel_selector" respawn="true" output="screen"
-	args="/joy 'm.buttons[9]==0 and m.buttons[10]==1' /navigation/safe_vel /input_vel 'True' /navigation/safe_vel">
-    <remap from="mux" to="cmd_vel_mux" />
-    <param name="default_select" value="/navigation/unsafe_vel"/>
-  </node>
+  </group>
 
   <!-- check cable and filter baser_controller/command -->
   <node pkg="roseus" type="roseus" name="cable_warning" respawn="true"

jsk_pr2_robot/jsk_pr2_startup/pr2.launch

@@ -11,6 +11,7 @@
   <arg name="launch_sound_play" default="true" />
   <arg name="launch_mjpeg_server" default="false" />
   <arg name="launch_gripper_sensor" default="true" />
+  <arg name="launch_virtual_force" default="true" />
   <arg name="launch_safety_warning" default="true" />
   <arg name="launch_tablet" default="false" />
   <arg name="launch_jsk_pcl" default="false" />
@@ -121,6 +122,11 @@
     <include file="$(find pr2_gripper_sensor_action)/launch/pr2_gripper_sensor_actions.launch" />
   </group>
 
+  <!-- for virtual force publisher-->
+  <group if="$(arg launch_virtual_force)">
+    <include file="$(find virtual_force_publisher)/launch/dualarm_virtual_force_publisher.launch" />
+  </group>
+
   <!-- for iPad or android apps -->
   <include if="$(arg launch_jsk_pcl)"
            file="$(find jsk_pcl_ros)/launch/pointcloud_screenpoint.launch" >

jsk_robot_common/jsk_robot_startup/README.md

@@ -72,3 +72,48 @@ This script sets initial pose with relative pose from specified TF frame by publ
 ### Subscribing Topics
 
 * `/amcl_pose` (`geometry_msgs/PoseWithcovariancestamped`)
+
+
+## util/mux_selector.py
+
+This node check and select mux input topic on condition of the specified topic.
+This node takes three arguments for one topic.
+The first one is the topic to be monitored.
+When a message from this topic is received, it is assigned as a variable `m`.
+If a condition specified as the second argument,
+this node calls a service to select the topic specified as the third argument.
+
+### Usage
+
+```
+rosrun jsk_robot_startup mux_selector.py /joy1 'm.buttons[9]==1' /cmd_vel1 /joy2 'm.buttons[9]==1' /cmd_vel2
+```
+
+### Parameters
+
+* `~patient` (Double, default: 0.5)
+
+  Indicates the allowable range of the difference between the received topic time and the current time.
+
+* `~frequency` (Double, default: 20.0)
+
+  Frequency of processing loop.
+
+* `~default_select` (String, default: `None`)
+
+  Default topic name.
+
+* `~wait` (Bool, default: `False`)
+
+  If wait is `True`, this node waits for the topic to be received.
+
+### Subscribing Topics
+
+The topic specified in the argument is subscribed.
+
+
+## launch/safe_teleop.launch
+
+This launch file provides a set of nodes for safe teleoperation common to mobile robots. Robot-specific nodes such as `/joy`, `/teleop` or `/cable_warning` must be included in the teleop launch file for each robot, such as [safe_teleop.xml for PR2](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml) or [safe_teleop.xml for fetch](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml).
+
+![JSK teleop_base system](images/jsk_safe_teleop_system.png)