void OCLSLAM<CR, CW>::toggleSLAMStatus ()
{
slamStatus = !slamStatus;
if (slamStatus)
std::thread ([&] { slam (); }).detach ();
}
void OCLSLAM<CR, CW>::setSLAMStatus (bool flag)
{
slamStatus = flag;
if (slamStatus)
std::thread ([&] { slam (); }).detach ();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "slam");
ros::NodeHandle n; //Listener
Slam slam(n);
slam.initNode();
ROS_INFO("AAAAAA");
ros::spin();
return 0;
}
void OCLSLAM<CR, CW>::init ()
{
if (slam.target_type ().hash_code () != slamFuncHashCode) return;
// Host-Device Transfer ===============================================
kinect->deliverFrames (queue0, dBufferRGB, dBufferD);
// queue0.enqueueFillBuffer<cl_uchar> (dBufferRGB, (cl_uchar) 255, 0, 3 * n * sizeof (cl_uchar));
// queue0.enqueueFillBuffer<cl_ushort> (dBufferD, (cl_ushort) 2000, 0, n * sizeof (cl_ushort));
// ====================================================================
// --------------------------------------------------------------------
// Preprocessing ======================================================
if (gfRGBStatus) gfRGB.run (); else sepRGB.run ();
if (gfDStatus) gfD.run (); else convD.run ();
to8D.run ();
lm.run ();
queue0.enqueueCopyBuffer ((cl::Buffer &) to8D.get (GF::RGBDTo8D::Memory::D_OUT),
(cl::Buffer &) transform.get (ICP::ICPTransform<ICP::ICPTransformConfig::QUATERNION>::Memory::D_OUT),
0, 0, n * sizeof (cl_float8), nullptr, &eventGL); waitListGL[0] = eventGL;
// ====================================================================
// --------------------------------------------------------------------
// Postprocessing =====================================================
sp8DMap.run ();
sp8DMap.read (oclslam::SplitPC8D::Memory::H_OUT_PC3D, CL_FALSE);
// sp8DMap.read (oclslam::SplitPC8D::Memory::H_OUT_RGB, CL_FALSE);
queue0.flush ();
// ====================================================================
// --------------------------------------------------------------------
// OpenGL Rendering ===================================================
if (timeStep < maxPCGL)
{
glMtx.lock (); // Prevent the OpenGL renderer from reading the buffers
glFinish (); // Wait for OpenGL pending operations on the buffers to finish
// Take ownership of the OpenGL buffers
queue1.enqueueAcquireGLObjects ((std::vector<cl::Memory> *) &dBufferGL);
sp8D.setOffset (0);
sp8D.run (&waitListGL);
// Give up ownership of the OpenGL buffers
queue1.enqueueReleaseGLObjects ((std::vector<cl::Memory> *) &dBufferGL);
queue1.finish ();
timeStep++; // Count the current point cloud
glMtx.unlock ();
}
// ====================================================================
display ();
// --------------------------------------------------------------------
// Mapping ============================================================
queue0.finish ();
global_pos = octomap::point3d (0.0, 0.0, 0.0);
std::thread ([this] { _mapping (); }).detach ();
// ====================================================================
// Update SLAM function wrapper
slam = [this] { std::thread ([this] {
while (slamStatus) registerPointCloud ();
}).detach ();
};
if (slamStatus) slam ();
}
