-
Notifications
You must be signed in to change notification settings - Fork 0
/
publish_pointclouds_from_folder_process_script.py
204 lines (170 loc) · 7.2 KB
/
publish_pointclouds_from_folder_process_script.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
#!/usr/bin/env python
#
# Copyright (c) 2024 Numurus, LLC <https://www.numurus.com>.
#
# This file is part of nepi-engine
# (see https://github.com/nepi-engine).
#
# License: 3-clause BSD, see https://opensource.org/licenses/BSD-3-Clause
#
# Sample NEPI Process Script.
# 1. Read in and publish pointclouds as ros pointcloud2 messages from folder at specified rate
# 2. Run until Stopped
import os
import time
import sys
import rospy
import numpy as np
import open3d as o3d
from sensor_msgs.msg import Image, PointCloud2
from cv_bridge import CvBridge
from std_msgs.msg import UInt8, Empty, String, Bool, Float32, Int32
from std_msgs.msg import Header
from sensor_msgs.msg import PointField
from sensor_msgs import point_cloud2
from geometry_msgs.msg import Vector3, Transform
from nepi_ros_interfaces.msg import Frame3DTransform, Frame3DTransformUpdate
from nepi_edge_sdk_base import nepi_ros
from nepi_edge_sdk_base import nepi_pc
#########################################
# USER SETTINGS - Edit as Necessary
#########################################
Current_Path = (os.path.dirname(os.path.realpath(__file__)) )
### Open and publish image
#Source_Folder = Current_Path + "/sample_data/"
Source_Folder = "/mnt/nepi_storage/sample_data"
Publish_Image_Topic_Name = "test_pointcloud"
Publish_Image_Rate_Hz = 5
Zero_Transform = '0 0 0 0 0 0 0'
#########################################
# ROS NAMESPACE SETUP
#########################################
# ROS namespace setup
NEPI_BASE_NAMESPACE = nepi_ros.get_base_namespace()
TRANSFORM_PUB_NAMESPACE = NEPI_BASE_NAMESPACE + "app_pointcloud/add_transform"
#########################################
# Node Class
#########################################
class publish_pointclouds_from_folder_process(object):
seq_num = 0
transforms_msg_list = []
#######################
### Node Initialization
def __init__(self):
rospy.loginfo("Starting Initialization Processes")
# Get list of image files in the folder
print("Looking for pcd files in folder: " + Source_Folder)
if os.path.exists(Source_Folder):
[pcd_file_list, num_pcd_files] = nepi_ros.get_file_list(Source_Folder,'pcd')
print(num_pcd_files)
[transforms_file_list, num_tf_files] = nepi_ros.get_file_list(Source_Folder,'txt')
if num_pcd_files > 0:
print("Found pointcloud files")
for pcd_file in pcd_file_list:
pcd_filename = os.path.basename(pcd_file)
print(pcd_filename)
transform_file = pcd_file.replace('.pcd','_transform.txt')
if transform_file not in transforms_file_list:
print("No transform file found, so creating one")
try:
new_file = open(transform_file,'w')
new_file.writelines(Zero_Transform)
new_file.close()
except:
pass
else:
print("No pcd files found in folder " + Source_Folder)
else:
print("Folder " + Source_Folder + " not found")
# Read Pointclouds in from found files in folder
self.pc_pubs_list = []
self.pc_msgs_list = []
[transforms_file_list, num_tf_files] = nepi_ros.get_file_list(Source_Folder,'txt')
print("Loading tranfer files")
for transform_file in transforms_file_list:
transform_filename = os.path.basename(transform_file).split('.')[0]
print(transform_filename)
for pcd_file in pcd_file_list:
o3d_pc = o3d.io.read_point_cloud(pcd_file)
self.pc_msgs_list.append(nepi_pc.o3dpc_to_rospc(o3d_pc))
pc_topic_name = os.path.basename(pcd_file).split('.')[0]
pc_topic_name = pc_topic_name.replace('-','_')
pc_topic_name = pc_topic_name.replace('.','')
pc_namespace = NEPI_BASE_NAMESPACE + 'pointclouds/' + pc_topic_name
# Publish pointcloud transforms to pointcloud app
transform_file = pcd_file.split('.')[0] + "_transform.txt"
if transform_file in transforms_file_list:
file = open(transform_file,'r')
transform_str = file.readline()
file.close()
transform_str_list = transform_str.split(' ')
transform_val_list = []
for str_val in transform_str_list:
transform_val_list.append(float(str_val))
else:
transform_val_list = [0,0,0,0,0,0,0]
transform_msg = Frame3DTransform()
transform_msg.translate_vector.x = transform_val_list[0]
transform_msg.translate_vector.y = transform_val_list[1]
transform_msg.translate_vector.z = transform_val_list[2]
transform_msg.rotate_vector.x = transform_val_list[3]
transform_msg.rotate_vector.y = transform_val_list[4]
transform_msg.rotate_vector.z = transform_val_list[5]
transform_msg.heading_offset = transform_val_list[6]
transform_update_msg = Frame3DTransformUpdate()
transform_update_msg.topic_namespace = pc_namespace
transform_update_msg.transform = transform_msg
self.transforms_msg_list.append(transform_update_msg)
print("Creating pointcloud publisher: " + pc_namespace)
self.pc_pubs_list.append(rospy.Publisher(pc_namespace, PointCloud2, queue_size=1))
# Create a transform publisher
print(TRANSFORM_PUB_NAMESPACE)
self.transform_pub = rospy.Publisher(TRANSFORM_PUB_NAMESPACE,Frame3DTransformUpdate,queue_size=10)
nepi_ros.sleep(1,10)
# Start Pointclouds publisher
pub_interval_sec = float(1)/Publish_Image_Rate_Hz
print("Starting pointcloud publishers")
rospy.Timer(rospy.Duration(pub_interval_sec), self.publish_pcs_callback)
## Initiation Complete
rospy.loginfo("Initialization Complete")
#######################
### Node Methods
### Callback to publish pointcloud_test
def publish_pcs_callback(self,timer):
while(len(self.pc_pubs_list) != len(self.pc_msgs_list)):
nepi_ros.sleep(1,10)
seq_num = self.seq_num + 1
for ind in range(len(self.pc_msgs_list)):
if not rospy.is_shutdown():
self.pc_msgs_list[ind].header.stamp = rospy.Time.now()
self.pc_pubs_list[ind].publish(self.pc_msgs_list[ind])
print(self.transforms_msg_list[ind])
if self.transforms_msg_list[ind] != None:
transform_add_topic = nepi_ros.find_topic(TRANSFORM_PUB_NAMESPACE)
print(transform_add_topic)
if transform_add_topic != "":
#print("Publishing transform update msg")
#print(transform_update_msg)
self.transform_pub.publish(self.transforms_msg_list[ind])
self.transforms_msg_list[ind] = None
#######################
# Node Cleanup Function
def cleanup_actions(self):
rospy.loginfo("Shutting down: Executing script cleanup actions")
#########################################
# Main
#########################################
if __name__ == '__main__':
current_filename = sys.argv[0].split('/')[-1]
current_filename = current_filename.split('.')[0]
rospy.loginfo(("Starting " + current_filename), disable_signals=True) # Disable signals so we can force a shutdown
rospy.init_node(name=current_filename)
#Launch the node
node_name = current_filename.rpartition("_")[0]
rospy.loginfo("Launching node named: " + node_name)
node_class = eval(node_name)
node = node_class()
#Set up node shutdown
rospy.on_shutdown(node.cleanup_actions)
# Spin forever (until object is detected)
rospy.spin()