Projets \ Automatic and manual LIDAR road labels 2019 \ Guyancourt \ Guyancourt files \ rvizDisplay.py

Nom : rvizDisplay.py (rvizdisplay.py)

Description :

Python script

Créateur : Edouard Capellier 

Taille : 2.03Ko

Création : 08/02/2021 10:20

Licence : Creative Commons (CC BY 4.0)

import rospy
import numpy as np
from sensor_msgs.msg import PointCloud2, PointField
import glob
import os

NODE_NAME = "lidar_road_labels"
OUT_TOPIC = "labelled_scan"
FRAME_NAME = "velodyne"
HEIGHT = 32
WIDTH = 1800

rospy.init_node(NODE_NAME)
pc_pub = rospy.Publisher(OUT_TOPIC, PointCloud2, queue_size=10)

msg = PointCloud2()
msg.header.frame_id = FRAME_NAME
msg.fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1),
              PointField('ro', 12, PointField.FLOAT32, 1),
              PointField('phi', 16, PointField.FLOAT32, 1),
              PointField('theta', 20, PointField.FLOAT32, 1),
              PointField('intensity', 24, PointField.FLOAT32, 1),
              PointField('validity', 28, PointField.FLOAT32, 1),
              PointField('label', 32, PointField.FLOAT32, 1)] 

msg.point_step = 36
msg.is_bigendian = False
msg.is_dense = True
msg.height, msg.width = HEIGHT, WIDTH
msg.row_step = msg.point_step * msg.width

locations = os.listdir("./")

locationsFinal = list(locations)

print("Available locations:")
idx = 0

for location in locations:
	if(os.path.isdir(os.path.join("./",location))):
		idx = idx+1
		print(str(idx)+". " + location)
	else:
		locationsFinal.remove(location)

n = 0

print("Type the index of the location you want to display")

while((n<1) or (n>len(locationsFinal))):
        valueFromUser = raw_input()

	if(valueFromUser.isdigit()):
		n = int(valueFromUser)
	else:
		n = 0
	if((n<1) or (n>len(locationsFinal))):
		print("Invalid value entered. Please enter a valid location index.")

print("Selected "+locationsFinal[n-1]+". Iterating over the labelled scans.")

scans = os.listdir("./"+locationsFinal[n-1]+"/scans")

for scan in scans:
	print("Displaying scan "+scan[:-4])
	msg.data = np.load("./"+locationsFinal[n-1]+"/scans/"+scan)[:,12:1812,:].tostring()
	msg.header.stamp = rospy.Time.now()
	pc_pub.publish(msg)
	raw_input("Press enter for the next scan")
	

Télécharger Retour