00001
00002
00003
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()