classification_result_visualizer.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
4 
5 from dynamic_reconfigure.server import Server
6 import rospy
7 from geometry_msgs.msg import PoseArray
8 from jsk_topic_tools import ConnectionBasedTransport
9 from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
10 from jsk_recognition_msgs.msg import ClassificationResult
11 from jsk_recognition_msgs.msg import PeoplePoseArray
12 from jsk_rviz_plugins.cfg import ClassificationResultVisualizerConfig
13 from posedetection_msgs.msg import ObjectDetection
14 import message_filters as MF
15 from std_msgs.msg import ColorRGBA
16 from visualization_msgs.msg import Marker, MarkerArray
17 
18 
19 def is_valid_pose(pose):
20  return pose.position.x == 0.0 and\
21  pose.position.y == 0.0 and\
22  pose.position.z == 0.0
23 
24 
25 class ClassificationResultVisualizer(ConnectionBasedTransport):
26  def __init__(self):
27  super(ClassificationResultVisualizer, self).__init__()
28  self.srv = Server(ClassificationResultVisualizerConfig,
29  self.config_callback)
30  self.pub_marker = self.advertise("~output", MarkerArray, queue_size=10)
31 
32  def subscribe(self):
33  approximate_sync = rospy.get_param("~approximate_sync", False)
34  queue_size = rospy.get_param("~queue_size", 100)
35  slop = rospy.get_param("~slop", 0.1)
36 
37  sub_cls = MF.Subscriber(
38  "~input/classes", ClassificationResult, queue_size=1)
39  sub_box = MF.Subscriber(
40  "~input/boxes", BoundingBoxArray, queue_size=1)
41  sub_pose = MF.Subscriber(
42  "~input/poses", PoseArray, queue_size=1)
43  sub_people = MF.Subscriber(
44  "~input/people", PeoplePoseArray, queue_size=1)
45  sub_od = MF.Subscriber(
46  "~input/ObjectDetection", ObjectDetection, queue_size=1)
47 
48  if approximate_sync:
49  sync_box = MF.ApproximateTimeSynchronizer(
50  [sub_box, sub_cls], queue_size=queue_size, slop=slop)
51  sync_pose = MF.ApproximateTimeSynchronizer(
52  [sub_pose, sub_cls], queue_size=queue_size, slop=slop)
53  sync_people = MF.ApproximateTimeSynchronizer(
54  [sub_people, sub_cls], queue_size=queue_size, slop=slop)
55  sync_od = MF.ApproximateTimeSynchronizer(
56  [sub_od, sub_cls], queue_size=queue_size, slop=slop)
57  else:
58  sync_box = MF.TimeSynchronizer(
59  [sub_box, sub_cls], queue_size=queue_size)
60  sync_pose = MF.TimeSynchronizer(
61  [sub_pose, sub_cls], queue_size=queue_size)
62  sync_people = MF.TimeSynchronizer(
63  [sub_people, sub_cls], queue_size=queue_size)
64  sync_od = MF.TimeSynchronizer(
65  [sub_od, sub_cls], queue_size=queue_size)
66 
67  sync_box.registerCallback(self.box_msg_callback)
68  sync_pose.registerCallback(self.pose_msg_callback)
69  sync_people.registerCallback(self.people_msg_callback)
70  sync_od.registerCallback(self.od_msg_callback)
71 
72  self.subscribers = [sub_cls, sub_box, sub_pose, sub_people, sub_od]
73 
74  def unsubscribe(self):
75  for sub in self.subscribers:
76  sub.unregister()
77 
78  def config_callback(self, config, level):
79  self.text_color = {'r': config.text_color_red,
80  'g': config.text_color_green,
81  'b': config.text_color_blue,
82  'a': config.text_color_alpha}
83  self.text_offset = [config.text_offset_x,
84  config.text_offset_y,
85  config.text_offset_z]
86  self.text_size = config.text_size
87  self.marker_lifetime = config.marker_lifetime
88  self.show_proba = config.show_proba
89  return config
90 
91  def pose_msg_callback(self, pose, classes):
92  bboxes = BoundingBoxArray(header=pose.header)
93  for p in pose.poses:
94  b = BoundingBox(header=pose.header)
95  b.pose = p
96  bboxes.boxes.append(b)
97  self.box_msg_callback(bboxes, classes)
98 
99  def people_msg_callback(self, people, classes):
100  bboxes = BoundingBoxArray(header=people.header)
101  for p in people.poses:
102  b = BoundingBox()
103  for i, n in enumerate(p.limb_names):
104  if n in ["Neck", "Nose", "REye", "LEye", "REar", "LEar"]:
105  b.header = people.header
106  b.pose = p.poses[i]
107  break
108  if not b.header.frame_id:
109  b.header = people.header
110  b.pose = b.poses[0]
111  self.box_msg_callback(bboxes, classes)
112 
113  def od_msg_callback(self, od, classes):
114  bboxes = BoundingBoxArray(header=od.header)
115  for obj in od.objects:
116  b = BoundingBox()
117  b.pose = obj.pose
118  self.box_msg_callback(bboxes, classes)
119 
120  def box_msg_callback(self, bboxes, classes):
121  msg = MarkerArray()
122  show_proba = self.show_proba and len(classes.label_names) == len(classes.label_proba)
123  if show_proba:
124  cls_iter = list(zip(classes.label_names, classes.label_proba))
125  else:
126  cls_iter = classes.label_names
127  for i, data in enumerate(zip(bboxes.boxes, cls_iter)):
128  bbox, cls = data
129  if show_proba:
130  text = "%s (%.3f)" % cls
131  else:
132  text = cls
133 
134  if is_valid_pose(bbox.pose):
135  continue
136 
137  m = Marker(type=Marker.TEXT_VIEW_FACING,
138  action=Marker.MODIFY,
139  header=bbox.header,
140  id=i,
141  pose=bbox.pose,
142  color=ColorRGBA(**self.text_color),
143  text=text,
144  ns=classes.classifier,
145  lifetime=rospy.Duration(self.marker_lifetime))
146  m.scale.z = self.text_size
147  m.pose.position.x += self.text_offset[0]
148  m.pose.position.y += self.text_offset[1]
149  m.pose.position.z += self.text_offset[2]
150  msg.markers.append(m)
151 
152  self.pub_marker.publish(msg)
153 
154 
155 if __name__ == '__main__':
156  rospy.init_node("classification_result_visualizer")
158  rospy.spin()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58