#!/usr/bin/env python

import rospy
from jsk_apc2016_common.msg import BinInfoArray

import tf2_ros
from geometry_msgs.msg import TransformStamped


class PublishTF(object):
    def __init__(self):
        bin_info_array = rospy.wait_for_message(
                "~input/bin_info_array", BinInfoArray, timeout=50)

        bbox_dict = self.bin_info_array_to_bbox_dict(bin_info_array)

        # publish bbox tf
        self.publish_bbox_tf(bbox_dict)

    def publish_bbox_tf(self, bbox_dict):
        # publish bounding boxes' center as static tf
        broadcaster = tf2_ros.StaticTransformBroadcaster()
        for bin_, bbox in bbox_dict.iteritems():
            bin_tf = TransformStamped()

            bin_tf.header.stamp = rospy.Time.now()
            bin_tf.header.frame_id = bbox.header.frame_id
            bin_tf.child_frame_id = 'bin_' + bin_

            bin_tf.transform.translation.x = bbox.pose.position.x
            bin_tf.transform.translation.y = bbox.pose.position.y
            bin_tf.transform.translation.z = bbox.pose.position.z
            bin_tf.transform.rotation.x = bbox.pose.orientation.x
            bin_tf.transform.rotation.y = bbox.pose.orientation.y
            bin_tf.transform.rotation.z = bbox.pose.orientation.z
            bin_tf.transform.rotation.w = bbox.pose.orientation.w

            broadcaster.sendTransform(bin_tf)

    def bin_info_array_to_bbox_dict(self, bin_info_array):
        bbox_dict = {}
        for bin_ in bin_info_array.array:
            bbox_dict[bin_.name] = bin_.bbox
        return bbox_dict

if __name__ == '__main__':
    rospy.init_node('sib_publish_tf')
    pub_tf = PublishTF()
    rospy.spin()

@@ -89,62 +89,17 @@
 ort 
-tf2_ros%0Afrom geometry_msgs.msg import TransformStamped
+roslaunch
 %0A%0A%0Ac
@@ -366,16 +366,32 @@
 %0A       
+ self.pub_dict =
  self.pu
@@ -524,58 +524,21 @@
     
-broadcaster = tf2_ros.StaticTransformBroadcaster()
+tf_nodes = %7B%7D
 %0A   
@@ -599,246 +599,241 @@
     
-bin_tf = TransformStamped()%0A%0A            bin_tf.header.stamp = rospy.Time.now()%0A            bin_tf.header.frame_id = bbox.header.frame_id%0A            bin_tf.child_frame_id = 'bin_' + bin_%0A%0A            bin_tf.transform.translation.x = bbox
+# something like 1 0 2 0 0 0 1%0A            transform_string = (%0A                    str(bbox_dict%5Bbin_%5D.pose.position.x) + ' ' +%0A                    str(bbox_dict%5Bbin_%5D.pose.position.y) + ' ' +%0A                    str(bbox_dict%5Bbin_%5D
 .pos
@@ -839,25 +839,34 @@
 se.position.
-x
+z) + ' ' +
 %0A           
@@ -870,438 +870,884 @@
     
-bin_tf.transform.translation.y = bbox.pose.position.y%0A            bin_tf.transform.translation.z = bbox.pose.position.z%0A            bin_tf.transform.rotation.x = bbox.pose.orientation.x%0A            bin_tf.transform.rotation.y = bbox.pose.orientation.y%0A            bin_tf.transform.rotation.z = bbox.pose.orientation.z%0A            bin_tf.transform.rotation.w = bbox.pose.orientation.w%0A%0A            broadcaster.sendTransform(bin_tf)
+        str(bbox_dict%5Bbin_%5D.pose.orientation.x) + ' ' +%0A                    str(bbox_dict%5Bbin_%5D.pose.orientation.y) + ' ' +%0A                    str(bbox_dict%5Bbin_%5D.pose.orientation.z) + ' ' +%0A                    str(bbox_dict%5Bbin_%5D.pose.orientation.w) + ' ')%0A%0A            tf_arg_string = (%0A                    transform_string +%0A                    str(bbox_dict%5Bbin_%5D.header.frame_id) + ' ' +%0A                    'bin_' + bin_ + ' ' + '100')  # last number is period%0A%0A            tf_nodes%5Bbin_%5D = roslaunch.core.Node(%0A                    package=%22tf%22,%0A                    name=%22pub_bin_%22 + bin_,%0A                    node_type=%22static_transform_publisher%22,%0A                    args=tf_arg_string,%0A                    respawn=True)%0A            launch = roslaunch.scriptapi.ROSLaunch()%0A            launch.start()%0A            launch.launch(tf_nodes%5Bbin_%5D)%0A        return tf_nodes
 %0A%0A  

Revert "[jsk_apc2016_common] publish_bin_tf now uses tf2_ros static_tf_publisher"