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 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(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)
