libSmart/collect_raw_point_cloud.py at master · RustIron/libSmart · GitHub

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
from libSmart.Coordinator.AgentsDev import cameraAgent, robotAgent, graphAgent
from libSmart.Coordinator.Coordinator import coordinator
from libSmart.Robot.robot import Robot
from libSmart.Sensor.kinect import Kinector
from libSmart.DataFlow.AgentsData import cameraData, robotData
from libSmart.DataFlow.datacenter import Nodes
from libSmart.DataFlow.dataGraphs import PreprocessorGraph
from libSmart.DataFlow.dataProcessor.config import Jointlists3
from IPython.core.debugger import Tracer

#Tracer()()
cA = cameraAgent(Kinector())
rA = robotAgent(Robot())

nodes = Nodes()
#Tracer()()
cameraDataBlock = cameraData().linkAgent(cA)
robotDataBlock  = robotData().linkAgent(rA)
nodes.create('camera').register_dev_data(cameraDataBlock)
nodes.create('joint',Jointlists3).register_dev_data(robotDataBlock)

ProcG = PreprocessorGraph(nodes)
ProcG.register_input_nodes('camera')
ProcG.register_output_nodes('joint')

gA = graphAgent(ProcG)

manager = coordinator()
manager.register_dev(cA)
manager.register_dev(rA)
manager.register_dev(gA)
manager.run()

pc_scanned = nodes['pointcloud_0'].data +\
			 nodes['pointcloud_1'].data +\
			 nodes['pointcloud_2'].data +\
			 nodes['pointcloud_3'].data

pc_scanned.show()
pc_scanned.save('./Data/scannedData/plier/plier21')
'''
nodes['pointcloud_0'].save()
nodes['pointcloud_1'].save()
nodes['pointcloud_2'].save()
nodes['pointcloud_3'].save()
'''