When i attempt to convert my real sensor point cloud data into a shape using the function simSurfRec.reconstruct_scale_space, my simulation freezes the moment i call the function and I'm not quite sure why.
Here is the code
Code: Select all
function click_callback()
createdShape=simSurfRec.reconstruct_scale_space(pointCloudHandle,4,12,300,0.001)
a=2
end
function createDlg()
if not ui then
local xml =[[<ui title="point cloud" closeable="false" placement="relative">
<button text="Take a point cloud picture to interact with" on-click="click_callback"/>
<label text="(The robot can operate within a point cloud, or within a shape (i.e. a mesh))"/>
</ui>]]
ui=simUI.create(xml)
if uiPos then
simUI.setPosition(ui,uiPos[1],uiPos[2])
else
uiPos={}
uiPos[1],uiPos[2]=simUI.getPosition(ui)
end
end
end
Code: Select all
function sysCall_init()
showDlg()
size=1
pointCloudHandle=sim.getObject('/PointCloud',{noError=true})
shapeHandle=sim.getObject('/Shape',{noError=true})
floorhandle=sim.getObjectHandle('.')
if(not sim.isHandle(pointCloudHandle)) then
sim.createPointCloud(0.02,100,0,4)
pointCloudHandle=sim.getObject('/PointCloud')
end
sub=simROS.subscribe('/camera/depth_registered/points', 'sensor_msgs/PointCloud2', 'pointcloudMessage_callback')
simROS.subscriberTreatUInt8ArrayAsString(sub)
a=0
end
function pointcloudMessage_callback(pts)
if a==0 then
sim.removePointsFromPointCloud(pointCloudHandle,0,nil,0)
local points={}
local col={}
local num=pts.width*pts.height
tables=sim.unpackFloatTable(pts.data,0,0,0)
tables1=sim.unpackUInt8Table(pts.data,0,0,0)
for i=0,num-1 do
if(tables[8*i+1]==tables[8*i+1]) then
table.insert(col,tables1[32*i+19])
table.insert(col,tables1[32*i+18])
table.insert(col,tables1[32*i+17])
table.insert(points,-tables[8*i+1])
table.insert(points,-tables[8*i+2])
table.insert(points,tables[8*i+3])
end
end
sim.insertPointsIntoPointCloud(pointCloudHandle,3,points,col,nil)
sim.setObjectPosition(pointCloudHandle,floorhandle,{0,0,0.2})
yawAngle=-90*(math.pi/180)
pitchAngle=0*(math.pi/180)
rollAngle=105*(math.pi/180)
alphaAngle,betaAngle,gammaAngle=sim.yawPitchRollToAlphaBetaGamma(yawAngle,pitchAngle,rollAngle)
sim.setObjectOrientation(pointCloudHandle,floorhandle,{alphaAngle,betaAngle,gammaAngle})
a=1
end
end