def callback(box_array):
autoware_array = DetectedObjectArray()
autoware_array.header = box_array.header
for box in box_array.boxes:
autoware_object = DetectedObject()
autoware_object.header = box.header
autoware_object.pose = box.pose
autoware_object.dimensions = box.dimensions
autoware_array.objects.append(autoware_object)
pub.publish(autoware_array)
def get_objects_and_publish(self, image):
self.seq += 1
image_pil = Image.fromarray(image)
objects = self.yolo.get_detections(image_pil)
det_obj_arr = DetectedObjectArray()
for obj in objects:
det_obj = DetectedObject()
det_obj.label = obj['label']
det_obj.score = obj['score']
det_obj.x = int(obj['x'])
det_obj.y = int(obj['y'])
det_obj.width = obj['width']
det_obj.height = obj['height']
twist = Twist()
twist.linear.x = 0.0
twist.angular.z = 0.0
det_obj.velocity = twist
det_obj_arr.objects.append(det_obj)
self.pub_2d.publish(det_obj_arr)
def on_scan(scan):
start = timeit.default_timer()
rospy.loginfo("Got scan")
gen = []
for p in pc2.read_points(scan, field_names = ("x", "y", "z", "intensity"), skip_nans=True):
gen.append(np.array([p[0], p[1], p[2], p[3]/100.0]))
gen_numpy = np.array(gen, dtype=np.float32)
front_lidar = get_filtered_lidar(gen_numpy, cnf.boundary)
bev_map = makeBEVMap(front_lidar, cnf.boundary)
bev_map = torch.from_numpy(bev_map)
with torch.no_grad():
detections, bev_map, fps = do_detect(configs, model, bev_map, is_front=True)
print(fps)
objects_msg = DetectedObjectArray()
objects_msg.header.stamp = rospy.Time.now()
objects_msg.header.frame_id = scan.header.frame_id
flag = False
for j in range(configs.num_classes):
class_name = ID_TO_CLASS_NAME[j]
if len(detections[j]) > 0:
flag = True
for det in detections[j]:
_score, _x, _y, _z, _h, _w, _l, _yaw = det
yaw = -_yaw
x = _y / cnf.BEV_HEIGHT * cnf.bound_size_x + cnf.boundary['minX']
y = _x / cnf.BEV_WIDTH * cnf.bound_size_y + cnf.boundary['minY']
z = _z + cnf.boundary['minZ']
w = _w / cnf.BEV_WIDTH * cnf.bound_size_y
l = _l / cnf.BEV_HEIGHT * cnf.bound_size_x
obj = DetectedObject()
obj.header.stamp = rospy.Time.now()
obj.header.frame_id = scan.header.frame_id
obj.score = 0.9
obj.pose_reliable = True
obj.space_frame = scan.header.frame_id
obj.label = class_name
obj.score = _score
obj.pose.position.x = x
obj.pose.position.y = y
obj.pose.position.z = z
[qx, qy, qz, qw] = euler_to_quaternion(yaw, 0, 0)
obj.pose.orientation.x = qx
obj.pose.orientation.y = qy
obj.pose.orientation.z = qz
obj.pose.orientation.w = qw
obj.dimensions.x = l
obj.dimensions.y = w
obj.dimensions.z = _h
objects_msg.objects.append(obj)
if flag is True:
pub.publish(objects_msg)
stop = timeit.default_timer()
print('Time: ', stop - start)
def create_detection_aw_msg(im, output_dict, category_index, bridge):
print 'create_detection_aw_msg'
boxes = output_dict["detection_boxes"]
scores = output_dict["detection_scores"]
classes = output_dict["detection_classes"]
msg = DetectedObjectArray()
msg.header = im.header
scores_above_threshold = np.where(scores > 0.5)[0]
KK = 0
for s in scores_above_threshold:
# Get the properties
KK = KK + 1
bb = boxes[s, :]
sc = scores[s]
cl = classes[s]
# Create the detection message
detection = DetectedObject()
detection.header = im.header
detection.label = category_index[int(cl)]['name']
detection.score = sc
detection.color.r = 1.0
detection.color.g = 1.0
detection.color.b = 1.0
detection.color.a = 1.0
detection.image_frame = im.header.frame_id
detection.x = int((im.width - 1) * bb[1])
detection.y = int((im.height - 1) * bb[0])
detection.width = int((im.width - 1) * (bb[3] - bb[1]))
detection.height = int((im.height - 1) * (bb[2] - bb[0]))
msg.objects.append(detection)
return msg, KK
def __init__(self, local_frame, global_frame, pose):
# TF
self.global_frame = global_frame
self.local_frame = local_frame
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.trans = None
#
self.pub_dummy_obstacle = rospy.Publisher(
"detection/lidar_detector/objects",
DetectedObjectArray,
queue_size=1)
self.obstacle_msg = DetectedObjectArray()
self.obstacle_msg.header.frame_id = self.local_frame
self.object = DetectedObject()
self.pose = pose
self.object.pose = pose.pose
self.obstacle_msg.objects.append(self.object)
def __init__(self):
self.sensor_frame = "left_os1/os1_sensor"
self.loadTestCase("./test-cases/test1.json")
#
self.pub_fake_object = rospy.Publisher(
"/detection/lidar_detector/objects",
DetectedObjectArray,
queue_size=10)
self.sub_current_pose = rospy.Subscriber("/current_pose", PoseStamped,
self.cbCurrentPose)
# We assume circular obstacle
self.msg_pub_fake = DetectedObjectArray()
self.marker_array_viz = MarkerArray()
self.msg_pub_fake.header.frame_id = self.sensor_frame
self.buffer = tf2_ros.Buffer(rospy.Duration(10.0))
self.listener = tf2_ros.TransformListener(self.buffer)
# Timer to publish
self.obst_timer = rospy.Timer(
rospy.Duration(1.0 / OBSTACLE_PUBLISH_HZ),
self.cbTimerDetectorObjectPublish)
def second_inference(header, points, measure_time):
t0 = time.time()
m = voxel_generator.generate(points, max_voxels=60000)
voxels = m['voxels']
coords = m['coordinates']
num_points = m['num_points_per_voxel']
# add batch idx to coords
coords = np.pad(coords, ((0, 0), (1, 0)),
mode='constant',
constant_values=0)
voxels = torch.tensor(voxels, dtype=torch.float32, device=device)
coords = torch.tensor(coords, dtype=torch.int32, device=device)
num_points = torch.tensor(num_points, dtype=torch.int32, device=device)
# Detection
example = {
"anchors": anchors,
"voxels": voxels,
"num_points": num_points,
"coordinates": coords,
}
t1 = time.time()
pred = net(example)[0]
# print(pred)
t2 = time.time()
# Simple Vis-- color the points of detected object
# pred = pred
boxes_lidar = pred["box3d_lidar"].detach().cpu().numpy() # tuple
class_names = pred["label_preds"].detach().cpu().numpy() # tuple
# class_names_str = ['car','pedestrian','cyclist','van'] # KITTI
class_names_str = [
'car', 'bicycle', 'bus', 'construction_vehicle', 'motorcycle',
'pedestrian', 'traffic_cone', 'trailer', 'truck', 'barrier'
] # nuscenes
### convert box to ros msg
detect_object_array = DetectedObjectArray()
detect_object_array.header = header
print('object nums', len(class_names))
if len(class_names) != 0:
for i in range(len(class_names)):
# if class_names[i] != 0:
# continue
detect_object = DetectedObject()
detect_object.header = header
detect_object.valid = True
detect_object.pose_reliable = True
detect_object.velocity_reliable = False
detect_object.acceleration_reliable = False
detect_object.label = class_names_str[class_names[i]]
detect_object.pose.position.x = float(
boxes_lidar[i][0]) # 需要确认一下是不是0
detect_object.pose.position.y = float(
boxes_lidar[i][1]) # 需要确认一下是不是0
detect_object.pose.position.z = float(
boxes_lidar[i][2]) # 需要确认一下是不是0
detect_object.dimensions.x = float(boxes_lidar[i][3])
detect_object.dimensions.y = float(boxes_lidar[i][4])
detect_object.dimensions.z = float(boxes_lidar[i][5])
q = quaternion_from_euler(0, 0, -float(boxes_lidar[i][6]))
detect_object.pose.orientation.x = q[0]
detect_object.pose.orientation.y = q[1]
detect_object.pose.orientation.z = q[2]
detect_object.pose.orientation.w = q[3]
detect_object_array.objects.append(detect_object)
t3 = time.time()
if measure_time:
print(f" Preprocess time = {(t1-t0)* 1000:.3f} ms")
print(f" Second network detection time = {(t2-t1)* 1000:.3f} ms")
print(f" convert box to ros msg = {(t3-t2)* 1000:.3f} ms")
return detect_object_array
pred_boxes = np.tile(boxes, (1, scores.shape[1]))
# pred_boxes = boxes
pred_boxes /= im_scales[0]
scores = scores.squeeze()
pred_boxes = pred_boxes.squeeze()
det_toc = time.time()
detect_time = det_toc - det_tic
misc_tic = time.time()
# if vis:
im2show = np.copy(im)
### convert box to ros msg
detect_object_array = DetectedObjectArray()
detect_object_array.header = header
for j in xrange(1, len(pascal_classes)):
inds = torch.nonzero(scores[:, j] > thresh).view(-1)
# if there is det
if inds.numel() > 0:
cls_scores = scores[:, j][inds]
_, order = torch.sort(cls_scores, 0, True)
if args.class_agnostic:
cls_boxes = pred_boxes[inds, :]
else:
cls_boxes = pred_boxes[inds][:, j * 4:(j + 1) * 4]
cls_dets = torch.cat((cls_boxes, cls_scores.unsqueeze(1)), 1)
# cls_dets = torch.cat((cls_boxes, cls_scores), 1)
def velo_callback(msg):
global proc
global seq_num
time_start = rospy.get_rostime()
callback_time = rospy.get_rostime() - msg.header.stamp
callback_time_secs = callback_time.secs + callback_time.nsecs/1e9
print("Time between message publish and callback:", callback_time_secs, "seconds")
arr_bbox = BoundingBoxArray()
arr_dobject = DetectedObjectArray()
#pcl_msg = pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z","intensity","ring"))
#pcl_msg = pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z"))
#np_p = np.array(list(pcl_msg), dtype=np.float32)
np_p = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
#print("np_p shape: ", np_p.shape)
#np_p = np.delete(np_p, -1, 1) # delete "ring" field
# convert to xyzi point cloud
x = np_p[:, 0].reshape(-1)
y = np_p[:, 1].reshape(-1)
z = np_p[:, 2].reshape(-1)
if np_p.shape[1] == 4: # if intensity field exists
i = np_p[:, 3].reshape(-1)
else:
i = np.zeros((np_p.shape[0], 1)).reshape(-1)
cloud = np.stack((x, y, z, i)).T
#print("cloud[0]:",cloud[0,0])
# start processing
dt_boxes_corners, scores, dt_box_lidar, label = proc.run(cloud)
#print("dt_boxes_corners :",dt_boxes_corners)
# print(scores)
# # field of view cut
cond = hv_in_range(x=np_p[:, 0],
y=np_p[:, 1],
z=np_p[:, 2],
fov=[-90, 90],
fov_type='h')
cloud_ranged = cloud[cond]
cloud_ranged_pc2 = xyz_array_to_pointcloud2(cloud_ranged[:,:3],frame_id="velodyne") ##pay attention here, xyz_array_to_pointcloud2 only works for nX3 array!!!
# # print(cond.shape, np_p.shape, cloud.shape)
# publish to /velodyne_poitns_modified
# publish_test(cloud_ranged, msg.header.frame_id)
#publish_test(proc.inputs['points'][0], msg.header.frame_id)
#print("shape for points_display", points_display[0].shape)
#publish_test(points_display[0], msg.header.frame_id)
label_n = []
for la,la2 in enumerate(label):
#print("la is:",la)
#print("la2 is:",la2)
if la2 == 'Car':
label_n.append(1)
elif la2 == 'Cyclist':
label_n.append(2)
elif la2 == 'Pedestrian':
label_n.append(3)
else:
label_n.append(4)
# process results
#print("label_n is ",label_n)
end_time_1 = rospy.get_rostime() - time_start
end_time_1_sec = end_time_1.secs + end_time_1.nsecs/1e9
print("end_time_1 is :",end_time_1_sec)
segment_index = box_np_ops.points_in_rbbox(cloud, dt_box_lidar, lidar=True)
end_time_2 = rospy.get_rostime() - time_start
end_time_2_sec = end_time_2.secs + end_time_2.nsecs/1e9
print("end_time_2 is :",end_time_2_sec)
#print("shape of segment_index is",segment_index.shape)
#print("segment_index is", segment_index)
#print(type(cloud))
#segment_cloud = np.matmul(cloud.T,segment_index)
segment_cloud_agg = np.array([])
seq_num = seq_num + 1
if scores.size != 0:
# print('Number of detections: ', results['name'].size)
for i in range(scores.size):
bbox = BoundingBox()
dobject = DetectedObject()
#DetectedObject, DetectedObjectArray
#box_test = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
dobject.header.frame_id = msg.header.frame_id
#box_test.header.frame_id = msg.header.frame_id
# bbox.header.stamp = rospy.Time.now()
q = quaternion_from_euler(0,0,-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]+dt_box_lidar[i][5]/2)
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.label = label_n[i]
dobject.header = msg.header
dobject.header.seq = seq_num
#dobject.id = i
#dobject.header.label = label[i]
#dobject.header.score = scores[i]
dobject.label = label[i]
dobject.score = 1.
dobject.space_frame = "velodyne"
dobject.pose = bbox.pose
dobject.dimensions = bbox.dimensions
segment_cloud = cloud[segment_index[:,i],:3] #pay attention here, xyz_array_to_pointcloud2 only works for nX3 array!!!
#print("segment_cloud",segment_cloud)
#print("shape_segment_pc:", segment_cloud.shape)
segment_cloud_pc2 = xyz_array_to_pointcloud2(segment_cloud)
#segment_cloud = cloud
#segment_cloud_pc2 = pc2.create_cloud(header=msg.header,
# fields=_make_point_field(4), # 4 PointFields as channel description
# points=segment_cloud)
#dobject.pointcloud = segment_cloud_pc2
dobject.valid = True
#print("shape of segment_cloud",segment_cloud.shape)
#if segment_cloud_agg.size == 0:
# segment_cloud_agg = segment_cloud
#else:
# segment_cloud_agg = np.append(segment_cloud_agg,segment_cloud,axis=0)
#box_test.pose.orientation.x = q[0]
#box_test.pose.orientation.x = q[1]
#box_test.pose.orientation.x = q[2]
#box_test.pose.orientation.x = q[3]
#box_test.pose.position.x = 5
#box_test.pose.position.y = 0
#box_test.pose.position.z = -1
#box_test.dimensions.x = float(dt_box_lidar[i][3])
#box_test.dimensions.y = float(dt_box_lidar[i][4])
#box_test.dimensions.z = float(dt_box_lidar[i][5])
arr_bbox.boxes.append(bbox)
arr_dobject.objects.append(dobject)
arr_bbox.header.frame_id = msg.header.frame_id
arr_dobject.header = msg.header
arr_dobject.header.frame_id = msg.header.frame_id
#arr_dobject.header = msg.header.frame_id
# arr_bbox.header.stamp = rospy.Time.now()
#print("arr_bbox.boxes.size() : {} ".format(len(arr_bbox.boxes)))
#if len(arr_bbox.boxes) is not 0:
# for i in range(0, len(arr_bbox.boxes)):
# print("[+] [x,y,z,dx,dy,dz] : {}, {}, {}, {}, {}, {}".\
# format(arr_bbox.boxes[i].pose.position.x,arr_bbox.boxes[i].pose.position.y,arr_bbox.boxes[i].pose.position.z,\
# arr_bbox.boxes[i].dimensions.x,arr_bbox.boxes[i].dimensions.y,arr_bbox.boxes[i].dimensions.z))
# publish to /voxelnet_arr_bbox
end_time_3 = rospy.get_rostime() - time_start
end_time_3_sec = end_time_3.secs + end_time_3.nsecs/1e9
print("end_time_3 is :",end_time_3_sec)
pub_arr_bbox.publish(arr_bbox)
pub_arr_dobject.publish(arr_dobject)
pub_ranged_cloud.publish(cloud_ranged_pc2)
#print("size of segment_cloud_agg",segment)
#publish_segment(segment_cloud_agg,msg.header.frame_id)
#pub_test_box.publish(box_test)
#arr_bbox.boxes.clear()
arr_bbox.boxes = []
arr_dobject.objects = []