def parseBoxes(data): global lidar_array global filter_boxes filter_boxes = BoundingBoxArray() data_ped = BoundingBoxArray() data_car = BoundingBoxArray() data_follow = BoundingBoxArray() # What is that ? previous_targets = BoundingBoxArray() #WE NEED TO INITIALIZE IT data_ped.header = data.header data_car.header = data.header data_follow.header = data.header filter_boxes.header = data.header lidar_array = Multi_targets() data_copy = data # We do that to be able to modify time stamp. Does it work ? #Remove flickering loop for i in range(len(data.boxes)): x = data_copy.boxes[i].pose.position.x y = data_copy.boxes[i].pose.position.y for j in range(len(previous_targets)): x_previous = previous_targets.boxes[j].pose.position.x y_previous = previous_targets.boxes[j].pose.position.y distance = math.sqrt( (x - x_previous)**2 + (y - y_previous)**2 ) # WE CAN ALSO COMPARE THE BOX SIZE if distance < 0.25: = data_copy.boxes[i].header.stamp.secs
def callback_with_cluster_box(self, cluster_boxes_msg, instance_boxes_msg, instance_label_msg): labeled_cluster_boxes = BoundingBoxArray() labeled_instance_boxes = BoundingBoxArray() labeled_cluster_boxes.header = cluster_boxes_msg.header labeled_instance_boxes.header = instance_boxes_msg.header for index, box in enumerate(cluster_boxes_msg.boxes): if not box.pose.position.x == 0.0: tmp_box = BoundingBox() tmp_box.header = box.header tmp_box.pose = box.pose tmp_box.dimensions = box.dimensions # TODO fix index indent, jsk_pcl_ros_utils/label_to_cluster_point_indices_nodelet.cpp tmp_box.label = index + 1 labeled_cluster_boxes.boxes.append(tmp_box) for box, label in zip(instance_boxes_msg.boxes, instance_label_msg.labels): tmp_box = BoundingBox() tmp_box.header = box.header tmp_box.pose = box.pose tmp_box.dimensions = box.dimensions tmp_box.label = label.id labeled_instance_boxes.boxes.append(tmp_box) self.labeled_cluster_boxes_pub.publish(labeled_cluster_boxes) self.labeled_instance_boxes_pub.publish(labeled_instance_boxes)
def callback(data): header = data.header frame = header.seq boxes = BoundingBoxArray() #3D Boxes with JSK boxes.header = header rects = ImageObj() #Rects Autoware rects.header = header rects.type = "car" texts = PictogramArray() #Labels with JSK texts.header = header if kitti_data.has_key(frame) == True: for b in kitti_data[frame]: b.header = header boxes.boxes.append(b) if auto_boxes.has_key(frame) == True: for rect in auto_boxes[frame]: rects.obj.append(rect) if pictogram_texts.has_key(frame) == True: for txt in pictogram_texts[frame]: txt.header = header texts.pictograms.append(txt) pub.publish(boxes) pub_boxes.publish(rects) pub_pictograms.publish(texts)
def callback(data): header = data.header frame = header.seq boxes = BoundingBoxArray() boxes.header = header texts = PictogramArray() texts.header = header if kitti_data.has_key(frame) == True: for b in kitti_data[frame]: b.header = header boxes.boxes.append(b) str = b rospy.loginfo(str) if pictogram_texts.has_key(frame) == True: for txt in pictogram_texts[frame]: txt.header = header texts.pictograms.append(txt) str = txt rospy.loginfo(str) pub_boxes.publish(boxes) pub_pictograms.publish(texts)
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
def msg_builder(self, header): msg = BoundingBoxArray() msg.header = header for b in self.bboxes: msg.boxes.append(self.bboxes[b].to_msg()) return msg
def callback(msg): new_msg = BoundingBoxArray() new_msg.header = msg.header trans = concatenate_matrices(translation_matrix(transform[0:3]), euler_matrix(*transform[3:6])) for box in msg.boxes: old_pose = concatenate_matrices(translation_matrix([ box.pose.position.x, box.pose.position.y, box.pose.position.z]), quaternion_matrix([ box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z, box.pose.orientation.w])) new_pose = concatenate_matrices(old_pose, trans) translation = translation_from_matrix(new_pose) rotation = quaternion_from_matrix(new_pose) box.pose.position.x = translation[0] box.pose.position.y = translation[1] box.pose.position.z = translation[2] box.pose.orientation.x = rotation[0] box.pose.orientation.y = rotation[1] box.pose.orientation.z = rotation[2] box.pose.orientation.w = rotation[3] # box.dimensions.z, box.dimensions.y = box.dimensions.y, box.dimensions.z new_msg.boxes.append(box) p.publish(new_msg)
def callback(msg): global pub, frame_id, min_z, max_z, tf_listener boxes = msg.boxes # latest (pos, rot) = tf_listener.lookupTransform(msg.header.frame_id, frame_id, rospy.Time(0)) origin_pose = concatenate_matrices(translation_matrix(pos), quaternion_matrix(rot)) result = BoundingBoxArray() result.header = msg.header min_z_pos = 1000000.0 min_box = None for box in msg.boxes: box_pos = [ box.pose.position.x, box.pose.position.y, box.pose.position.z ] # [x, y, z, w] box_rot = [ box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z, box.pose.orientation.w ] box_pose = concatenate_matrices(translation_matrix(box_pos), quaternion_matrix(box_rot)) box_global_pose = concatenate_matrices(origin_pose, box_pose) box_global_pos = translation_from_matrix(box_global_pose) if box_global_pos[2] > min_z and box_global_pos[2] < max_z: result.boxes.append(box) if box_global_pos[2] < min_z_pos: min_z_pos = box_global_pos[2] min_box = box if len(result.boxes) > 1: result.boxes = [min_box] pub.publish(result)
def callback(msg): global pub, frame_id, min_z, max_z, tf_listener boxes = msg.boxes # latest (pos, rot) = tf_listener.lookupTransform(msg.header.frame_id, frame_id, rospy.Time(0)) origin_pose = concatenate_matrices( translation_matrix(pos), quaternion_matrix(rot)) result = BoundingBoxArray() result.header = msg.header min_z_pos = 1000000.0 min_box = None for box in msg.boxes: box_pos = [box.pose.position.x, box.pose.position.y, box.pose.position.z] # [x, y, z, w] box_rot = [box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z, box.pose.orientation.w] box_pose = concatenate_matrices( translation_matrix(box_pos), quaternion_matrix(box_rot)) box_global_pose = concatenate_matrices(origin_pose, box_pose) box_global_pos = translation_from_matrix(box_global_pose) if box_global_pos[2] > min_z and box_global_pos[2] < max_z: result.boxes.append(box) if box_global_pos[2] < min_z_pos: min_z_pos = box_global_pos[2] min_box = box if len(result.boxes) > 1: result.boxes = [min_box] pub.publish(result)
def callback(box_array, coef_array): # We assume that the size of box_array and polygon_array are 1 if len(box_array.boxes) > 0 and len(coef_array.coefficients) > 0: if (box_array.boxes[0].header.frame_id == coef_array.coefficients[0].header.frame_id): box = box_array.boxes[0] box_pos = numpy.array([ box.pose.position.x, box.pose.position.y, box.pose.position.z ]) coef = coef_array.coefficients[0].values n = numpy.array([coef[0], coef[1], coef[2]]) d = (coef[0] * box_pos[0] + coef[1] * box_pos[1] + coef[2] * box_pos[2] + coef[3] / math.sqrt(coef[0] * coef[0] + coef[1] * coef[1] + coef[2] * coef[2])) required_distance = distance - d rospy.loginfo("required_distance: %f" % (required_distance)) new_box_pos = required_distance * n + box_pos box.pose.position.x = new_box_pos[0] box.pose.position.y = new_box_pos[1] box.pose.position.z = new_box_pos[2] result_box_array = BoundingBoxArray() result_box_array.header = box_array.header result_box_array.boxes = [box] pub_box_array.publish(result_box_array) else: rospy.logwarn("frame_id of box array and coef array are not same") else: rospy.logwarn("Size of box array and coef array are not enough")
def callback(data): header = data.header frame = header.seq boxes = BoundingBoxArray() #3D Boxes with JSK boxes.header = header rects = image_obj() #Rects Autoware rects.header = header rects.type = "car" texts = PictogramArray() #Labels with JSK texts.header = header if kitti_data.has_key(frame) == True: for b in kitti_data[frame]: b.header = header boxes.boxes.append(b) if auto_boxes.has_key(frame) == True: for rect in auto_boxes[frame]: rects.obj.append(rect) if pictogram_texts.has_key(frame) == True: for txt in pictogram_texts[frame]: txt.header = header texts.pictograms.append(txt) pub.publish(boxes) pub_boxes.publish(rects) pub_pictograms.publish(texts)
def parseBoxes(data): global pub_ped global pub_car data_ped = BoundingBoxArray() data_car = BoundingBoxArray() data_follow = BoundingBoxArray() counterp = 0 counterc = 0 data_ped.header = data.header data_car.header = data.header data_follow.header = data.header for i in range(len(data.boxes)): x = data.boxes[i].pose.position.x y = data.boxes[i].pose.position.y height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y minl = 1000; minr = 1000; minf = 1000; if (y<50 and y>0) and (x<2.5 and x>-2.5): # # Vehicle # if (height < 5 and height > 0.5) and (length < 20 and length > 0) and (width < 20 and width > 0): data_follow.boxes.append(data.boxes[i]) if (y<20 and y>-10) and (x<8 and x>-8): height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y # Pedstrian if (height < 1.8 and height > 0.5) and (length < 1.5 and length > 0) and (width < 1.5 and width > 0): data_ped.boxes.append(data.boxes[i]) counterp = counterp + 1 elif (height < 5 and height > 0.5) and (length < 10 and length > 0) and (width < 10 and width > 0): data_car.boxes.append(data.boxes[i]) counterc = counterc + 1 if x>0 and x<minr and y<10 and y>-10: minr = x if x<0 and abs(x)<minl and y<10 and y>-10 minl = abs(x) if y>0 and y<minf and x<10 and x>10 minf = y pub_ped.publish(data_ped) pub_car.publish(data_car) pub_follow.publish(data_follow)
def parseBoxes(data): global lidar_array global filter_box = BoundingBoxArray() data_ped = BoundingBoxArray() data_car = BoundingBoxArray() data_follow = BoundingBoxArray() counterp = 0 counterc = 0 data_ped.header = data.header data_car.header = data.header data_follow.header = data.header lidar_array = Multi_targets() for i in range(len(data.boxes)): lidar_target = target() x = data.boxes[i].pose.position.x y = data.boxes[i].pose.position.y height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y lidar_target.pos_x = x lidar_target.pos_y = y if (y<50 and y>0) and (x<2.5 and x>-2.5): data_follow.boxes.append(data.boxes[i]) if (y<20 and y>-10) and (x<8 and x>-8): height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y # Pedstrian if (height < 1.8 and height > 0.5) and (length < 1.5 and length > 0) and (width < 1.5 and width > 0): data_ped.boxes.append(data.boxes[i]) # lidar_target.category = 0 # lidar_target.counter = .1 # Vehicle elif (height < 5 and height > 0.5) and (length < 10 and length > 0) and (width < 10 and width > 0): data_car.boxes.append(data.boxes[i]) # lidar_target.category = 1 # lidar_target.counter = .1 lidar_array.data.append(lidar_target) filter_box.boxes.append(data.boxes[i]) # pub_ly_fuse.publish(ly_array) pub_car.publish(data_car) pub_follow.publish(data_follow)
def msg_builder(self, header): msg = BoundingBoxArray() msg.header = header for t in self.trace: if t.active: msg.boxes.append(t.to_msg()) return msg
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 callback(msg): box_array = BoundingBoxArray() box_array.header = msg.header for footstep in msg.footsteps: box = BoundingBox() box.header = msg.header box.pose = footstep.pose box.dimensions = footstep.dimensions box_array.boxes.append(box) pub.publish(box_array)
def LeadCarUpdate(msg): global lidar_array, filter_boxes leadc_boxes = BoundingBoxArray() leadc_boxes.header = filter_boxes.header compare_boxes = BoundingBoxArray() compare_boxes.header = filter_boxes.header compare_boxes.boxes = filter_boxes.boxes[:] if len(msg.data)==0: print('no lead car') else: rc_x = msg.data[0].pos_x rc_y = msg.data[0].pos_y rc_v = msg.data[0].speed distances = [] for i in range(len(compare_boxes.boxes)): lidar_x = compare_boxes.boxes[i].pose.position.x lidar_y = compare_boxes.boxes[i].pose.position.y distance = np.sqrt((rc_x-lidar_x)**2+(rc_y-lidar_y)**2) distances.append(distance) print(min(distances)) if (min(distances)<5): obj_i = distances.index(min(distances)) leadc_boxes.boxes.append(compare_boxes.boxes[obj_i]) else: leadc_box = BoundingBox() leadc_box.header = filter_boxes.boxes[1].header leadc_box.pose.position.x = rc_x leadc_box.pose.position.y = rc_y leadc_box.pose.position.z = .5 leadc_box.pose.orientation.x = 0 leadc_box.pose.orientation.y = 0 leadc_box.pose.orientation.z = 0 leadc_box.pose.orientation.w = 0 leadc_box.dimensions.x = 5 leadc_box.dimensions.y = 5 leadc_box.dimensions.z = 5 leadc_boxes.boxes.append(leadc_box) pub_leadc.publish(leadc_boxes)
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
def callback(msg): box_array = BoundingBoxArray() box_array.header = msg.header for footstep in msg.footsteps: box = BoundingBox() box.header = msg.header box.pose = footstep.pose box.dimensions = footstep.dimensions box.pose.position.z += (z_max + z_min) / 2.0 box.dimensions.z = z_max - z_min box_array.boxes.append(box) pub.publish(box_array)
def callback(self, instance_boxes_msg, instance_label_msg): labeled_instance_boxes = BoundingBoxArray() labeled_instance_boxes.header = instance_boxes_msg.header for box, label in zip(instance_boxes_msg.boxes, instance_label_msg.labels): tmp_box = BoundingBox() tmp_box.header = box.header tmp_box.pose = box.pose tmp_box.dimensions = box.dimensions tmp_box.label = label.id labeled_instance_boxes.boxes.append(tmp_box) self.labeled_instance_boxes_pub.publish(labeled_instance_boxes)
def callback(data): header = data.header frame = header.seq boxes = BoundingBoxArray() boxes.header = header texts = PictogramArray() texts.header = header arrows = MarkerArray() paths = MarkerArray() if bounding_boxes.has_key(frame) == True: for bbox in bounding_boxes[frame]: bbox.header = header boxes.boxes.append(bbox) maxbox = len(boxes.boxes) # print("maxbox: %s"%maxbox) if pictogram_texts.has_key(frame) == True: for txt in pictogram_texts[frame]: txt.header = header texts.pictograms.append(txt) # str = txt # rospy.loginfo(str) if marker_arrows.has_key(frame) == True: for arrow in marker_arrows[frame]: arrow.header = header arrows.markers.append(arrow) maxarrow = len(arrows.markers) # print("maxarrow: %s"%(maxarrow)) if marker_paths.has_key(frame) == True: for predict in marker_paths[frame]: predict.header = header paths.markers.append(predict) # print(predict) pub_boxes.publish(boxes) pub_pictograms.publish(texts) pub_arrows.publish(arrows) pub_paths.publish(paths)
def bounding_box_array_callback(msg): rospy.loginfo("CHECKING FOR VALIDITY OF BOXES") valid_boxes = BoundingBoxArray() for m in msg.boxes: box_dim = Vector3() box_dim = m.dimensions if (box_dim.x > min_threshold_x_) and \ (box_dim.y > min_threshold_y_) and \ (box_dim.z > min_threshold_z_): valid_boxes.boxes.append(m) print "LENGHT: ", len(valid_boxes.boxes) if (len(valid_boxes.boxes) == 0): int_stamp = Int32Stamped() int_stamp.header = msg.header int_stamp.data = -1 pub_signal_.publish(int_stamp) else: valid_boxes.header = msg.header pub_boxes_.publish(valid_boxes)
def callback(boxes_msg): # we expect odom (trans, rot) = tf_listener.lookupTransform(frame_id, boxes_msg.header.frame_id, rospy.Time(0)) origin_matrix = concatenate_matrices(translation_matrix(trans), quaternion_matrix(rot)) min_y_pose = None min_y_dist = 100.0 min_y_box = None for box in boxes_msg.boxes: pose = box.pose # pose to matrix pose_matrix = concatenate_matrices( translation_matrix( (pose.position.x, pose.position.y, pose.position.z)), quaternion_matrix(((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)))) box_pose = concatenate_matrices(origin_matrix, pose_matrix) box_pos = translation_from_matrix(box_pose) if (box.dimensions.x < min_x or box.dimensions.x > max_x or box.dimensions.y < min_y or box.dimensions.y > max_y or box.dimensions.z < min_z or box.dimensions.z > max_z): continue if 0.8 < box_pos[2] and box_pos[2] < 1.0: if min_y_dist > abs(box_pos[1]): min_y_dist = abs(box_pos[1]) min_y_pose = pose min_y_box = box if min_y_pose: pose_msg = PoseStamped() pose_msg.header = boxes_msg.header pose_msg.pose = min_y_pose pub.publish(pose_msg) box_arr = BoundingBoxArray() box_arr.header = boxes_msg.header box_arr.boxes = [min_y_box] pub_box.publish(box_arr) rospy.loginfo("Success to detect handle bbx") return rospy.logerr("Failed to detect handle bbox")
def callback(box_array, cluster_indices): if len(box_array.boxes) > 0: min_index = 0 min_distance = 100000 for box, i in zip(box_array.boxes, range(len(box_array.boxes))): distance = math.sqrt(box.pose.position.x * box.pose.position.x + box.pose.position.y * box.pose.position.y + box.pose.position.z * box.pose.position.z) if min_distance > distance: min_distance = distance min_index = i result_box_array = BoundingBoxArray() result_box_array.header = box_array.header result_box_array.boxes.append(box_array.boxes[min_index]) pub_box_array.publish(result_box_array) pub_indices.publish(cluster_indices.cluster_indices[min_index]) result_cluster_indices = ClusterPointIndices() result_cluster_indices.header = cluster_indices.header result_cluster_indices.cluster_indices = [cluster_indices.cluster_indices[min_index]] pub_cluster_indices.publish(result_cluster_indices) else: rospy.logwarn("No bounding box input")
def callback(box_array, coef_array): # We assume that the size of box_array and polygon_array are 1 if len(box_array.boxes) > 0 and len(coef_array.coefficients) > 0: if (box_array.boxes[0].header.frame_id == coef_array.coefficients[0].header.frame_id): box = box_array.boxes[0] box_pos = numpy.array([box.pose.position.x, box.pose.position.y, box.pose.position.z]) coef = coef_array.coefficients[0].values n = numpy.array([coef[0], coef[1], coef[2]]) d = (coef[0] * box_pos[0] + coef[1] * box_pos[1] + coef[2] * box_pos[2] + coef[3] / math.sqrt(coef[0] * coef[0] + coef[1] * coef[1] + coef[2] * coef[2])) required_distance = distance - d rospy.loginfo("required_distance: %f" % (required_distance)) new_box_pos = required_distance * n + box_pos box.pose.position.x = new_box_pos[0] box.pose.position.y = new_box_pos[1] box.pose.position.z = new_box_pos[2] result_box_array = BoundingBoxArray() result_box_array.header = box_array.header result_box_array.boxes = [box] pub_box_array.publish(result_box_array) else: rospy.logwarn("frame_id of box array and coef array are not same") else: rospy.logwarn("Size of box array and coef array are not enough")
def callback(box_array, cluster_indices): if len(box_array.boxes) > 0: min_index = 0 min_distance = 100000 for box, i in zip(box_array.boxes, range(len(box_array.boxes))): distance = math.sqrt(box.pose.position.x * box.pose.position.x + box.pose.position.y * box.pose.position.y + box.pose.position.z * box.pose.position.z) if min_distance > distance: min_distance = distance min_index = i result_box_array = BoundingBoxArray() result_box_array.header = box_array.header result_box_array.boxes.append(box_array.boxes[min_index]) pub_box_array.publish(result_box_array) pub_indices.publish(cluster_indices.cluster_indices[min_index]) result_cluster_indices = ClusterPointIndices() result_cluster_indices.header = cluster_indices.header result_cluster_indices.cluster_indices = [ cluster_indices.cluster_indices[min_index] ] pub_cluster_indices.publish(result_cluster_indices) else: rospy.logwarn("No bounding box input")
def callback(boxes_msg): # we expect odom (trans, rot) = tf_listener.lookupTransform(frame_id, boxes_msg.header.frame_id, rospy.Time(0)) origin_matrix = concatenate_matrices(translation_matrix(trans), quaternion_matrix(rot)) min_y_pose = None min_y_dist = 100.0 min_y_box = None for box in boxes_msg.boxes: pose = box.pose # pose to matrix pose_matrix = concatenate_matrices(translation_matrix((pose.position.x, pose.position.y, pose.position.z)), quaternion_matrix(((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)))) box_pose = concatenate_matrices(origin_matrix, pose_matrix) box_pos = translation_from_matrix(box_pose) if (box.dimensions.x < min_x or box.dimensions.x > max_x or box.dimensions.y < min_y or box.dimensions.y > max_y or box.dimensions.z < min_z or box.dimensions.z > max_z): continue if 0.8 < box_pos[2] and box_pos[2] < 1.0: if min_y_dist > abs(box_pos[1]): min_y_dist = abs(box_pos[1]) min_y_pose = pose min_y_box = box if min_y_pose: pose_msg = PoseStamped() pose_msg.header = boxes_msg.header pose_msg.pose = min_y_pose pub.publish(pose_msg) box_arr = BoundingBoxArray() box_arr.header = boxes_msg.header box_arr.boxes = [min_y_box] pub_box.publish(box_arr) rospy.loginfo("Success to detect handle bbx") return rospy.logerr("Failed to detect handle bbox")
bba = BoundingBoxArray() bbapub = rospy.Publisher('boundingboxarray', BoundingBoxArray) box.dimensions.x = 0.1 box.dimensions.y = 1 box.dimensions.z = 1 while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/map', '/r_gripper_palm_link', rospy.Time(0)) # import ipdb; ipdb.set_trace(); except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue rospy.loginfo("trans:%s rot:%s", trans, rot) box.header.stamp = rospy.Time.now() box.header.frame_id = "/map" box.pose.position = Point(*trans) print box.pose.position box.pose.position.x += 0 box.pose.position.y += (box.dimensions.y / 2) box.pose.position.z += (box.dimensions.z / 2) print box.pose.position # box.pose.position = Point(x=trans[0], y=trans[1], z=trans[2]) box.pose.orientation = Quaternion(*rot) print box bba.header = box.header bba.boxes = [box] bbapub.publish(bba) rate.sleep()
bb.dimensions.x = bb.dimensions.x + margin_x bb.dimensions.y = bb.dimensions.y + margin_y bb.dimensions.z = bb.dimensions.z + margin_z bba.boxes = [bb] pub.publish(bba) if __name__ == '__main__': rospy.init_node('pub_boundingbox_for_scan') pub = rospy.Publisher("~box", BoundingBoxArray) bba = BoundingBoxArray() bb = BoundingBox() r = rospy.Rate(1) while not rospy.is_shutdown(): bb.header.stamp = rospy.Time.now() bb.header.frame_id = "/map" bb.pose.position.x = 3.7 bb.pose.position.y = 8.3 bb.pose.position.z = 0.3 bb.pose.orientation.x = 0 bb.pose.orientation.y = 0 bb.pose.orientation.z = 0 bb.pose.orientation.w = 1 bb.dimensions.x = 1.8 bb.dimensions.y = 2.0 bb.dimensions.z = 0.2 bba.boxes = [bb] bba.header = bb.header pub.publish(bba) r.sleep()
def send_3d_bbox_anno(publisher, center, dim, angle, header): """ param center: x,y,z dims: h, w, l angles: angle publisher: ros publisher header: ros header """ header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'camera_color_frame' box_arr = BoundingBoxArray() box_arr.header = header # ------------------------------------------------------------------------------------------------------ # set variables # ------------------------------------------------------------------------------------------------------ box_arr = BoundingBoxArray() box_arr.header = header pred_x = center[0] pred_y = center[1] pred_z = center[2] pred_h = dim[0] pred_w = dim[1] pred_l = dim[2] pred_r_y = angle # ------------------------------------------------------------------------------------------------------ # cerate 3D box # ------------------------------------------------------------------------------------------------------ box_pred = BoundingBox() box_pred.header.stamp = rospy.Time.now() box_pred.header.frame_id = "camera_color_frame" box_pred.pose.position.x = pred_x box_pred.pose.position.y = pred_y box_pred.pose.position.z = pred_z # possible modofications #print("Location :",location_array[i][0],location_array[i][1],location_array[i][2]) #rotation_pred = quaternion_about_axis(pred_r_y+(np.pi/2),(0,1,0)) #rotation_pred = quaternion_about_axis(-np.pi/2,(0,0,1)) #qy_pred = quaternion_about_axis(pred_r_y+(np.pi/2),(0,1,0)) #qz_pred = quaternion_about_axis(-np.pi/2,(0,0,1)) #rotation_pred = quaternion_multiply(qy_pred, qz_pred) rotation_pred = R.from_euler('z', -pred_r_y, degrees=False).as_quat() #[0. 0. 0.09410831 0.99556196] box_pred.pose.orientation.x = rotation_pred[0] box_pred.pose.orientation.y = rotation_pred[1] box_pred.pose.orientation.z = rotation_pred[2] box_pred.pose.orientation.w = rotation_pred[3] box_pred.dimensions.x = pred_h #location_array[i][3] box_pred.dimensions.y = pred_w #location_array[i][4] box_pred.dimensions.z = pred_l #location_array[i][5] #box_pred.value = (counter % 100) / 100.0 box_arr.boxes.append(box_pred) # ------------------------------------------------------------------------------------------------------ # publish teh box to rviz # ------------------------------------------------------------------------------------------------------ publisher.publish(box_arr)
def parseBoxes(data): global lidar_array global filter_boxes filter_boxes = BoundingBoxArray() data_ped = BoundingBoxArray() data_car = BoundingBoxArray() data_follow = BoundingBoxArray() data_ped.header = data.header data_car.header = data.header data_follow.header = data.header filter_boxes.header = data.header lidar_array = Multi_targets() minl = 1000; minr = 1000; minf = 1000; for i in range(len(data.boxes)): lidar_target = target() x = data.boxes[i].pose.position.x y = data.boxes[i].pose.position.y height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y lidar_target.pos_x = x lidar_target.pos_y = y if (y<50 and y>0) and (x<2.5 and x>-2.5): data_follow.boxes.append(data.boxes[i]) if (y<50 and y>-10) and (x<8 and x>-12): height = data.boxes[i].dimensions.z length = data.boxes[i].dimensions.x width = data.boxes[i].dimensions.y # Pedstrian if (height < 1.8 and height > 0.5) and (length < 1.5 and length > 0) and (width < 1.5 and width > 0): data_ped.boxes.append(data.boxes[i]) # lidar_target.category = 0 # lidar_target.counter = .1 # Vehicle elif (height < 5 and height > 0.5) and (length < 10 and length > 0) and (width < 10 and width > 0): data_car.boxes.append(data.boxes[i]) # lidar_target.category = 1 # lidar_target.counter = .1 lidar_array.data.append(lidar_target) filter_boxes.boxes.append(data.boxes[i]) if x>0 and x<minr and y<10 and y>-10: minr = x if x<0 and abs(x)<minl and y<10 and y>-10: minl = abs(x) if y>0 and y<minf and x<10 and x>10: minf = y if minf < 5: front_pub.publish(1) elif minf < 10: front_pub.publish(2) else: front_pub.publish(3) if minl < 5: left_pub.publish(1) elif minl < 10: left_pub.publish(2) else: left_pub.publish(3) if minr < 5: right_pub.publish(1) elif minr < 10: right_pub.publish(2) else: right_pub.publish(3) pub_ped.publish(data_ped) pub_car.publish(data_car) pub_follow.publish(data_follow)
def merge_boxes(self, mask_rcnn_boxes, qatm_boxes, red_boxes): boxes = BoundingBoxArray() boxes.header = self.header boxes.boxes = mask_rcnn_boxes.boxes + qatm_boxes.boxes + red_boxes.boxes return boxes
def callback(self, front_MarkerArray, back_MarkerArray, TwistStamped): # print("front",len(front_MarkerArray.markers)/4) # print("back",len(back_MarkerArray.markers)/4) # # Concat front and back MarkerArray Messages add_MarkerArray = copy.deepcopy(front_MarkerArray) for i in range(len(back_MarkerArray.markers)): add_MarkerArray.markers.append(back_MarkerArray.markers[i]) # print("add",len(add_MarkerArray.markers)/4) # print("done") if len(add_MarkerArray.markers) == 0: return header = add_MarkerArray.markers[0].header frame = header.seq boxes = BoundingBoxArray() #3D Boxes with JSK boxes.header = header texts = PictogramArray() #Labels with JSK texts.header = header obj_ori_arrows = MarkerArray() #arrow with visualization_msgs velocity_markers = MarkerArray() #text with visualization_msgs obj_path_markers = MarkerArray() # passed path warning_line_markers = MarkerArray() dets = np.zeros((0, 9)) # (None, 9) : 9는 사용할 3d bbox의 파라미터 개수 obj_box_info = np.empty((0, 7)) obj_label_info = np.empty((0, 2)) # frame을 rviz에 출력 overlayTxt = OverlayText() overlayTxt.left = 10 overlayTxt.top = 10 overlayTxt.width = 1200 overlayTxt.height = 1200 overlayTxt.fg_color.a = 1.0 overlayTxt.fg_color.r = 1.0 overlayTxt.fg_color.g = 1.0 overlayTxt.fg_color.b = 1.0 overlayTxt.text_size = 12 overlayTxt.text = "Frame_seq : {}".format(frame) det_boxes = BoundingBoxArray() #3D Boxes with JSK det_boxes.header = header # Receive each objects info in this frame for object_info in add_MarkerArray.markers: #extract info [ frame,type(label),tx,ty,tz,h,w,l,ry ] if object_info.ns == "/detection/lidar_detector/box_markers": tx = object_info.pose.position.x ty = object_info.pose.position.y tz = object_info.pose.position.z l = object_info.scale.x w = object_info.scale.y h = object_info.scale.z quaternion_xyzw = [object_info.pose.orientation.x, object_info.pose.orientation.y, \ object_info.pose.orientation.z, object_info.pose.orientation.w] rz = tf.transformations.euler_from_quaternion( quaternion_xyzw)[2] obj_box_info = np.append( obj_box_info, [[-ty, -tz, tx - 0.27, h, w, l, -rz + np.pi / 2]], axis=0) size_det = Vector3(l, w, h) det_box = BoundingBox() det_box.header = header det_box.pose.position = Point(tx, ty, tz) q_det_box = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz) # 어쩔 수 없이 끝단에서만 90도 돌림 det_box.pose.orientation = Quaternion(*q_det_box) det_box.dimensions = size_det det_boxes.boxes.append(det_box) elif object_info.ns == "/detection/lidar_detector/label_markers": label = object_info.text.strip() if label == '': label = 'None' obj_label_info = np.append(obj_label_info, [[frame, label]], axis=0) dets = np.concatenate((obj_label_info, obj_box_info), axis=1) self.pub_det_markerarray.publish(det_boxes) del current_id_list[:] # All Detection Info in one Frame bboxinfo = dets[dets[:, 0] == str(frame), 2:9] # [ tx, ty, tz, h, w, l, rz ] additional_info = dets[dets[:, 0] == str(frame), 0:2] # frame, labe reorder = [3, 4, 5, 0, 1, 2, 6] # [tx,ty,tz,h,w,l,ry] -> [h,w,l,tx,ty,tz,theta] reorder_back = [3, 4, 5, 0, 1, 2, 6] # [h,w,l,tx,ty,tz,theta] -> [tx,ty,tz,h,w,l,ry] reorder2velo = [2, 0, 1, 3, 4, 5, 6] bboxinfo = bboxinfo[:, reorder] # reorder bboxinfo parameter [h,w,l,x,y,z,theta] bboxinfo = bboxinfo.astype(np.float64) dets_all = {'dets': bboxinfo, 'info': additional_info} # ObjectTracking from Detection trackers = self.mot_tracker.update(dets_all) # h,w,l,x,y,z,theta trackers_bbox = trackers[:, 0:7] trackers_info = trackers[:, 7:10] # id, frame, label trackers_bbox = trackers_bbox[:, reorder_back] # reorder_back bboxinfo parameter [tx,ty,tz,h,w,l,ry] trackers_bbox = trackers_bbox[:, reorder2velo] # reorder coordinate system cam to velo trackers_bbox = trackers_bbox.astype(np.float64) trackers_bbox[:, 0] = trackers_bbox[:, 0] trackers_bbox[:, 1] = trackers_bbox[:, 1] * -1 trackers_bbox[:, 2] = trackers_bbox[:, 2] * -1 trackers_bbox[:, 6] = trackers_bbox[:, 6] * -1 # for문을 통해 각 objects들의 정보를 추출하여 사용 for b, info in zip(trackers_bbox, trackers_info): bbox = BoundingBox() bbox.header = header # parameter 뽑기 [tx,ty,tz,h,w,l,rz] tx_trk, ty_trk, tz_trk = float(b[0]), float(b[1]), float(b[2]) rz_trk = float(b[6]) size_trk = Vector3(float(b[5]), float(b[4]), float(b[3])) obj_id = info[0] label_trk = info[2] bbox_color = colorCategory20(int(obj_id)) odom_mat = get_odom(self.tf2, "velo_link", "map") xyz = np.array(b[:3]).reshape(1, -1) points = np.array((0, 3), float) if odom_mat is not None: points = get_transformation(odom_mat, xyz) # 이전 x frame 까지 지나온 points들을 저장하여 반환하는 함수 # obj_id와 bbox.label은 단지 type차이만 날뿐 같은 데이터 # path_points_list = points_path(tx_trk, ty_trk, tz_trk, obj_id) path_points_list = points_path(points[0, 0], points[0, 1], points[0, 2], obj_id) map_header = copy.deepcopy(header) map_header.frame_id = "/map" bbox_color = colorCategory20(int(obj_id)) path_marker = Marker( type=Marker.LINE_STRIP, id=int(obj_id), lifetime=rospy.Duration(0.5), # pose=Pose(Point(0,0,0), Quaternion(0, 0, 0, 1)), # origin point position scale=Vector3(0.1, 0.0, 0.0), # line width header=map_header, color=bbox_color) path_marker.points = path_points_list obj_path_markers.markers.append(path_marker) # Tracker들의 BoundingBoxArray 설정 bbox.pose.position = Point(tx_trk, ty_trk, tz_trk / 2.0) q_box = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림 bbox.pose.orientation = Quaternion(*q_box) bbox.dimensions = size_trk bbox.label = int(obj_id) boxes.boxes.append(bbox) picto_text = Pictogram() picto_text.header = header picto_text.mode = Pictogram.STRING_MODE picto_text.pose.position = Point(tx_trk, ty_trk, -tz_trk) # q = tf.transformations.quaternion_from_euler(0.7, 0.0, -0.7) picto_text.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5) picto_text.size = 4 picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1) picto_text.character = label_trk + ' ' + str(bbox.label) texts.pictograms.append(picto_text) # GPS sensor values oxtLinear = TwistStamped.twist.linear # oxtLinear = TwistStamped.twist.linear # Tracker들의 속도 추정 obj_velo, dx_t, dy_t, dz_t = obj_velocity([tx_trk, ty_trk, tz_trk], bbox.label, oxtLinear) if obj_velo != None: obj_velo = np.round_(obj_velo, 1) # m/s obj_velo = obj_velo * 3.6 # km/h obj_velo_scale = convert_velo2scale(obj_velo) # # Tracker들의 Orientation q_ori = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림 obj_ori_arrow = Marker( type=Marker.ARROW, id=bbox.label, lifetime=rospy.Duration(0.2), pose=Pose(Point(tx_trk, ty_trk, tz_trk / 2.0), Quaternion(*q_ori)), scale=Vector3(obj_velo_scale, 0.5, 0.5), header=header, # color=ColorRGBA(0.0, 1.0, 0.0, 0.8)) color=bbox_color) obj_ori_arrows.markers.append(obj_ori_arrow) obj_velo_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=bbox.label, lifetime=rospy.Duration(0.5), pose=Pose(Point(tx_trk, ty_trk, tz_trk), Quaternion(0.0, -0.5, 0.0, 0.5)), scale=Vector3(1.5, 1.5, 1.5), header=header, color=ColorRGBA(1.0, 1.0, 1.0, 1.0), text="{}km/h".format(obj_velo)) velocity_markers.markers.append(obj_velo_marker) current_id_list.append(bbox.label) # Warning object line warning_line = Marker( type=Marker.LINE_LIST, id=int(obj_id), lifetime=rospy.Duration(0.2), pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)), # origin point position scale=Vector3(0.2, 0.0, 0.0), # line width header=header, color=ColorRGBA(1.0, 0.0, 0.0, 1.0)) d = dist_from_objBbox(tx_trk, ty_trk, tz_trk, size_trk.x, size_trk.y, size_trk.z) if d < MIN_WARNING_DIST: warning_line.points = Point(tx_trk, ty_trk, tz_trk), Point(0.0, 0.0, 0.0) warning_line_markers.markers.append(warning_line) # Change Outer Circle Color outer_circle_color = ColorRGBA(1.0 * 25 / 255, 1.0, 0.0, 1.0) if len(warning_line_markers.markers) > 0: outer_circle_color = ColorRGBA(1.0 * 255 / 255, 1.0 * 0 / 255, 1.0 * 0 / 255, 1.0) # ego_vehicle's warning boundary outer_circle = Marker( type=Marker.CYLINDER, id=int(obj_id), lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -2.0), Quaternion(0, 0, 0, 1)), scale=Vector3(8.0, 8.0, 0.1), # line width header=header, color=outer_circle_color) inner_circle = Marker( type=Marker.CYLINDER, id=int(obj_id), lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -1.8), Quaternion(0, 0, 0, 1)), scale=Vector3(7.0, 7.0, 0.2), # line width header=header, color=ColorRGBA(0.22, 0.22, 0.22, 1.0)) # ego-vehicle velocity selfvelo = np.sqrt(oxtLinear.x**2 + oxtLinear.y**2 + oxtLinear.z**2) selfvelo = np.round_(selfvelo, 1) # m/s selfvelo = selfvelo * 3.6 # km/h oxtAngular = TwistStamped.twist.angular q_gps = tf.transformations.quaternion_from_euler( oxtAngular.x, oxtAngular.y, oxtAngular.z) # # ego-vehicle 사진 출력 ego_car = Marker(type=Marker.MESH_RESOURCE, id=0, lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -1.8), Quaternion(0, 0, 0, 1)), scale=Vector3(1.5, 1.5, 1.5), header=header, action=Marker.ADD, mesh_resource=CAR_DAE_PATH, color=ColorRGBA(1.0, 1.0, 1.0, 1.0)) # Self ego Velocity text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(0.5), pose=Pose(Point(-7.0, 0.0, 0.0), Quaternion(0, 0, 0, 1)), scale=Vector3(1.5, 1.5, 1.5), header=header, color=ColorRGBA(1.0, 1.0, 1.0, 1.0), text="{}km/h".format(selfvelo)) for i in prior_trk_xyz.keys(): if i not in current_id_list: prior_trk_xyz.pop(i) self.pub_frame_seq.publish(overlayTxt) self.pub_boxes.publish(boxes) self.pub_pictograms.publish(texts) self.pub_selfvelo_text.publish(text_marker) # self.pub_selfveloDirection.publish(arrow_marker) self.pub_objs_ori.publish(obj_ori_arrows) self.pub_objs_velo.publish(velocity_markers) self.pub_path.publish(obj_path_markers) self.pub_warning_lines.publish(warning_line_markers) self.pub_ego_outCircle.publish(outer_circle) self.pub_ego_innerCircle.publish(inner_circle) self.pub_ego_car.publish(ego_car)
def _decompose(self, *bboxes_msgs): out_msg = BoundingBoxArray() out_msg.header = bboxes_msgs[0].header for bboxes_msg in bboxes_msgs: out_msg.boxes.extend(bboxes_msg.boxes) self._pub.publish(out_msg)