marker_array = MarkerArray()
marker_array.markers.append(marker)
for l, instance in clusters.objects_map.iteritems():
cluster_name = "Cluster_{}_{}".format(l, instance.heavier_label)
frame = instance.getBoxRF()
#frame.M = PyKDL.Rotation()
box = instance.getBBox()
marker_array.markers.append(createBoxMarker(
"Box_{}".format(l),
box,
cluster_name
))
node.broadcastTransform(
frame, cluster_name, "world", node.getCurrentTime())
print "INstance"
print instance.toString()
for m in marker_array.markers:
m.header.stamp = node.getCurrentTime()
vis_topic.publish(marker_array)
node.tick()
else:
try:
os.mkdir(os.path.join(output_path, output_name))
except:
pass
instance_list = []
counter = 0
box = tools.VBox(boxes_opt_data[i])
boxes_opt.append(box)
while node.isActive():
mar = MarkerArray()
for i in range(0, len(instances)):
name = 'instance_{}'.format(i)
volume = tools.createMarkerVolume(name, instances[i].points)
mar.markers.append(volume)
for i in range(0, len(boxes)):
name = 'box_{}'.format(i)
node.broadcastTransform(boxes[i].frame, name, 'world',
node.getCurrentTime())
mar.markers.append(
tools.createBoxMarker(name, boxes[i], name, color=(1.0, 0.0, 0.0)))
for i in range(0, len(boxes_opt)):
name = 'box_opt_{}'.format(i)
node.broadcastTransform(boxes_opt[i].frame, name, 'world',
node.getCurrentTime())
mar.markers.append(
tools.createBoxMarker(name,
boxes_opt[i],
name,
color=(0.0, 1.0, 0.0)))
vis_topic.publish(mar)
node.tick()
instance_original_frame = instance.getRF()
instance_original_box = createBoxMarker(
"instance_original_box", instance.getBBox(), "instance")
instance.optimizeSVD()
instance_new_frame = instance.getRF()
instance_new_box = createBoxMarker(
"instance_new_box", instance.getBBox(), "instance_opt", color=(0.0, 1.0, 0.0))
while node.isActive():
node.broadcastTransform(instance_original_frame,
"instance", "world", node.getCurrentTime())
node.broadcastTransform(instance_new_frame,
"instance_opt", "world", node.getCurrentTime())
print "Box area:", instance.getBoxArea()
marker_array = MarkerArray()
marker_array.markers.append(volume_marker)
marker_array.markers.append(instance_original_box)
marker_array.markers.append(instance_new_box)
for m in marker_array.markers:
m.header.stamp = node.getCurrentTime()
vis_topic.publish(marker_array)
node.tick()