def draw(self): origin = [self.cx, self.cy, self.cz] quat = conversions.yaw_to_quat(self.theta) pose = conversions.trans_rot_to_pose(origin, quat) ps = gm.PoseStamped() ps.pose = pose ps.header.frame_id = "/base_link" ps.header.stamp = rospy.Time(0) print "(%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f), (%.3f, %.3f, %.3f)"%( self.cx, self.cy, self.cz, quat[0], quat[1], quat[2], quat[3], self.sx/2., self.sy/2., self.sz/2.) #rviz.draw_marker(ps, 0, type=Marker.CUBE, rgba=(0,1,0,.25), scale=(self.sx,self.sy,self.sz)) self.marker_handle = rviz.place_kin_tree_from_link(ps, "base_link") view = View( VGroup( HGroup(Item("cx"), Item("cy"), Item("cz")), HGroup(Item("sx"), Item("sy"), Item("sz")), Item("theta") ), buttons = OKCancelButtons, width = 500, resizable = True) if __name__ == "__main__": rospy.init_node("draw_box",disable_signals = True) rviz = RvizWrapper() box = Box(rviz) box.configure_traits()