diff --git a/jsk_baxter_robot/baxtereus/baxter-interface.l b/jsk_baxter_robot/baxtereus/baxter-interface.l
index 0ee94ff98d..cc78ecb996 100644
--- a/jsk_baxter_robot/baxtereus/baxter-interface.l
+++ b/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))
diff --git a/jsk_baxter_robot/baxtereus/baxter-softhand-interface.l b/jsk_baxter_robot/baxtereus/baxter-softhand-interface.l
index 7023b2b309..647575af31 100644
--- a/jsk_baxter_robot/baxtereus/baxter-softhand-interface.l
+++ b/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))))
diff --git a/jsk_baxter_robot/jsk_baxter_startup/README.md b/jsk_baxter_robot/jsk_baxter_startup/README.md
index f605d0ad7c..148880c623 100644
--- a/jsk_baxter_robot/jsk_baxter_startup/README.md
+++ b/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
diff --git a/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_play.launch b/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_play.launch
new file mode 100644
index 0000000000..59cc14e94e
--- /dev/null
+++ b/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_play.launch
@@ -0,0 +1,94 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ queue_size: 100
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_record.launch b/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_record.launch
new file mode 100644
index 0000000000..9df527e6af
--- /dev/null
+++ b/jsk_baxter_robot/jsk_baxter_startup/baxter_rosbag_record.launch
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md
index 4f69533439..d7aebcbe7d 100644
--- a/jsk_robot_common/jsk_robot_startup/README.md
+++ b/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 ``<(<)``, ``>(>)``, ``&(&)``, ``'(')`` and ``"(")``.
+
### Usage
```
diff --git a/jsk_robot_common/jsk_robot_startup/scripts/shutdown.py b/jsk_robot_common/jsk_robot_startup/scripts/shutdown.py
index 085bed021a..ff05431dbc 100755
--- a/jsk_robot_common/jsk_robot_startup/scripts/shutdown.py
+++ b/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,6 +32,19 @@ 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(
@@ -31,7 +52,34 @@ def __init__(self):
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:
diff --git a/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py
index 33b7f8eac6..2add3855d3 100755
--- a/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py
+++ b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py
@@ -82,6 +82,11 @@ def _status_cb(self, msg):
rospy.logwarn("smach does not have DESCRIPTION, see https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_robot_common/jsk_robot_startup#smach_to_mailpy for more info")
if 'IMAGE' in local_data_str and local_data_str['IMAGE']:
rospy.loginfo("- image_str -> {}".format(local_data_str['IMAGE'][:64]))
+ if 'INFO' in local_data_str:
+ rospy.loginfo("- info_str -> {}".format(local_data_str['INFO']))
+ else:
+ rospy.logwarn("smach does not have INFO, see https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_robot_common/jsk_robot_startup#smach_to_mailpy for more info")
+
# Store data for every callerid to self.smach_state_list[caller_id]
caller_id = msg._connection_header['callerid']
@@ -143,6 +148,9 @@ def _send_mail(self, subject, state_list):
changeline = EmailBody()
changeline.type = 'html'
changeline.message = "
"
+ separator = EmailBody()
+ separator.type = 'text'
+ separator.message = "---------------"
for x in state_list:
if 'DESCRIPTION' in x:
description = EmailBody()
@@ -157,6 +165,17 @@ def _send_mail(self, subject, state_list):
image.img_data = x['IMAGE']
email_msg.body.append(image)
email_msg.body.append(changeline)
+ email_msg.body.append(changeline)
+ email_msg.body.append(changeline)
+ email_msg.body.append(separator)
+ email_msg.body.append(changeline)
+ for x in state_list:
+ if 'INFO' in x:
+ info = EmailBody()
+ info.type = 'text'
+ info.message = x['INFO']
+ email_msg.body.append(info)
+ email_msg.body.append(changeline)
# rospy.loginfo("body:{}".format(email_msg.body))
if subject:
@@ -172,16 +191,22 @@ def _send_mail(self, subject, state_list):
self.pub_email.publish(email_msg)
def _send_twitter(self, subject, state_list):
- text = ""
+ text = u""
if subject:
+ # In python2, str is byte object, so we need to decode it as utf-8
+ if isinstance(subject, bytes):
+ subject = subject.decode('utf-8')
text += subject
for x in state_list:
if 'DESCRIPTION' in x and x['DESCRIPTION']:
- text += '\n' + x['DESCRIPTION']
+ desc = x['DESCRIPTION']
+ if isinstance(desc, bytes):
+ desc = desc.decode('utf-8')
+ text += '\n' + desc
if 'IMAGE' in x and x['IMAGE']:
img_txt = x['IMAGE']
if isinstance(img_txt, bytes):
- img_txt = img_txt.decode('ascii')
+ img_txt = img_txt.decode('utf-8')
text += img_txt
if len(text) > 1:
self.pub_twitter.publish(String(text))