Merge branch 'master' into pr2-tweet-image-server

jsk_baxter_robot/baxtereus/baxter-interface.l

@@ -2,6 +2,15 @@
 (require :baxter "package://baxtereus//baxter-util.l")
 (ros::load-ros-package "baxter_core_msgs")
 
+(defvar *enable-roseus-resume* (ros::rospack-find "roseus_resume"))
+(if *enable-roseus-resume*
+  (progn
+    (require :roseus_resume "package://roseus_resume/euslisp/interruption-handler.l")
+    (warning-message 2 "roseus_resume is enabled.~%"))
+  (progn
+    (unless (find-package "ROSEUS_RESUME") (make-package "ROSEUS_RESUME"))
+    (warning-message 3 "roseus_resume is disabled.~%")))
+
 (defparameter *suction* 65537) ;;vacuum
 (defparameter *electoric* 65538)
 (defvar *wait-for-suction* 5000000)
@@ -47,6 +56,14 @@
      (ros::spin-once)
      (if mvit-rb (setq moveit-robot mvit-rb))
      (if mvit-env (send self :set-moveit-environment (send mvit-env :init :robot moveit-robot)))
+     (if *enable-roseus-resume*
+       (progn
+         (warning-message 3 "Installing interruption handler...~%")
+         (roseus_resume::install-interruption-handler self
+            right-gripper-action
+            left-gripper-action)
+         (roseus_resume::install-default-intervention self)
+         (warning-message 3 "...done.~%")))
      ))
   (:right-property-cb (msg)
    (setq right-gripper-type (send msg :id))
@@ -383,7 +400,7 @@
    ))
 
 
-(defun baxter-init (&key (safe t) (type :default-controller) (moveit t))
+(defun baxter-init (&key (safe t) (type :default-controller) (moveit t) &allow-other-keys)
   (let (mvit-env mvit-rb)
     (when moveit
       (setq mvit-env (instance baxter-moveit-environment))

jsk_baxter_robot/baxtereus/baxter-softhand-interface.l

@@ -178,7 +178,7 @@
 
 (defun baxter-init
   (&key (safe t) (type :default-controller) (moveit t)
-        (lgripper :softhand) (rgripper :softhand))
+        (lgripper :softhand) (rgripper :softhand) &allow-other-keys)
   (let (mvit-env mvit-rb)
     (if moveit
       (progn
@@ -196,6 +196,11 @@
         (if safe
           (setq *baxter* (instance baxter-robot-safe :init))
           (setq *baxter* (instance baxter-robot :init)))
+        ;; move end-coords for softhand
+        (if (equal lgripper :softhand)
+          (send (send *baxter* :larm :end-coords) :translate #f(20 10 0)))
+        (if (equal rgripper :softhand)
+          (send (send *baxter* :rarm :end-coords) :translate #f(20 -10 0)))
         (send *baxter* :angle-vector (send *ri* :state :potentio-vector))))
     (if (equal lgripper :parallel) (send *ri* :calib-grasp :larm))
     (if (equal rgripper :parallel) (send *ri* :calib-grasp :rarm))))

jsk_baxter_robot/jsk_baxter_startup/README.md

@@ -53,6 +53,18 @@ roseus baxter-softhand-interface.l
 (baxter-init)
 ```
 
+### Rosbag record
+
+```bash
+roslaunch jsk_baxter_startup baxter_rosbag_record.launch
+```
+
+### Rosbag play
+
+```bash
+roslaunch jsk_baxter_startup baxter_rosbag_play.launch
+```
+
 ## ROS nodes
 
 ### xdisplay\_image\_topic.py

jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_play.launch

@@ -0,0 +1,94 @@
+<launch>
+  <arg name="rosbag" />
+  <arg name="launch_rviz" default="true" />
+  <arg name="loop" default="true" />
+  <arg name="lgripper_type" default="softhand" />
+  <arg name="rgripper_type" default="softhand" />
+  <arg name="manager" default="rosbag_play_nodelet_manager" />
+
+  <!-- rosbag flag -->
+  <arg name="loop_flag" value="--loop" if="$(arg loop)" />
+  <arg name="loop_flag" value="" unless="$(arg loop)" />
+
+  <!-- gripper and display-->
+  <arg name="finger" default="standard_narrow" />
+  <arg name="finger_tip" default="paddle_tip" />
+  <arg name="left_electric_gripper" default="$(eval arg('lgripper_type') == 'parallel')" />
+  <arg name="right_electric_gripper" default="$(eval arg('rgripper_type') == 'parallel')" />
+
+  <!-- realsense camera -->
+  <arg name="REALSENSE_CAMERA_NS" value="realsense_torso"/>
+  <arg name="REALSENSE_RGB_IMAGE" value="/$(arg REALSENSE_CAMERA_NS)/color/image_rect_color" />
+  <arg name="REALSENSE_RGB_CAMERA_INFO" value="/$(arg REALSENSE_CAMERA_NS)/color/camera_info" />
+  <arg name="REALSENSE_DEPTH_IMAGE"
+       value="/$(arg REALSENSE_CAMERA_NS)/aligned_depth_to_color/compressed/image_raw" />
+  <arg name="REALSENSE_DEPTH_CAMERA_INFO"
+       value="/$(arg REALSENSE_CAMERA_NS)/aligned_depth_to_color/compressed/camera_info" />
+
+  <!-- spherical camera -->
+  <arg name="SPHERICAL_CAMERA_NS" value="kodak_head" />
+  <arg name="SPHERICAL_RGB_IMAGE" value="/$(arg SPHERICAL_CAMERA_NS)/image_raw" />
+
+  <!-- spherical stereo camera -->
+  <arg name="SPHERICAL_LEFT_CAMERA_NS" value="elp_head_left" />
+  <arg name="SPHERICAL_RIGHT_CAMERA_NS" value="elp_head_right" />
+  <arg name="SPHERICAL_LEFT_RGB_IMAGE" value="/$(arg SPHERICAL_LEFT_CAMERA_NS)/image_raw" />
+  <arg name="SPHERICAL_RIGHT_RGB_IMAGE" value="/$(arg SPHERICAL_RIGHT_CAMERA_NS)/image_raw" />
+
+  <!-- baxter params -->
+  <param name="/use_sim_time" value="true" />
+  <param name="/robot_description"
+         command="$(find xacro)/xacro --inorder
+                  $(find jsk_baxter_startup)/jsk_baxter_description/baxter.urdf.xacro
+                  gazebo:=false finger:=$(arg finger) finger_tip:=$(arg finger_tip)
+                  left_electric_gripper:=$(arg left_electric_gripper)
+                  right_electric_gripper:=$(arg right_electric_gripper)" />
+
+  <!-- rosbag play -->
+  <node pkg="rosbag" type="play" name="rosbag_play"
+        args="$(arg rosbag) $(arg loop_flag) --clock" output="screen" />
+
+  <!-- robot state publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="joint_states" to="/robot/joint_states"/>
+  </node>
+
+  <!-- nodelet manager -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" />
+
+  <!-- realsense rgb, depth and pointcloud -->
+  <node name="realsense_rgb_decompress" pkg="image_transport" type="republish"
+        args="compressed in:=$(arg REALSENSE_RGB_IMAGE)
+              raw out:=$(arg REALSENSE_RGB_IMAGE)" />
+  <node name="realsense_depth_decompress" pkg="image_transport" type="republish"
+        args="compressedDepth in:=$(arg REALSENSE_DEPTH_IMAGE)
+              raw out:=$(arg REALSENSE_DEPTH_IMAGE)" />
+  <node name="realsense_point_cloud_xyzrgb" pkg="nodelet" type="nodelet"
+        args="load depth_image_proc/point_cloud_xyzrgb $(arg manager)" output="screen" >
+    <remap from="rgb/camera_info" to="$(arg REALSENSE_RGB_CAMERA_INFO)" />
+    <remap from="rgb/image_rect_color" to="$(arg REALSENSE_RGB_IMAGE)" />
+    <remap from="depth_registered/image_rect" to="$(arg REALSENSE_DEPTH_IMAGE)" />
+    <remap from="depth_registered/points" to="/$(arg REALSENSE_CAMERA_NS)/depth_registered/points" />
+    <rosparam>
+      queue_size: 100
+    </rosparam>
+  </node>
+
+  <!-- spherical rgb -->
+  <node name="spherical_rgb_decompress" pkg="image_transport" type="republish"
+        args="compressed in:=$(arg SPHERICAL_RGB_IMAGE)
+              raw out:=$(arg SPHERICAL_RGB_IMAGE)" />
+
+  <!-- spherical stereo rgb -->
+  <node name="spherical_left_rgb_decompress" pkg="image_transport" type="republish"
+        args="compressed in:=$(arg SPHERICAL_LEFT_RGB_IMAGE)
+              raw out:=$(arg SPHERICAL_LEFT_RGB_IMAGE)" />
+  <node name="spherical_right_rgb_decompress" pkg="image_transport" type="republish"
+        args="compressed in:=$(arg SPHERICAL_RIGHT_RGB_IMAGE)
+              raw out:=$(arg SPHERICAL_RIGHT_RGB_IMAGE)" />
+
+  <!-- launch rviz -->
+  <node if="$(arg launch_rviz)" name="$(anon rviz)" pkg="rviz" type="rviz"
+        args="-d $(find jsk_baxter_startup)/config/baxter_default.rviz" />
+
+</launch>

jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_record.launch

@@ -0,0 +1,37 @@
+<launch>
+  <arg name="output_prefix" default="baxter" />
+  <arg name="compress" default="false" doc="whether compress rosbag or not." />
+
+  <arg name="realsense_camera_ns" default="realsense_torso" />
+  <arg name="spherical_camera_ns" default="kodak_head" />
+  <arg name="spherical_left_camera_ns" default="elp_head_left" />
+  <arg name="spherical_right_camera_ns" default="elp_head_right" />
+
+  <!-- compress flag-->
+  <arg if="$(arg compress)" name="compress_flag" value="--bz2" />
+  <arg unless="$(arg compress)" name="compress_flag" value="" />
+
+  <!-- rosbag -->
+  <node name="baxter_rosbag_record" pkg="rosbag" type="record"
+        args="/rosout
+              /robot/joint_states
+              /diagnostics_agg
+              /tf
+              /tf_static
+              /audio
+              /$(arg realsense_camera_ns)/color/image_rect_color/compressed
+              /$(arg realsense_camera_ns)/color/camera_info
+              /$(arg realsense_camera_ns)/depth/compressed/image_rect_raw/compressedDepth
+              /$(arg realsense_camera_ns)/depth/compressed/camera_info
+              /$(arg realsense_camera_ns)/aligned_depth_to_color/compressed/image_raw/compressedDepth
+              /$(arg realsense_camera_ns)/aligned_depth_to_color/compressed/camera_info
+              /$(arg spherical_camera_ns)/image_raw/compressed
+              /$(arg spherical_camera_ns)/camera_info
+              /$(arg spherical_left_camera_ns)/image_raw/compressed
+              /$(arg spherical_left_camera_ns)/camera_info
+              /$(arg spherical_right_camera_ns)/image_raw/compressed
+              /$(arg spherical_right_camera_ns)/camera_info
+              -q $(arg compress_flag) -b 0
+              --output-prefix $(arg output_prefix)"
+        output="screen" />
+</launch>

jsk_robot_common/jsk_robot_startup/README.md

@@ -181,12 +181,21 @@ This node shuts down or reboots the robot itself according to the rostopic. Note
 
 * `shutdown` (`std_msgs/Empty`)
 
-  Input topic that trigger shutdown
+  Input topic that trigger shutdown.
+
+  If `~input_condition` is set, evaluated `~input_condition` is `True` and this node received this topic, shutdown will be executed.
+
+  If you want to force a shutdown in any case, set `~input_condition` to `None` and send `shutdown` topic.
 
 * `reboot` (`std_msgs/Empty`)
 
   Input topic that trigger reboot
 
+* `~input` (`AnyMsg`)
+
+  Input ros message for `~input_condition`.
+
+
 ### Parameters
 
 * `~shutdown_command` (String, default: "/sbin/shutdown -h now")
@@ -197,6 +206,47 @@ This node shuts down or reboots the robot itself according to the rostopic. Note
 
   Command to reboot the system. You can specify the reboot command according to your system.
 
+* `~input_condition` (String, default: ``None``)
+
+  Specify condition to run `~shutdown_command` even if shutdown topic is received. Use a Python expression that returns a bool value.
+  In addition to a Python builtin functions, you can use ``topic`` (the topic of the message), ``m`` (the message) and ``t`` (time of message).
+
+  For example, ``~input`` topic is ``std_msgs/String`` and if you want to check whether a sentence is a ``hello``, you can do the following.
+
+  ```
+  input_condition: m.data == 'hello'
+  ```
+
+  If you want to check the frame id of the header, you can do the following.
+
+  ```
+  input1_condition: m.header.frame_id in ['base', 'base_link']
+  ```
+
+  For example, to prevent shutdown while the real Fetch is charging, write as follows.
+
+  ```
+  input_condition: 'm.is_charging is False'
+  ```
+
+  In this case, the `~input` is the `/battery_state` (`power_msgs/BatteryState`) topic.
+  `power_msgs/BatteryState` has the following values and `is_charging` is `True` if charging, `False` otherwise.
+
+  ```
+  $ rosmsg show power_msgs/BatteryState
+  string name
+  float32 charge_level
+  bool is_charging
+  duration remaining_time
+  float32 total_capacity
+  float32 current_capacity
+  float32 battery_voltage
+  float32 supply_voltage
+  float32 charger_voltage
+  ```
+
+  Note that, use escape sequence when using the following symbols ``<(&lt;)``, ``>(&gt;)``, ``&(&amp;)``, ``'(&apos;)`` and ``"(&quot;)``.
+
 ### Usage
 
 ```

jsk_robot_common/jsk_robot_startup/scripts/shutdown.py

@@ -1,10 +1,18 @@
 #!/usr/bin/env python
 
 import os
+from importlib import import_module
+
 import rospy
 from std_msgs.msg import Empty
 
 
+def expr_eval(expr):
+    def eval_fn(topic, m, t):
+        return eval(expr)
+    return eval_fn
+
+
 class Shutdown(object):
     """
     This node shuts down or reboots the robot itself
@@ -24,14 +32,54 @@ class Shutdown(object):
 
     def __init__(self):
         rospy.loginfo('Start shutdown node.')
+
+        self.condition = rospy.get_param(
+            '~input_condition', None)
+        if self.condition is not None:
+            self.topic_name = rospy.resolve_name('~input')
+            self.filter_fn = expr_eval(self.condition)
+            self.last_received_topic = None
+            self.sub_check_topic = rospy.Subscriber(
+                self.topic_name,
+                rospy.AnyMsg,
+                callback=self.callback,
+                queue_size=1)
+
         rospy.Subscriber('shutdown', Empty, self.shutdown)
         rospy.Subscriber('reboot', Empty, self.reboot)
         self.shutdown_command = rospy.get_param(
             '~shutdown_command', '/sbin/shutdown -h now')
         self.reboot_command = rospy.get_param(
             '~reboot_command', '/sbin/shutdown -r now')
 
+    def callback(self, msg):
+        if isinstance(msg, rospy.msg.AnyMsg):
+            package, msg_type = msg._connection_header['type'].split('/')
+            ros_pkg = package + '.msg'
+            msg_class = getattr(import_module(ros_pkg), msg_type)
+            self.sub_check_topic.unregister()
+            deserialized_sub = rospy.Subscriber(
+                self.topic_name, msg_class, self.callback)
+            self.sub_check_topic = deserialized_sub
+            msg = msg_class().deserialize(msg._buff)
+        self.last_received_topic = self.topic_name, msg, rospy.Time.now()
+
     def shutdown(self, msg):
+        if self.condition is not None:
+            if self.last_received_topic is None:
+                rospy.loginfo(
+                    'received shutdown. '
+                    'However input topic "{}" has not been received.'
+                    .format(self.topic_name))
+                return
+            topic_name, m, t = self.last_received_topic
+            if self.filter_fn(topic_name, m, t) is False:
+                rospy.loginfo(
+                    'received shutdown. '
+                    'However condition "{}" is not satisfied.'
+                    .format(self.condition))
+                return
+
         rospy.loginfo('Shut down robot.')
         ret = os.system(self.shutdown_command)
         if ret != 0: