def votenet_callback(req):
points = pointcloud2_to_array(req.pc)
result_points, pred_map_cls = point_cloud_to_detections(points)
pc = array_to_pointcloud2(
np.asarray(result_points, dtype=[('x', '<f4'), ('y', '<f4'), ('z', '<f4')]))
msg = VotenetResponse(pc=pc)
return msg
def read_pointcloud_frames(topic_name, bagfile):
'''returns a list of tuples: (stamp,numpy_array) where stamp is the time and numpy_array is a labelled array with the pointcloud data'''
frames = []
for topic, msg, t in bagfile.read_messages(topics=[topic_name]):
pc_array = point_cloud2.pointcloud2_to_array(msg)
frames.append((msg.header.stamp, pc_array))
return frames
def save_cloud(self, seq):
cloud_img = get_xyz_points(pointcloud2_to_array(self.point_cloud),
remove_nans=False,
dtype=np.float32)
# DO NOT COPY THE POINT CLOUD ANYMORE. ALWAYS USES RESHAPED VIEWS
cloud_xyz = cloud_img.view()
cloud_xyz.shape = (-1, 3)
cloud = pcl.PointCloud(cloud_xyz)
path = os.path.join(self.folder, '{:04d}_cloud.pcd'.format(seq))
cloud.to_file(path)
def are_clouds_equal(self, ros_cloud, open3d_cloud):
open3d_xyz_asarray = np.asarray(open3d_cloud.points)
ros_xyz_asarray = pointcloud2_to_xyz_array(ros_cloud)
self.assertEqual(1, ros_cloud.height)
self.assertEqual(ros_cloud.width, open3d_xyz_asarray.shape[0])
self.assertTrue(np.all(np.isclose(open3d_xyz_asarray,
ros_xyz_asarray)))
if open3d_cloud.has_colors():
open3d_colors_asarray = np.asarray(open3d_cloud.colors) * 255
ros_colors_asarray = get_rgb_points(
split_rgb_field(pointcloud2_to_array(ros_cloud)))
self.assertTrue(
np.all(np.isclose(open3d_colors_asarray, ros_colors_asarray)))
def process(self, pc_msg, objects_msg):
pc = pointcloud2_to_array(pc_msg)
num_mask = np.isfinite(pc['x']) \
& np.isfinite(pc['y']) \
& np.isfinite(pc['z'])
self.track_prev = {}
for track_id in self.track_current:
self.track_prev[track_id] = self.track_current[track_id]
self.track_current = {}
for object_msg in objects_msg.objects:
if object_msg.track_id < 1:
continue
obj_mask = rle_msg_to_mask(object_msg.rle).astype(np.bool)
obj_pc = s2u(pc[num_mask & obj_mask][['x', 'y', 'z']])
if len(obj_pc) < 50:
continue
local_center = clastering(obj_pc)
self.track_current[object_msg.track_id] = {
'n_frame': self.n_frame,
'center': local_center,
}
all_markers = create_marker_array(self.track_current, self.colors,
pc_msg.header, 1., 0)
all_markers.extend(
create_marker_array(self.track_prev, self.colors, pc_msg.header,
1.2, len(self.track_current)))
self.centers_pub.publish(MarkerArray(all_markers))
self.br.publish(self.n_frame, pc_msg.header)
self.n_frame += 1
def callbackROS(velo_ros):
global index
if index in toLook:
arr = rospc2.pointcloud2_to_array(velo_ros)
velo = get_xyzi_points(arr)
#velo = passthrough(velo, 0, 70, -40, 40, -3.0, 3.5)
intensities = velo[:, 3]
xyz = np.hstack((velo[:, 0:3], np.ones((velo.shape[0], 1))))
#xyz = xyz.dot(T_cam_velo.T)
#t = np.copy(xyz)
#xyz[:,0] = t[:,2]
#xyz[:,1] = t[:,0]
#xyz[:,2] = t[:,1]
xyz[:, 3] = intensities / 255
indexs = int(np.where(toLook == index)[0])
indexs = str(indexs).zfill(4)
xyz.reshape((xyz.size)) #one dimension
xyz.astype(np.float32).tofile(folder + 'pc' + indexs + '.bin')
index += 1
def main():
listener = RsListener()
rrate = rospy.Rate(100)
try:
while not rospy.is_shutdown():
if listener.pcmsg is not None:
cv_image = listener.bridge.imgmsg_to_cv2(
listener.imgmsg, desired_encoding='passthrough')
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
tags = listener.detector.detect(gray)
if len(tags) > 0:
# try:
corners = np.transpose(tags[0].corners)
# print("found apriltags:")
# print("no: ", len(tags))
# print(tags)
# print(corners)
x = corners[0, :]
y = corners[1, :]
x = [int(temp) for temp in x]
y = [int(temp) for temp in y]
cx = int(tags[0].center[0])
cy = int(tags[0].center[1])
x.append(cx)
y.append(cy)
points = []
pcarray = pc2_np.pointcloud2_to_array(listener.pcmsg)
points = []
for ix, iy in zip(x, y):
p = []
ptem = pcarray[iy, ix]
for idim in range(3):
p.append(ptem[idim])
# p = pcarray[iy, ix][0:3]
points.append(p)
print(points)
if not listener.points:
listener.points = [points]
listener.tfs = [listener.tf]
else:
listener.points.append(points)
listener.tfs.append(listener.tf)
listener.tf = None
listener.flag_record = False
# except IndexError:
else:
listener.flag_record = True
pass
listener.pcmsg = None
listener.imgmsg = None
else:
rrate.sleep()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if i == 0:
continue
bag_in = rosbag.Bag(sys.argv[i], 'r')
name = sys.argv[i][:sys.argv[i].rindex(".bag")]
bag_out = rosbag.Bag(name + "-1700points_per_scan-4D.bag", 'w')
for topic, msg, time in bag_in.read_messages():
'''if rospy.is_shutdown():
break'''
'''if topic == "/tf":
for tr in msg.transforms:
tr.header.stamp = rospy.Time.now()
pub_tf.publish(msg)'''
if topic == "/camera/depth/points":
point_cloud = msg
pts = point_cloud2.pointcloud2_to_array(point_cloud)
d = {}
for raw in pts:
for pt in raw:
pt = np.array([pt[0], pt[1], pt[2], 1])
if isnan(pt[0]) or isnan(pt[1]) or isnan(pt[2]):
continue
cell = world_to_cell(pt, 0.05)
if cell in d:
d[cell] = np.append(d[cell], pt)
else:
d[cell] = np.array([pt])
float_array = np.array([])
'''marker_msg = Marker()
marker_msg.header.stamp = rospy.Time.now()
marker_msg.header.frame_id = point_cloud.header.frame_id
def lidar_callback(self, msg):
pc_arr = point_cloud2.pointcloud2_to_array(msg)
pc_arr = structured_to_unstructured(pc_arr)
#print(pc_arr.shape)
pc_arr = pc_arr.reshape(-1, 4)
lidar_boxes, lidar_scores = self.model.predict(pc_arr)
#print(lidar_boxes)
# points.dtype=[('x', np.float32),('y', np.float32),('z', np.float32),('intensity', np.float32)]
# cloud_msg=point_cloud2.array_to_pointcloud2(points,rospy.Time.now(),"rslidar")
if lidar_boxes is not None:
num_detects = len(lidar_boxes) #if len(lidar_boxes)<=10 else 10
arr_bbox = BoundingBoxArray()
arr_score = MarkerArray()
for i in range(num_detects):
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
bbox.header.stamp = rospy.Time.now()
bbox.pose.position.x = float(lidar_boxes[i][0])
bbox.pose.position.y = float(lidar_boxes[i][1])
#bbox.pose.position.z = float(lidar_boxes[i][2])
bbox.pose.position.z = float(
lidar_boxes[i][2]) + float(lidar_boxes[i][5]) / 2
bbox.dimensions.x = float(lidar_boxes[i][3]) # width
bbox.dimensions.y = float(lidar_boxes[i][4]) # length
bbox.dimensions.z = float(lidar_boxes[i][5]) # height
q = Quaternion(axis=(0, 0, 1),
radians=float(-lidar_boxes[i][6]))
bbox.pose.orientation.x = q.x
bbox.pose.orientation.y = q.y
bbox.pose.orientation.z = q.z
bbox.pose.orientation.w = q.w
arr_bbox.boxes.append(bbox)
marker = Marker()
marker.header.frame_id = msg.header.frame_id
marker.header.stamp = rospy.Time.now()
marker.ns = "basic_shapes"
marker.id = i
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(0.15)
marker.scale.x = 4
marker.scale.y = 4
marker.scale.z = 4
# Marker的颜色和透明度
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1
marker.color.a = 1
marker.pose.position.x = float(lidar_boxes[i][0])
marker.pose.position.y = float(lidar_boxes[i][1])
marker.pose.position.z = float(
lidar_boxes[i][2]) + float(lidar_boxes[i][5]) / 2
marker.text = str(np.around(lidar_scores[i], 2))
arr_score.markers.append(marker)
arr_bbox.header.frame_id = msg.header.frame_id
arr_bbox.header.stamp = rospy.Time.now()
print("Number of detections: {}".format(num_detects))
self.pub_bbox.publish(arr_bbox)
self.pub_text.publish(arr_score)
def pointcloud2_to_array_xyz(pc):
points = pointcloud2_to_array(pc)
xyzs = np.vstack((points['x'], points['y'], points['z'])).T
return xyzs
def getVelodyne(self, msg):
# Recebendo odometria
teste = pc2.pointcloud2_to_array(msg)
x = []
y = []
z = []
# Filtrando dados (pontos) para diminuir quantidade de processamento
for i, j in enumerate(teste['x']):
if i%5 == 0:
x.append(j)
for i, j in enumerate(teste['y']):
if i%5 == 0:
y.append(j)
for i, j in enumerate(teste['z']):
if i%5 == 0:
z.append(j)
# Transformando valores em um array
teste_menor = np.array([(i, j, k) for i, j, k in zip(x, y, z)], teste.dtype)
# Criando dataframe para armazenar os dados
df = pd.DataFrame(teste_menor, columns=['x','y','z'])
print('--------------------------------------------------------------------------------------')
df = df[df['z'] > 0.2]
# Definindo tolerancias para os filtros
tolerance = 0.1
negative_tolerance = -0.1
# Filtrando menor distancia frontal
self.frente = np.nan
distancia_frontal = df[df['y'] < tolerance]
distancia_frontal = distancia_frontal[distancia_frontal['y'] > negative_tolerance]
distancia_frontal = distancia_frontal[distancia_frontal['x'] > negative_tolerance]
print('Distancia frontal: ')
if(distancia_frontal.isnull().values.all()):
print('Nada a frente')
self.frente = np.nan
else:
distancia_frontal.reset_index(drop=True, inplace=True)
value = distancia_frontal[['x']].idxmin()
distancia_frontal = distancia_frontal.iloc[value][:]
self.frente = distancia_frontal.at[value[0],'x']
print(self.frente)
print('')
# Filtrando menor distancia da direita (90 graus)
self.direita = np.nan
distancia_direita = df[df['x'] < tolerance]
distancia_direita = distancia_direita[distancia_direita['y'] < 0]
distancia_direita = distancia_direita[distancia_direita['x'] > negative_tolerance]
print('Distancia direita: ')
if(distancia_direita.isnull().values.all()):
print('Nada a direita')
self.direita = np.nan
else:
distancia_direita.reset_index(drop=True, inplace=True)
value = distancia_direita[['y']].idxmin()
distancia_direita = distancia_direita.iloc[value][:]
self.direita = distancia_direita.at[value[0],'y']
self.direita = abs(self.direita)
print(self.direita)
print('')
# Filtrando menor distancia da esquerda (90 graus)
self.esquerda = np.nan
distancia_esquerda = df[df['x'] < tolerance]
distancia_esquerda = distancia_esquerda[distancia_esquerda['y'] > 0]
distancia_esquerda = distancia_esquerda[distancia_esquerda['x'] > negative_tolerance]
print('Distancia esquerda: ')
if(distancia_esquerda.isnull().values.all()):
print('Nada a esquerda')
self.esquerda = np.nan
else:
distancia_esquerda.reset_index(drop=True, inplace=True)
value = distancia_esquerda[['y']].idxmin()
distancia_esquerda = distancia_esquerda.iloc[value][:]
self.esquerda = distancia_esquerda.at[value[0],'y']
print(self.esquerda)
print('')
# Filtrando distancia diagonal direita (45 graus)
#self.s_direita = np.nan
dd_direita = df.loc[((abs(df['x'] + df['y'])) <= (tolerance)) & df['x'] > 0]
print('Distancia diagonal direita: ')
if(dd_direita.isnull().values.all()):
print('Nada na diagonal direita')
#self.s_direita = np.nan
else:
dd_direita.reset_index(drop=True, inplace=True)
value = dd_direita[['x']].idxmin()
dd_direita = dd_direita.iloc[value][:]
#print(dd_direita.at[value[0],'x'])
#x = dd_direita.at[value[0],'x']**2
self.s_direita = dd_direita.at[value[0],'y']
self.s_direita = abs(self.s_direita)
#soma = x + y
#hip_direita = math.sqrt(soma)
print(self.s_direita)
print('')
# Filtrando distancia diagonal esquerda (45 graus)
#self.s_direita = np.nan
dd_esquerda = df.loc[((abs(df['x'] - 2*df['y'])) <= (tolerance)) & df['x'] > 0]
print('Distancia diagonal esquerda: ')
if(dd_esquerda.isnull().values.all()):
print('Nada na diagonal esquerda')
#self.s_direita = np.nan
else:
dd_esquerda.reset_index(drop=True, inplace=True)
value = dd_esquerda[['x']].idxmin()
dd_esquerda = dd_esquerda.iloc[value][:]
#print(dd_esquerda.at[value[0],'x'])
#x = dd_esquerda.at[value[0],'x']**2
self.s_esquerda = dd_esquerda.at[value[0],'y']
#soma = x + y
#hip_esquerda = math.sqrt(soma)
#print(hip_esquerda)
print(self.s_esquerda)
print('')
