def to_msg(self, boxes, class_ids, header):
        box_arr = BoundingBoxArray()
        box_arr.header = header

        for i in range(len(boxes)):
            if str(self.classes[class_ids[i]]) == "person":
                x, y, w, h = boxes[i]
                box = BoundingBox()
                box.label = i
                box.value = 0

                box.pose.position.x = x
                box.pose.position.y = y
                box.pose.position.z = 0

                box.pose.orientation.x = 0
                box.pose.orientation.y = 0
                box.pose.orientation.x = 0
                box.pose.orientation.w = 0

                box.dimensions.x = w
                box.dimensions.y = h
                box.dimensions.z = 0

                box_arr.boxes.append(box)

        return box_arr
Esempio n. 2
0
    def init_boundingboxarray(self, num_boxes=30):
        self.boundingBoxArray_object = BoundingBoxArray()

        h = std_msgs.msg.Header()
        h.stamp = rospy.Time.now(
        )  # Note you need to call rospy.init_node() before this will work
        h.frame_id = "world"

        self.boundingBoxArray_object.header = h

        self.minimum_dimension = 0.2
        self.init_x_position = 1.0

        for i in range(num_boxes):
            new_box = BoundingBox()
            new_box.header = h

            new_box.pose = Pose()
            new_box.pose.position.x = self.init_x_position + i * self.minimum_dimension

            new_box.dimensions = Vector3()
            new_box.dimensions.x = self.minimum_dimension
            new_box.dimensions.y = self.minimum_dimension
            new_box.dimensions.z = self.minimum_dimension

            new_box.label = i
            new_box.value = i * self.minimum_dimension

            self.boundingBoxArray_object.boxes.append(new_box)

        self.publish_once(self.boundingBoxArray_object)
    def create_bb_msg(self, cur_bb, is_map_coord=False):
        bb_corners = np.array([cur_bb['upper_left_px'], cur_bb['lower_right_px']])
        if is_map_coord is True:
            bb_corners_map_coord = bb_corners
        else:
            bb_corners_px_coord = bb_corners
            bb_corners_map_coord = self.img2map.pixel2map(bb_corners_px_coord)
        bb_corners_mean = np.mean(bb_corners_map_coord, axis=0)
        bb_corners_diff = np.abs(bb_corners_map_coord[0, :] - bb_corners_map_coord[1, :])

        cur_bb_msg = BoundingBox()
        cur_bb_msg.header.frame_id = 'map'
        cur_bb_msg.header.stamp = rospy.Time.now()
        cur_bb_msg.pose.position.x = bb_corners_mean[0]
        cur_bb_msg.pose.position.y = bb_corners_mean[1]
        cur_bb_msg.pose.position.z = 0
        cur_bb_msg.pose.orientation.x = 0
        cur_bb_msg.pose.orientation.y = 0
        cur_bb_msg.pose.orientation.z = 0
        cur_bb_msg.pose.orientation.w = 1
        cur_bb_msg.dimensions.x = bb_corners_diff[0]
        cur_bb_msg.dimensions.y = bb_corners_diff[1]
        cur_bb_msg.dimensions.z = 0.1
        cur_bb_msg.value = 1.0
        cur_bb_msg.label = 1
        return cur_bb_msg
Esempio n. 4
0
def rslidar_callback(msg):
    t_t = time.time()

    #calib = getCalibfromFile(calib_file)
    #calib = getCalibfromROS(calibmsg)

    frame = msg.header.seq

    arr_bbox = BoundingBoxArray()

    msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
    np_p = get_xyz_points(msg_cloud, True)
    print("  ")
    #scores, dt_box_lidar, types = proc_1.run(np_p)
    scores, dt_box_lidar, types, pred_dict = proc_1.run(np_p, calib, frame)

    annos_sorted = sortbydistance(dt_box_lidar, scores, types)
    #pp_AB3DMOT_list  = anno_to_AB3DMOT(pred_dict, msg)
    pp_AB3DMOT_list = anno_to_AB3DMOT(dt_box_lidar, scores, types, msg)
    pp_list = anno_to_sort(dt_box_lidar, scores, types)
    pp_3D_list = anno_to_3Dsort(annos_sorted, types)
    MarkerArray_list = anno_to_rviz(dt_box_lidar, scores, types, msg)

    if scores.size != 0:
        for i in range(scores.size):
            if scores[i] > threshold:
                bbox = BoundingBox()
                bbox.header.frame_id = msg.header.frame_id
                bbox.header.stamp = rospy.Time.now()
                q = yaw2quaternion(float(dt_box_lidar[i][6]))
                bbox.pose.orientation.x = q[1]
                bbox.pose.orientation.y = q[2]
                bbox.pose.orientation.z = q[3]
                bbox.pose.orientation.w = q[0]
                bbox.pose.position.x = float(
                    dt_box_lidar[i][0]) - movelidarcenter
                bbox.pose.position.y = float(dt_box_lidar[i][1])
                bbox.pose.position.z = float(dt_box_lidar[i][2])
                bbox.dimensions.x = float(dt_box_lidar[i][3])
                bbox.dimensions.y = float(dt_box_lidar[i][4])
                bbox.dimensions.z = float(dt_box_lidar[i][5])
                bbox.value = scores[i]
                bbox.label = int(types[i])
                arr_bbox.boxes.append(bbox)

    print("total callback time: ", time.time() - t_t)
    arr_bbox.header.frame_id = msg.header.frame_id
    arr_bbox.header.stamp = msg.header.stamp
    if len(arr_bbox.boxes) is not 0:
        pub_arr_bbox.publish(arr_bbox)
        arr_bbox.boxes = []
    else:
        arr_bbox.boxes = []
        pub_arr_bbox.publish(arr_bbox)

    pubRviz.publish(MarkerArray_list)
    pubSort.publish(pp_list)
    pub3DSort.publish(pp_3D_list)
    pubAB3DMOT.publish(pp_AB3DMOT_list)
Esempio n. 5
0
def rslidar_callback(msg):
    t_t = time.time()

    arr_bbox = BoundingBoxArray()

    #t = time.time()
    msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
    np_p = get_xyz_points(msg_cloud, True)
    print("  ")
    #print("prepare cloud time: ", time.time() - t)

    #t = time.time()
    scores, dt_box_lidar, types = proc_1.run(np_p)
    #print("network forward time: ", time.time() - t)
    # filter_points_sum = []
    #t = time.time()
    if scores.size != 0:
        for i in range(scores.size):
            bbox = BoundingBox()
            bbox.header.frame_id = msg.header.frame_id
            bbox.header.stamp = rospy.Time.now()

            q = yaw2quaternion(float(dt_box_lidar[i][6]))
            bbox.pose.orientation.x = q[0]
            bbox.pose.orientation.y = q[1]
            bbox.pose.orientation.z = q[2]
            bbox.pose.orientation.w = q[3]

            bbox.pose.position.x = float(dt_box_lidar[i][0])
            bbox.pose.position.y = float(dt_box_lidar[i][1])
            bbox.pose.position.z = float(dt_box_lidar[i][2])
            bbox.dimensions.x = float(dt_box_lidar[i][3])
            bbox.dimensions.y = float(dt_box_lidar[i][4])
            bbox.dimensions.z = float(dt_box_lidar[i][5])
            bbox.value = scores[i]
            bbox.label = types[i]
            arr_bbox.boxes.append(bbox)

            # filter_points = np_p[point_indices[:,i]]
            # filter_points_sum.append(filter_points)
    #print("publish time cost: ", time.time() - t)
    # filter_points_sum = np.concatenate(filter_points_sum, axis=0)
    # filter_points_sum = filter_points_sum[:, :3]

    # print("output of concatenate:", filter_points_sum)
    # filter_points_sum = np.arange(24).reshape(8,3)
    # cluster_cloud = xyz_array_to_pointcloud2(filter_points_sum, stamp=rospy.Time.now(), frame_id=msg.header.frame_id)
    # pub_segments.publish(cluster_cloud)

    print("total callback time: ", time.time() - t_t)
    arr_bbox.header.frame_id = msg.header.frame_id
    arr_bbox.header.stamp = rospy.Time.now()
    if len(arr_bbox.boxes) is not 0:
        pub_arr_bbox.publish(arr_bbox)
        arr_bbox.boxes = []
    else:
        arr_bbox.boxes = []
        pub_arr_bbox.publish(arr_bbox)
    def callback(self, box_msg):
        labeled_boxes = {}
        label_buf = []
        orientation = Pose().orientation

        for box in box_msg.boxes:
            if box.label in label_buf:
                labeled_boxes[box.label] += [self.get_points(box)]
                orientation = box.pose.orientation
            else:
                labeled_boxes[box.label] = [self.get_points(box)]
                label_buf.append(box.label)

        bounding_box_msg = BoundingBoxArray()

        for label, boxes in zip(labeled_boxes.keys(), labeled_boxes.values()):
            thresh = self.thresh
            if self.label_lst[label] == 'shelf_flont':
                thresh = 2.0

            clustering = Clustering()
            boxes = np.array(boxes)
            result = clustering.clustering_wrapper(boxes, thresh)

            for cluster in result:
                max_candidates = [
                    boxes[i][0] + (boxes[i][1] * 0.5) for i in cluster.indices
                ]
                min_candidates = [
                    boxes[i][0] - (boxes[i][1] * 0.5) for i in cluster.indices
                ]
                candidates = np.array(max_candidates + min_candidates)

                dimension = candidates.max(axis=0) - candidates.min(axis=0)
                center = candidates.min(axis=0) + (dimension * 0.5)

                distances = self.get_distances(
                    np.array([boxes[i][0] for i in cluster.indices]), [
                        np.linalg.norm(boxes[i][1]) * 0.5
                        for i in cluster.indices
                    ])
                tmp_box = BoundingBox()
                tmp_box.header = box_msg.header
                tmp_box.dimensions.x = dimension[0]
                tmp_box.dimensions.y = dimension[1]
                tmp_box.dimensions.z = dimension[2]
                tmp_box.pose.position.x = center[0]
                tmp_box.pose.position.y = center[1]
                tmp_box.pose.position.z = center[2]
                tmp_box.pose.orientation = orientation
                tmp_box.label = label
                tmp_box.value = distances.mean()
                bounding_box_msg.boxes.append(tmp_box)

            bounding_box_msg.header = box_msg.header
        self.box_pub.publish(bounding_box_msg)
def convert(header, xx_cube):
    boundingboxarray_msg = BoundingBoxArray()
    boundingboxarray_msg.header = header

    num_xx = int(xx_cube.shape[0] / 8)
    for i in range(num_xx):
        cube_raw = xx_cube[range(8 * i, 8 * i + 8), :]
        bb_in_camera = np.column_stack((cube_raw, np.ones((8, 1))))
        bb_in_lidar = np.linalg.inv(calib.lidar_to_cam).dot(bb_in_camera.T).T
        boundingbox_msg = BoundingBox()
        boundingbox_msg.header = boundingboxarray_msg.header

        # boundingbox中心点位置
        boundingbox_msg.pose.position.x = bb_in_lidar[:, 0].mean()
        boundingbox_msg.pose.position.y = bb_in_lidar[:, 1].mean()
        boundingbox_msg.pose.position.z = bb_in_lidar[:, 2].mean()

        # 寻找y坐标最小的顶点,计算相邻两个顶点的旋转角及边长
        bb_bottom = bb_in_lidar[:4]
        min_idx = np.where(bb_bottom[:, 1] == bb_bottom[:, 1].min())[0][0]
        theta = math.atan2(
            bb_bottom[(min_idx + 1) % 4, 1] - bb_bottom[min_idx, 1],
            bb_bottom[(min_idx + 1) % 4, 0] - bb_bottom[min_idx, 0])
        b_1 = (
            (bb_bottom[(min_idx + 1) % 4, 1] - bb_bottom[min_idx, 1])**2 +
            (bb_bottom[(min_idx + 1) % 4, 0] - bb_bottom[min_idx, 0])**2)**0.5
        b_2 = (
            (bb_bottom[(min_idx + 3) % 4, 1] - bb_bottom[min_idx, 1])**2 +
            (bb_bottom[(min_idx + 3) % 4, 0] - bb_bottom[min_idx, 0])**2)**0.5
        if theta < 90 * math.pi / 180:
            rotation_angle = theta
            dimension_x = b_1
            dimension_y = b_2
        else:
            rotation_angle = theta - 90 * math.pi / 180
            dimension_x = b_2
            dimension_y = b_1

        # boundingbox旋转角四元数
        boundingbox_msg.pose.orientation.x = 0
        boundingbox_msg.pose.orientation.y = 0
        boundingbox_msg.pose.orientation.z = math.sin(0.5 * rotation_angle)
        boundingbox_msg.pose.orientation.w = math.cos(0.5 * rotation_angle)

        # boundingbox尺寸
        boundingbox_msg.dimensions.x = dimension_x
        boundingbox_msg.dimensions.y = dimension_y
        boundingbox_msg.dimensions.z = bb_in_lidar[:, 2].max(
        ) - bb_in_lidar[:, 2].min()

        boundingbox_msg.value = 0
        boundingbox_msg.label = 0
        boundingboxarray_msg.boxes.append(boundingbox_msg)

    return boundingboxarray_msg
Esempio n. 8
0
    def detector_callback(self, pcl_msg):
        start = time.time()
        # rospy.loginfo('Processing Pointcloud with PointRCNN')
        arr_bbox = BoundingBoxArray()
        seq = pcl_msg.header.seq
        stamp = pcl_msg.header.stamp
        # in message pointcloud has x pointing forward, y pointing to the left and z pointing upward
        pts_lidar = np.array([[
            p[0], p[1], p[2], p[3]
        ] for p in pc2.read_points(
            pcl_msg, skip_nans=True, field_names=("x", "y", "z", "intensity"))
                              ],
                             dtype=np.float32)
        scores, dt_box_lidar, types = self.run_model(pts_lidar)

        # TODO: question convert into torch tensors? torch.from_numpy(pts_lidar)
        # move onto gpu if available

        # TODO: check if needs translation/rotation to compensate for tilt etc.

        if scores.size != 0:
            for i in range(scores.size):
                bbox = BoundingBox()
                bbox.header.frame_id = pcl_msg.header.frame_id
                bbox.header.stamp = rospy.Time.now()
                # bbox.header.seq = pcl_msg.header.seq
                q = yaw2quaternion(float(dt_box_lidar[i][6]))
                bbox.pose.orientation.x = q[1]
                bbox.pose.orientation.y = q[2]
                bbox.pose.orientation.z = q[3]
                bbox.pose.orientation.w = q[0]
                bbox.pose.position.x = float(dt_box_lidar[i][0])
                bbox.pose.position.y = float(dt_box_lidar[i][1])
                bbox.pose.position.z = float(dt_box_lidar[i][2])
                bbox.dimensions.x = float(dt_box_lidar[i][3])
                bbox.dimensions.y = float(dt_box_lidar[i][4])
                bbox.dimensions.z = float(dt_box_lidar[i][5])
                bbox.value = scores[i]
                bbox.label = int(types[i])
                arr_bbox.boxes.append(bbox)

        # rospy.loginfo("3D detector time: {}".format(time.time() - start))

        arr_bbox.header.frame_id = pcl_msg.header.frame_id
        arr_bbox.header.stamp = pcl_msg.header.stamp
        arr_bbox.header.seq = pcl_msg.header.seq

        if len(arr_bbox.boxes) != 0:
            self.pub_arr_bbox.publish(arr_bbox)
            arr_bbox.boxes = []
        else:
            arr_bbox.boxes = []
            self.pub_arr_bbox.publish(arr_bbox)
Esempio n. 9
0
def dummyBoundingBoxPublisher():
    pub = rospy.Publisher('/dummy_bounding_box', BoundingBox, queue_size=1)
    rospy.init_node('dummyBoundingBoxPublisher_node', anonymous=True)
    rate = rospy.Rate(25)

    boundingBox_object = BoundingBox()
    i = 0
    pose_object = Pose()
    dimensions_object = Vector3()
    minimum_dimension = 0.2
    boundingBox_object.label = 1234

    while not rospy.is_shutdown():
        h = std_msgs.msg.Header()
        h.stamp = rospy.Time.now(
        )  # Note you need to call rospy.init_node() before this will work
        h.frame_id = "world"

        boundingBox_object.header = h

        sinus_value = math.sin(i / 10.0)
        boundingBox_object.value = sinus_value

        # Change Pose to see effects
        pose_object.position.x = 1.0
        pose_object.position.y = 0.0
        pose_object.position.z = sinus_value

        # ai, aj, ak == roll, pitch, yaw
        quaternion = tf.transformations.quaternion_from_euler(ai=0,
                                                              aj=0,
                                                              ak=sinus_value)
        pose_object.orientation.x = quaternion[0]
        pose_object.orientation.y = quaternion[1]
        pose_object.orientation.z = quaternion[2]
        pose_object.orientation.w = quaternion[3]

        dimensions_object.x = sinus_value / 10 + minimum_dimension
        dimensions_object.y = minimum_dimension
        dimensions_object.z = minimum_dimension

        # Assign pose and dimension objects
        boundingBox_object.pose = pose_object
        boundingBox_object.dimensions = dimensions_object
        pub.publish(boundingBox_object)
        rate.sleep()

        i += 1
Esempio n. 10
0
def rslidar_callback(msg):
    # t_t = time.time()
    arr_bbox = BoundingBoxArray()
    msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
    np_p = get_xyz_points(msg_cloud, True)
 
    print("  ")
    seq = msg.header.seq
    stamp = msg.header.stamp
    input_points = {
        'stamp': stamp,
        'seq': seq,
        'points': np_p
    }
    if(proc_1.get_lidar_data(input_points)):
        scores, dt_box_lidar, types = proc_1.run()

        if scores.size != 0:
            for i in range(scores.size):
                bbox = BoundingBox()
                bbox.header.frame_id = msg.header.frame_id
                bbox.header.stamp = rospy.Time.now()
                q = yaw2quaternion(float(dt_box_lidar[i][8]))
                bbox.pose.orientation.x = q[1]
                bbox.pose.orientation.y = q[2]
                bbox.pose.orientation.z = q[3]
                bbox.pose.orientation.w = q[0]
                bbox.pose.position.x = float(dt_box_lidar[i][0])
                bbox.pose.position.y = float(dt_box_lidar[i][1])
                bbox.pose.position.z = float(dt_box_lidar[i][2])
                bbox.dimensions.x = float(dt_box_lidar[i][4])
                bbox.dimensions.y = float(dt_box_lidar[i][3])
                bbox.dimensions.z = float(dt_box_lidar[i][5])
                bbox.value = scores[i]
                bbox.label = int(types[i])
                arr_bbox.boxes.append(bbox)
        # print("total callback time: ", time.time() - t_t)
        arr_bbox.header.frame_id = msg.header.frame_id
        arr_bbox.header.stamp = msg.header.stamp
        if len(arr_bbox.boxes) is not 0:
            pub_arr_bbox.publish(arr_bbox)
            arr_bbox.boxes = []
        else:
            arr_bbox.boxes = []
            pub_arr_bbox.publish(arr_bbox)
Esempio n. 11
0
def vis_callback(msg):
    obj_bba = BoundingBoxArray()
    obj_bba.header.frame_id = 'map'

    for obj in msg.objects:
        obj_bb = BoundingBox()
        res = tosm.query_individual(obj.object_name + str(obj.ID))
        obj_bb.header = obj.header
        pose = res.pose[0].replace('[', '').replace(']', '').split(',')
        size = res.size[0].replace('[', '').replace(']', '').split(',')

        obj_bb.pose.position.x = float(pose[0])
        obj_bb.pose.position.y = float(pose[1])
        obj_bb.pose.position.z = float(pose[2]) + float(size[2]) / 2.0
        obj_bb.pose.orientation.x = float(pose[3])
        obj_bb.pose.orientation.y = float(pose[4])
        obj_bb.pose.orientation.z = float(pose[5])
        obj_bb.pose.orientation.w = float(pose[6])

        # bounding box size
        obj_bb.dimensions.x = float(size[0])
        obj_bb.dimensions.y = float(size[1])
        obj_bb.dimensions.z = float(size[2])

        # likelihood
        obj_bb.value = 1

        # determine the color
        if (obj.object_name == "hingeddoor"):
            obj_bb.label = 1
        elif (obj.object_name == "elevatordoor"):
            obj_bb.label = 2
        elif (obj.object_name == "A"):
            obj_bb.label = 3
        elif (obj.object_name == "B"):
            obj_bb.label = 4
        elif (obj.object_name == "C"):
            obj_bb.label = 5
        else:
            obj_bb.label = 10

        obj_bba.boxes.append(obj_bb)

    ses_map_object_vis_pub.publish(obj_bba)
    def to_msg(self):
        msg = BoundingBox()
        #msg.header = header
        msg.label = self.id
        msg.value = self.d_area

        msg.pose.position.x = self.center[-1][0]
        msg.pose.position.y = self.center[-1][1]
        msg.pose.position.z = 0

        msg.pose.orientation.x = self.direction[0]
        msg.pose.orientation.y = self.direction[1]
        msg.pose.orientation.z = 0
        msg.pose.orientation.w = 0

        msg.dimensions.x = self.withe
        msg.dimensions.y = self.hight
        msg.dimensions.z = 0
        return msg
Esempio n. 13
0
    def to_msg(self):
        msg = BoundingBox()
        msg.label = self.id
        msg.value = 0

        msg.pose.position.x = self.bbox[0]
        msg.pose.position.y = self.bbox[1]
        msg.pose.position.z = 0

        msg.pose.orientation.x = 0
        msg.pose.orientation.y = 0
        msg.pose.orientation.x = 0
        msg.pose.orientation.w = 0

        msg.dimensions.x = self.bbox[2] - self.bbox[0]
        msg.dimensions.y = self.bbox[3] - self.bbox[1]
        msg.dimensions.z = 0

        #print (msg)

        return msg
  box_b.label = 5
  box_arr = BoundingBoxArray()
  now = rospy.Time.now()
  box_a.header.stamp = now
  box_b.header.stamp = now
  box_arr.header.stamp = now
  box_a.header.frame_id = "map"
  box_b.header.frame_id = "map"
  box_arr.header.frame_id = "map"
  q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1])
  box_a.pose.orientation.x = q[0]
  box_a.pose.orientation.y = q[1]
  box_a.pose.orientation.z = q[2]
  box_a.pose.orientation.w = q[3]
  box_b.pose.orientation.w = 1
  box_b.pose.position.y = 2
  box_b.dimensions.x = (counter % 10 + 1) * 0.1
  box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1
  box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1
  box_a.dimensions.x = 1
  box_a.dimensions.y = 1
  box_a.dimensions.z = 1
  box_a.value = (counter % 100) / 100.0
  box_b.value = 1 - (counter % 100) / 100.0
  box_arr.boxes.append(box_a)
  box_arr.boxes.append(box_b)
  pub.publish(box_arr)
  r.sleep()
  counter = counter + 1
  
Esempio n. 15
0
def rslidar_callback(msg):
    t_t = time.time()
    arr_bbox = BoundingBoxArray()

    msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
    np_p = get_xyz_points(msg_cloud, True)
    print("  ")
    scores, dt_box_lidar, types = proc_1.run(np_p)

    MarkerArray_list = MarkerArray()  ##CREO EL MENSAJE GENERAL

    if scores.size != 0:
        for i in range(scores.size):
            if scores[i] > 0.4:
                bbox = BoundingBox()
                bbox.header.frame_id = msg.header.frame_id
                bbox.header.stamp = rospy.Time.now()
                q = yaw2quaternion(float(dt_box_lidar[i][6]))
                bbox.pose.orientation.x = q[1]
                bbox.pose.orientation.y = q[2]
                bbox.pose.orientation.z = q[3]
                bbox.pose.orientation.w = q[0]
                bbox.pose.position.x = float(dt_box_lidar[i][0]) - 69.12 / 2
                bbox.pose.position.y = float(dt_box_lidar[i][1])
                bbox.pose.position.z = float(dt_box_lidar[i][2])
                bbox.dimensions.x = float(dt_box_lidar[i][3])
                bbox.dimensions.y = float(dt_box_lidar[i][4])
                bbox.dimensions.z = float(dt_box_lidar[i][5])
                bbox.value = scores[i]
                bbox.label = int(types[i])
                arr_bbox.boxes.append(bbox)

                obj = Marker()
                #obj.CUBE = 1
                obj.header.stamp = rospy.Time.now()
                obj.header.frame_id = msg.header.frame_id
                obj.type = Marker.CUBE
                obj.id = i
                obj.lifetime = rospy.Duration.from_sec(1)
                #obj.type = int(types[i])
                obj.pose.position.x = float(dt_box_lidar[i][0]) - 69.12 / 2
                obj.pose.position.y = float(dt_box_lidar[i][1])
                obj.pose.position.z = float(dt_box_lidar[i][2])
                q = yaw2quaternion(float(dt_box_lidar[i][6]))
                obj.pose.orientation.x = q[1]
                obj.pose.orientation.y = q[2]
                obj.pose.orientation.z = q[3]
                obj.pose.orientation.w = q[0]
                obj.scale.x = float(dt_box_lidar[i][3])
                obj.scale.y = float(dt_box_lidar[i][4])
                obj.scale.z = float(dt_box_lidar[i][5])
                obj.color.r = 255
                obj.color.a = 0.5

                MarkerArray_list.markers.append(obj)

    print("total callback time: ", time.time() - t_t)
    arr_bbox.header.frame_id = msg.header.frame_id
    arr_bbox.header.stamp = msg.header.stamp
    if len(arr_bbox.boxes) is not 0:
        pub_arr_bbox.publish(arr_bbox)
        arr_bbox.boxes = []
    else:
        arr_bbox.boxes = []
        pub_arr_bbox.publish(arr_bbox)

    pubMarker.publish(MarkerArray_list)
    box_a.label = 2
    box_b.label = 5
    box_arr = BoundingBoxArray()
    now = rospy.Time.now()
    box_a.header.stamp = now
    box_b.header.stamp = now
    box_arr.header.stamp = now
    box_a.header.frame_id = "map"
    box_b.header.frame_id = "map"
    box_arr.header.frame_id = "map"
    q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1])
    box_a.pose.orientation.x = q[0]
    box_a.pose.orientation.y = q[1]
    box_a.pose.orientation.z = q[2]
    box_a.pose.orientation.w = q[3]
    box_b.pose.orientation.w = 1
    box_b.pose.position.y = 2
    box_b.dimensions.x = (counter % 10 + 1) * 0.1
    box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1
    box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1
    box_a.dimensions.x = 1
    box_a.dimensions.y = 1
    box_a.dimensions.z = 1
    box_a.value = (counter % 100) / 100.0
    box_b.value = 1 - (counter % 100) / 100.0
    box_arr.boxes.append(box_a)
    box_arr.boxes.append(box_b)
    pub.publish(box_arr)
    r.sleep()
    counter = counter + 1