classification_result_visualizer.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 from dynamic_reconfigure.server import Server
00006 import rospy
00007 from geometry_msgs.msg import PoseArray
00008 from jsk_topic_tools import ConnectionBasedTransport
00009 from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
00010 from jsk_recognition_msgs.msg import ClassificationResult
00011 from jsk_recognition_msgs.msg import PeoplePoseArray
00012 from jsk_rviz_plugins.cfg import ClassificationResultVisualizerConfig
00013 from posedetection_msgs.msg import ObjectDetection
00014 import message_filters as MF
00015 from std_msgs.msg import ColorRGBA
00016 from visualization_msgs.msg import Marker, MarkerArray
00017 
00018 
00019 def is_valid_pose(pose):
00020     return pose.position.x == 0.0 and\
00021            pose.position.y == 0.0 and\
00022            pose.position.z == 0.0
00023 
00024 
00025 class ClassificationResultVisualizer(ConnectionBasedTransport):
00026     def __init__(self):
00027         super(ClassificationResultVisualizer, self).__init__()
00028         self.srv = Server(ClassificationResultVisualizerConfig,
00029                           self.config_callback)
00030         self.pub_marker = self.advertise("~output", MarkerArray, queue_size=10)
00031 
00032     def subscribe(self):
00033         approximate_sync = rospy.get_param("~approximate_sync", False)
00034         queue_size = rospy.get_param("~queue_size", 100)
00035         slop = rospy.get_param("~slop", 0.1)
00036 
00037         sub_cls = MF.Subscriber(
00038             "~input/classes", ClassificationResult, queue_size=1)
00039         sub_box = MF.Subscriber(
00040             "~input/boxes", BoundingBoxArray, queue_size=1)
00041         sub_pose = MF.Subscriber(
00042             "~input/poses", PoseArray, queue_size=1)
00043         sub_people = MF.Subscriber(
00044             "~input/people", PeoplePoseArray, queue_size=1)
00045         sub_od = MF.Subscriber(
00046             "~input/ObjectDetection", ObjectDetection, queue_size=1)
00047 
00048         if approximate_sync:
00049             sync_box = MF.ApproximateTimeSynchronizer(
00050                 [sub_box, sub_cls], queue_size=queue_size, slop=slop)
00051             sync_pose = MF.ApproximateTimeSynchronizer(
00052                 [sub_pose, sub_cls], queue_size=queue_size, slop=slop)
00053             sync_people = MF.ApproximateTimeSynchronizer(
00054                 [sub_people, sub_cls], queue_size=queue_size, slop=slop)
00055             sync_od = MF.ApproximateTimeSynchronizer(
00056                 [sub_od, sub_cls], queue_size=queue_size, slop=slop)
00057         else:
00058             sync_box = MF.TimeSynchronizer(
00059                 [sub_box, sub_cls], queue_size=queue_size)
00060             sync_pose = MF.TimeSynchronizer(
00061                 [sub_pose, sub_cls], queue_size=queue_size)
00062             sync_people = MF.TimeSynchronizer(
00063                 [sub_people, sub_cls], queue_size=queue_size)
00064             sync_od = MF.TimeSynchronizer(
00065                 [sub_od, sub_cls], queue_size=queue_size)
00066 
00067         sync_box.registerCallback(self.box_msg_callback)
00068         sync_pose.registerCallback(self.pose_msg_callback)
00069         sync_people.registerCallback(self.people_msg_callback)
00070         sync_od.registerCallback(self.od_msg_callback)
00071 
00072         self.subscribers = [sub_cls, sub_box, sub_pose, sub_people, sub_od]
00073 
00074     def unsubscribe(self):
00075         for sub in self.subscribers:
00076             sub.unregister()
00077 
00078     def config_callback(self, config, level):
00079         self.text_color = {'r': config.text_color_red,
00080                            'g': config.text_color_green,
00081                            'b': config.text_color_blue,
00082                            'a': config.text_color_alpha}
00083         self.text_offset = [config.text_offset_x,
00084                             config.text_offset_y,
00085                             config.text_offset_z]
00086         self.text_size = config.text_size
00087         self.marker_lifetime = config.marker_lifetime
00088         self.show_proba = config.show_proba
00089         return config
00090 
00091     def pose_msg_callback(self, pose, classes):
00092         bboxes = BoundingBoxArray(header=pose.header)
00093         for p in pose.poses:
00094             b = BoundingBox(header=pose.header)
00095             b.pose = p
00096             bboxes.boxes.append(b)
00097         self.box_msg_callback(bboxes, classes)
00098 
00099     def people_msg_callback(self, people, classes):
00100         bboxes = BoundingBoxArray(header=people.header)
00101         for p in people.poses:
00102             b = BoundingBox()
00103             for i, n in enumerate(p.limb_names):
00104                 if n in ["Neck", "Nose", "REye", "LEye", "REar", "LEar"]:
00105                     b.header = people.header
00106                     b.pose = p.poses[i]
00107                     break
00108             if not b.header.frame_id:
00109                 b.header = people.header
00110                 b.pose = b.poses[0]
00111         self.box_msg_callback(bboxes, classes)
00112 
00113     def od_msg_callback(self, od, classes):
00114         bboxes = BoundingBoxArray(header=od.header)
00115         for obj in od.objects:
00116             b = BoundingBox()
00117             b.pose = obj.pose
00118         self.box_msg_callback(bboxes, classes)
00119 
00120     def box_msg_callback(self, bboxes, classes):
00121         msg = MarkerArray()
00122         show_proba = self.show_proba and len(classes.label_names) == len(classes.label_proba)
00123         if show_proba:
00124             cls_iter = zip(classes.label_names, classes.label_proba)
00125         else:
00126             cls_iter = classes.label_names
00127         for i, data in enumerate(zip(bboxes.boxes, cls_iter)):
00128             bbox, cls = data
00129             if show_proba:
00130                 text = "%s (%.3f)" % cls
00131             else:
00132                 text = cls
00133 
00134             if is_valid_pose(bbox.pose):
00135                 continue
00136 
00137             m = Marker(type=Marker.TEXT_VIEW_FACING,
00138                        action=Marker.MODIFY,
00139                        header=bbox.header,
00140                        id=i,
00141                        pose=bbox.pose,
00142                        color=ColorRGBA(**self.text_color),
00143                        text=text,
00144                        ns=classes.classifier,
00145                        lifetime=rospy.Duration(self.marker_lifetime))
00146             m.scale.z = self.text_size
00147             m.pose.position.x += self.text_offset[0]
00148             m.pose.position.y += self.text_offset[1]
00149             m.pose.position.z += self.text_offset[2]
00150             msg.markers.append(m)
00151 
00152         self.pub_marker.publish(msg)
00153 
00154 
00155 if __name__ == '__main__':
00156     rospy.init_node("classification_result_visualizer")
00157     viz = ClassificationResultVisualizer()
00158     rospy.spin()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Wed May 1 2019 02:40:22