5 from dynamic_reconfigure.server
import Server
7 from geometry_msgs.msg
import PoseArray
8 from jsk_topic_tools
import ConnectionBasedTransport
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
20 return pose.position.x == 0.0
and\
21 pose.position.y == 0.0
and\
22 pose.position.z == 0.0
27 super(ClassificationResultVisualizer, self).
__init__()
28 self.
srv = Server(ClassificationResultVisualizerConfig,
30 self.
pub_marker = self.advertise(
"~output", MarkerArray, queue_size=10)
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)
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)
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)
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)
72 self.
subscribers = [sub_cls, sub_box, sub_pose, sub_people, sub_od]
80 'g': config.text_color_green,
81 'b': config.text_color_blue,
82 'a': config.text_color_alpha}
92 bboxes = BoundingBoxArray(header=pose.header)
94 b = BoundingBox(header=pose.header)
96 bboxes.boxes.append(b)
100 bboxes = BoundingBoxArray(header=people.header)
101 for p
in people.poses:
103 for i, n
in enumerate(p.limb_names):
104 if n
in [
"Neck",
"Nose",
"REye",
"LEye",
"REar",
"LEar"]:
105 b.header = people.header
108 if not b.header.frame_id:
109 b.header = people.header
114 bboxes = BoundingBoxArray(header=od.header)
115 for obj
in od.objects:
122 show_proba = self.
show_proba and len(classes.label_names) == len(classes.label_proba)
124 cls_iter = zip(classes.label_names, classes.label_proba)
126 cls_iter = classes.label_names
127 for i, data
in enumerate(zip(bboxes.boxes, cls_iter)):
130 text =
"%s (%.3f)" % cls
137 m = Marker(type=Marker.TEXT_VIEW_FACING,
138 action=Marker.MODIFY,
144 ns=classes.classifier,
150 msg.markers.append(m)
152 self.pub_marker.publish(msg)
155 if __name__ ==
'__main__':
156 rospy.init_node(
"classification_result_visualizer")
def od_msg_callback(self, od, classes)
def pose_msg_callback(self, pose, classes)
def config_callback(self, config, level)
def box_msg_callback(self, bboxes, classes)
def people_msg_callback(self, people, classes)