From 41a93e46735694a6fc8ff4801dcfdbe26bdd2ab9 Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Fri, 3 Jul 2015 13:21:45 +0900
Subject: [PATCH 1/2] [jsk_tools] import sanity_lib in __init__.py

---
 jsk_tools/src/jsk_tools/__init__.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/jsk_tools/src/jsk_tools/__init__.py b/jsk_tools/src/jsk_tools/__init__.py
index e3a12e370..9310ab549 100644
--- a/jsk_tools/src/jsk_tools/__init__.py
+++ b/jsk_tools/src/jsk_tools/__init__.py
@@ -1,4 +1,5 @@
 import shellblock_directive
 import video_directive
+import sanity_lib
 
 

From 6691ecdc0fa723cec3269cb2ee83148a895d98e8 Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Fri, 3 Jul 2015 13:22:26 +0900
Subject: [PATCH 2/2] [jsk_tools] Check msg type is same as published one

---
 jsk_tools/src/jsk_tools/sanity_lib.py | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/jsk_tools/src/jsk_tools/sanity_lib.py b/jsk_tools/src/jsk_tools/sanity_lib.py
index 818224e38..358bec992 100644
--- a/jsk_tools/src/jsk_tools/sanity_lib.py
+++ b/jsk_tools/src/jsk_tools/sanity_lib.py
@@ -49,7 +49,7 @@ def colored(string, color):
 class TopicPublishedChecker():
     is_topic_published = False
     is_topic_published_lock = Lock()
-    def __init__(self, topic_name,  timeout = 5, echo = False):
+    def __init__(self, topic_name, timeout=5, echo=False, data_class=None):
         self.topic_name = topic_name
         self.timeout = timeout
         self.launched_time = rospy.Time.now()
@@ -57,6 +57,8 @@ def __init__(self, topic_name,  timeout = 5, echo = False):
         self.echo = echo
         print " Checking %s" % (topic_name)
         msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
+        if (data_class is not None) and (msg_class is not data_class):
+            raise rospy.ROSException('Topic msg type is different.')
         self.sub = rospy.Subscriber(topic_name, msg_class, self.callback)
     def callback(self, msg):
         with self.is_topic_published_lock:
@@ -85,17 +87,25 @@ def check(self):
         finally:
             self.sub.unregister()
 
-def checkTopicIsPublished(topic_name, class_name,
+def checkTopicIsPublished(topic_name, class_name = None,
                           ok_message = "",
                           error_message = "",
                           timeout = 1,
                           other_topics = [],
                           echo = False):
+    """
+    @type class_name: type
+    @property class_name:
+        ROS message data class.
+        if not None, it checks if msg type is same as published one.
+    """
     checkers = []
-    checkers.append(TopicPublishedChecker(topic_name, timeout, echo))
+    checkers.append(TopicPublishedChecker(topic_name, timeout, echo,
+                                          data_class=class_name))
     if other_topics:
         for (tpc_name, cls) in other_topics:
-            checkers.append(TopicPublishedChecker(tpc_name, timeout, echo))
+            checkers.append(TopicPublishedChecker(tpc_name, timeout, echo,
+                                                  data_class=class_name))
     all_success = True
     for checker in checkers:
         if not checker.check():