[develop/pr2-noetic][jsk_robot_startup] Add commits related to smach_to_mail.py in develop/fetch but not in develop/pr2-noetic

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,20 +40,17 @@ 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
-        self.smach_start_time = {}
         self.timeout = rospy.get_param("~timeout", 1200)
         if rospy.has_param("~sender_address"):
             self.sender_address = rospy.get_param("~sender_address")
@@ -73,26 +73,42 @@ 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: {send_every_transition}, "
+            "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.
         '''
-        now = rospy.Time.now()
         rospy.logdebug("SmachToMail stop timer called")
-        if (self.smach_state_list and
-                self.smach_state_subject and
-                self.timeout is not None and
-                self.smach_start_time is not None):
+        if (self.smach_state_list and self.smach_state_subject):
             for key in self.smach_state_list.keys():
-                if (now - self.smach_start_time[key]).to_sec() > self.timeout:
-                    self._send_mail(
-                        self.smach_state_subject[key], self.smach_state_list[key])
-                    self._send_twitter(
-                        self.smach_state_subject[key], self.smach_state_list[key])
-                    self.smach_state_subject[key] = None
+                # 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):
         '''
@@ -134,7 +150,6 @@ def _status_cb(self, msg):
 
         # If we received START/INIT status, restart storing smach_state_list
         if status_str in ["START", "INIT"]:
-            self.smach_start_time[caller_id] = rospy.Time.now()
             self.smach_state_list[caller_id] = []
             # DESCRIPTION of START is MAIL SUBJECT
             if 'DESCRIPTION' in local_data_str:
@@ -169,6 +184,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:
@@ -184,9 +207,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()
@@ -201,7 +225,10 @@ def _send_mail(self, subject, state_list):
             if 'DESCRIPTION' in x:
                 description = EmailBody()
                 description.type = 'text'
-                description.message = x['DESCRIPTION']
+                description_txt = x['DESCRIPTION']
+                if isinstance(description_txt, bytes):
+                    description_txt = description_txt.decode('utf-8')
+                description.message = description_txt
                 email_msg.body.append(description)
                 email_msg.body.append(changeline)
             if 'IMAGE' in x and x['IMAGE']:
@@ -222,7 +249,10 @@ def _send_mail(self, subject, state_list):
             if 'INFO' in x:
                 info = EmailBody()
                 info.type = 'text'
-                info.message = x['INFO']
+                info_txt = x['INFO']
+                if isinstance(info_txt, bytes):
+                    info_txt = info_txt.decode('utf-8')
+                info.message = info_txt
                 email_msg.body.append(info)
                 email_msg.body.append(changeline)
         # rospy.loginfo("body:{}".format(email_msg.body))
@@ -296,7 +326,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__':