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 parseYoloBoxes(msg): global img, img_lock, lidar_array, fx, cx ly_array = Multi_targets() ly_array.data = lidar_array.data[:] #global yolobox #print 2 #print data.obj.x #pubbox.publish(yolo_box) if len(msg.obj) > 0: # print msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs img_local = None # copy the image, get the lock if (img is not None) and (img_lock == False): img_lock = True img_local = np.copy(img) img_lock = False else: print 'Waiting for image.' # overlay the boxes on the image and publish if img_local is not None: color = (0,0,0) if msg.type == 'person': color = (255,0,0) else: color = (0,0,255) for obj in msg.obj: yolo_t = target() x = obj.x y = obj.y w = obj.width h = obj.height s = obj.score # yolo_t.pos_x = x # yolo_t.pos_y = y # yolo_t.counter = s # if msg.type == 'person': # yolo_t.category = 0 # else: # yolo_t.category = 1 #calculate yolo angle yolo_angle = np.arctan(fx/((x+w/2)-cx)) if yolo_angle < 0: yolo_angle = yolo_angle + 3.1415 ly_angles = [] #find the angle diff with every object in ly_array for i in range(len(ly_array.data)): ly_x = ly_array.data[i].pos_x ly_y = ly_array.data[i].pos_y ly_angle = np.arctan(ly_y/ly_x) if ly_angle < 0: ly_angle = ly_angle + 3.1415 ly_angles.append(ly_angle) ly_angle_diff = [abs(angle - yolo_angle) for angle in ly_angles] # replace label for nearest object within 3 degrees and counter less that .25 # update score obj_i = ly_angle_diff.index(min(ly_angle_diff)) if (min(ly_angle_diff)<0.1): if msg.type=='person': ly_array.data[obj_i].category = 1 # else: # lidar_array.data[obj_i].category = 2 ly_array.data[obj_i].counter = s # print 'type: %s, x: %d, y: %d, h: %d, w: %d, score: %f\n' % (msg.type,x,y,h,w,s) cv2.rectangle(img_local, (x,y), (x+w, y+h), color = color) img_msg = CvBridge().cv2_to_imgmsg(img_local, encoding="bgr8") pub_img_proc.publish(img_msg) # pub_ped.publish(data_ped) pub_ly_fuse.publish(ly_array)
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 main():
global camera_list_targets, radar_list_targets, previous_camera_list_targets, previous_radar_list_targets
rospy.init_node('listener')
rospy.Subscriber("radar_targets", Float32MultiArray, callback_radar)
rospy.Subscriber("camera_targets", Float32MultiArray, callback_camera)
rospy.Subscriber("/vehicle/wheel_speeds", WheelSpeedReport, callback_wheel)
# ------------------------------------------------------------
# Initialize the figure
# f, ax = plt.subplots(1)
# plt.ion()
# ------------------------------------------------------------
rate = rospy.Rate(20)
while not rospy.is_shutdown():
# Refresh plot
# ax.clear()
# ------------------------------------------------------------
# If no targets detected, the global variables are not updated so
# they stay equal to they previous value. So if that happens, we set
# them to None. Maybe better to do an empty tuple instead.
if camera_list_targets == previous_camera_list_targets:
camera_list_targets = None
if radar_list_targets == previous_radar_list_targets:
radar_list_targets = None
# ------------------------------------------------------------
# Reshape the lists to make it easier to work.
# We only reshape if it's not a None, of course
# We change the name (ex: radar_list_targets_matrix to still be able to compare with previous value at the end of the main
radar_list_targets_matrix = []
camera_list_targets_matrix = []
if radar_list_targets != None:
radar_list_targets_matrix = np.array(radar_list_targets).reshape(
len(radar_list_targets) / 4,
4) # Number of rows and columns (x,y,speed,label)
if camera_list_targets != None:
camera_list_targets_matrix = np.array(camera_list_targets).reshape(
len(camera_list_targets) / 6, 6
) # Number of rows and columns (x,y,speed). 6 is the number of info from camera we have (x,y, speed, label, age, lane)
# ------------------------------------------------------------
# Here we do the fusion. We go through all the points and:
# -> Calculate the distance between all the camera points with the radar points
# -> If they are close, we do an average
# -> If not close, we keep them both
# We append everything in a new matrix
# LABEL: 1.0 = Car
# 2.0 = Unknown
all_targets = []
min_distance = 1.0 #Min distance between two points to be considered the same points
# Create an object of type Multi_targets() -> Will store all the detected targets (of type target)
target_array = Multi_targets()
targets_cars = Multi_targets()
global iteration
global list_cam_speed
global list_radar_speed
global list_wheel_speed
global previous_v_avg
previous_v_avg = 0 # Maybe change that if we start with an initial velocity very different from zero
for i in range(0,
len(camera_list_targets_matrix)): #camera targets loop
#print 'i = ', i
close_points = []
x_cam = camera_list_targets_matrix[i][0]
y_cam = camera_list_targets_matrix[i][1]
v_cam = camera_list_targets_matrix[i][2] #Speed
label_cam = camera_list_targets_matrix[i][3]
age_cam = camera_list_targets_matrix[i][4]
lane_cam = camera_list_targets_matrix[i][5]
close_points = np.append(close_points, [x_cam, y_cam, v_cam])
if lane_cam == 0:
for j in range(
0,
len(radar_list_targets_matrix)): #radar targets loop
x_radar = radar_list_targets_matrix[j][0]
y_radar = radar_list_targets_matrix[j][1]
v_radar = radar_list_targets_matrix[j][2] #Speed
label_radar = radar_list_targets_matrix[j][3]
distance = np.sqrt((x_radar - x_cam)**2 +
(y_radar - y_cam)**2)
if distance < min_distance:
close_points = np.append(close_points,
[x_radar, y_radar, v_radar])
# ---------------
# Plots for debugging
#list_cam_speed = list_cam_speed + [v_cam]
#list_radar_speed = list_radar_speed + [v_radar]
#plt.plot(list_cam_speed,'b')
#plt.plot(list_radar_speed,'r')
#plt.pause(0.001)
elif distance > min_distance:
#print 'No Fusion !'
# We add the label (1.0 = car, 2.0 = unknown)
b = np.append(radar_list_targets_matrix[j], 2.0)
all_targets.append(list(b))
target_far = target()
target_far.pos_x = x_radar
target_far.pos_y = y_radar
target_far.speed = v_radar
target_far.category = 2
target_far.counter = j
target_array.data.append(target_far)
matrix_close_points = np.reshape(close_points,
(len(close_points) / 3, 3))
x_avg = np.mean(matrix_close_points[:, 0])
y_avg = np.mean(matrix_close_points[:, 1])
v_avg = np.mean(matrix_close_points[:, 2])
# ---------------------
# This is for publishing
target_avg = target()
target_avg.pos_x = x_avg
target_avg.pos_y = y_avg
target_avg.category = 1
target_avg.counter = i # Not relevant to put i
#print 'speed diff = ', abs(v_avg - previous_v_avg)
# Remove incoherent points
if (abs(v_avg - previous_v_avg) < 10):
target_avg.speed = v_avg
previous_v_avg = v_avg
else:
target_avg.speedd = previous_v_avg
# Put that in the array of detected cars
targets_cars.data.append(target_avg)
# Append the avg point (the points close to car) in the target array
target_array.data.append(target_avg)
# ---------------------
# This is for plotting
# We average and add the label (1.0 = car)
point_average = [x_avg, y_avg, v_avg, 1.0]
all_targets.append(point_average)
# --------------------
# Debugging plot
# NAME IS WRONG BUT WE ACTUALLY PLOT THE SPEED FOR ACC (AVG SPEED)
#list_cam_speed = list_cam_speed + [target_avg.speed]
#plt.plot(list_cam_speed,'b')
#plt.pause(0.001) # Do I need this ?
# --------------------
# Publish for ACC
pub_acc.publish(targets_cars)
pub_all_targets.publish(target_array)
# ------------------------------------------------------------
# Here we plot the targets
# Change the indexes !
#print type(all_targets)
#print all_targets
#print ' '
#column_of_x = [i[0] for i in all_targets]
#column_of_y = [i[1] for i in all_targets]
#ax.scatter(column_of_x, column_of_y,color='green', marker='.')
# ------------------------------------------------------------
# Plot test
# for i in range(len(all_targets)):
# if all_targets[i][3] == 1.0:
# ax.scatter(all_targets[i][0], all_targets[i][1], color = 'green', marker = 'o')
# else:
# ax.scatter(all_targets[i][0], all_targets[i][1], color = 'red', marker = 'o')
# End plot test
# ------------------------------------------------------------
# plt.xlim([-20,20])
# plt.ylim([-1,40])
# plt.grid()
# f.canvas.draw() # Do I need this ?
# plt.legend(loc='upper left')
plt.grid()
# plt.pause(0.001) # Do I need this ?
# ------------------------------------------------------------
# Update the new previous global variables
previous_camera_list_targets = camera_list_targets
previous_radar_list_targets = radar_list_targets
# ------------------------------------------------------------
rate.sleep()
rospy.spin()
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 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 print data.boxes[i] # If the target is in front of the car if (y<50 and y>0) and (x<2.5 and x>-2.5): data_follow.boxes.append(data.boxes[i])