This PR added commits related to smach_to_mail.py that is in develop/…

…fetch branch (#1884)

* [jsk_robot_startup] Add timeout for smach notification

* [jsk_robot_startup] Refactor error handling in getting rosparam

* Fix sending notification every times if demo stuck and refactor _stop_timer_cb

* Fix unintended smach notification when demo fails

* [jsk_robot_startup] Use checking node status for timeout

* [jsk_robot_startup] Support dynamic reconfigure in smach_to_mail to send google chat for every state transition

* Update jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py

Co-authored-by: Naoto Tsukamoto <[email protected]>

---------

Co-authored-by: Naoto Tsukamoto <[email protected]>

jsk_robot_common/jsk_robot_startup/CMakeLists.txt

@@ -53,6 +53,7 @@ generate_dynamic_reconfigure_options(
   cfg/OdometryFeedbackWrapperReconfigure.cfg
   cfg/ConstantHeightFramePublisherReconfigure.cfg
   cfg/JointStatesThrottle.cfg
+  cfg/SmachNotificationReconfigure.cfg
 )
 
 add_message_files(

jsk_robot_common/jsk_robot_startup/cfg/SmachNotificationReconfigure.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PKG = "jsk_robot_startup"
+
+gen = ParameterGenerator()
+
+gen.add("send_every_transition", bool_t, 0, "Send notification every transition", False)
+gen.add("use_mail", bool_t, 0, "Use mail for smach notification", True)
+gen.add("use_twitter", bool_t, 0, "Use twitter for smach notification", True)
+gen.add("use_google_chat", bool_t, 0, "Use google chat for smach notification", True)
+
+exit(gen.generate(PKG, PKG, "SmachNotificationReconfigure"))

jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py

@@ -9,9 +9,12 @@
 import os
 import pickle
 import rospy
+import rosnode
 import sys
 
 from cv_bridge import CvBridge
+from dynamic_reconfigure.server import Server
+from jsk_robot_startup.cfg import SmachNotificationReconfigureConfig
 from jsk_robot_startup.msg import Email
 from jsk_robot_startup.msg import EmailBody
 from sensor_msgs.msg import CompressedImage
@@ -37,27 +40,26 @@ def __init__(self):
         rospy.init_node('server_name')
         # it should be 'smach_to_mail', but 'server_name'
         # is the default name of smach_ros
-        self.use_mail = rospy.get_param("~use_mail", True)
-        self.use_twitter = rospy.get_param("~use_twitter", True)
-        self.use_google_chat = rospy.get_param(
-            "~use_google_chat", _enable_google_chat)
         self.pub_email = rospy.Publisher("email", Email, queue_size=10)
         self.pub_twitter = rospy.Publisher("tweet", String, queue_size=10)
+        self.reconfigure_server = Server(
+            SmachNotificationReconfigureConfig, self._reconfigure_cb)
         rospy.Subscriber(
             "~smach/container_status", SmachContainerStatus, self._status_cb)
+        rospy.Timer(rospy.Duration(
+            rospy.get_param("~stop_duration", 3600)), self._stop_timer_cb)
         self.bridge = CvBridge()
         self.smach_state_list = {}  # for status list
         self.smach_state_subject = {}  # for status subject
-        if rospy.has_param("~sender_address"):
+        self.timeout = rospy.get_param("~timeout", 1200)
+        try:
             self.sender_address = rospy.get_param("~sender_address")
-        else:
-            rospy.logerr("Please set rosparam {}/sender_address".format(
-                rospy.get_name()))
-        if rospy.has_param("~receiver_address"):
             self.receiver_address = rospy.get_param("~receiver_address")
-        else:
-            rospy.logerr("Please set rosparam {}/receiver_address".format(
-                    rospy.get_name()))
+        except KeyError as e:
+            rospy.logerr(e)
+            rospy.logerr(
+                "Please set rosparam ~sender_address or ~receiver_address")
+            sys.exit()
 
         self.chat_space = rospy.get_param("~google_chat_space", None)
         if self.use_google_chat and self.chat_space is None:
@@ -69,6 +71,43 @@ def __init__(self):
             self.gchat_image_dir = rospy.get_param("~google_chat_tmp_image_dir", "/tmp")
             self._gchat_thread = None
 
+    def _reconfigure_cb(self, config, level):
+        self.use_mail = config['use_mail']
+        self.use_twitter = config['use_twitter']
+        self.use_google_chat = config['use_google_chat']
+        self.send_every_transition = config['send_every_transition']
+        rospy.loginfo(
+            "Switched parameters; use_mail: {use_mail}, "
+            "use_twitter: {use_twitter}, "
+            "use_google_chat: {use_google_chat}, "
+            "send_every_transition: {send_every_transition}".format(**config))
+        return config
+
+    def _stop_timer_cb(self, event):
+        '''
+        If smach does not go to finish/end state,
+        this is forced to send notification.
+        '''
+        rospy.logdebug("SmachToMail stop timer called")
+        if (self.smach_state_list and self.smach_state_subject):
+            for key in self.smach_state_list.keys():
+                # Check node status and force to send notification
+                if not rosnode.rosnode_ping(
+                        key, max_count=30, verbose=False):
+                    rospy.logwarn(
+                        "SmachToMail timer publishes stop signal. Send Notification.")
+                    if self.use_mail:
+                        self._send_mail(
+                            self.smach_state_subject[key], self.smach_state_list[key])
+                    if self.use_twitter:
+                        self._send_twitter(
+                            self.smach_state_subject[key], self.smach_state_list[key])
+                    if self.use_google_chat:
+                        self._send_google_chat(
+                            self.smach_state_subject[key], self.smach_state_list[key])
+                    del self.smach_state_subject[key]
+                    del self.smach_state_list[key]
+
     def _status_cb(self, msg):
         '''
         Recording starts when smach_state becomes start.
@@ -143,6 +182,14 @@ def _status_cb(self, msg):
         else:
             self.smach_state_list[caller_id].append(status_dict)
 
+        # send notification every transition
+        if (self.send_every_transition
+                and self.use_google_chat
+                and not self.smach_state_list[caller_id] is None):
+            rospy.loginfo("Send every transition called")
+            self._send_google_chat(
+                self.smach_state_subject[caller_id], [status_dict])
+
         # If we received END/FINISH status, send email, etc...
         if status_str in ["END", "FINISH", "FINISH-SUCCESS", "FINISH-FAILURE"]:
             if (caller_id not in self.smach_state_list) or self.smach_state_list[caller_id] is None:
@@ -158,9 +205,10 @@ def _status_cb(self, msg):
                     self._send_mail(self.smach_state_subject[caller_id], self.smach_state_list[caller_id])
                 if self.use_twitter:
                     self._send_twitter(self.smach_state_subject[caller_id], self.smach_state_list[caller_id])
-                if self.use_google_chat:
+                if self.use_google_chat and not self.send_every_transition:
                     self._send_google_chat(self.smach_state_subject[caller_id], self.smach_state_list[caller_id])
                 self.smach_state_list[caller_id] = None
+                self.smach_state_subject[caller_id] = None
 
     def _send_mail(self, subject, state_list):
         email_msg = Email()
@@ -276,7 +324,7 @@ def _send_google_chat(self, subject, state_list):
         result = self.gchat_ac.get_result()
         if not self._gchat_thread:
             self._gchat_thread = result.message_result.thread_name
-        rospy.loginfo("Sending google chat messsage: {} to {} chat space".format(text, self.chat_space))
+        rospy.loginfo("Sending google chat messsage:{} chat space, {} thread".format(self.chat_space, self._gchat_thread))
         rospy.logdebug("Google Chat result: {}".format(self.gchat_ac.get_result()))
 
 if __name__ == '__main__':