Example #1
0
/*---------------------------------------------------------------

						CHMTSLAM_3D_viewer

	Optional 3D real-time viewer within HMT-SLAM

  ---------------------------------------------------------------*/
void CHMTSLAM::thread_3D_viewer()
{
	CHMTSLAM* obj = this;
	CTicTac tictac;

	try
	{
		// Start thread:
		// -------------------------
		obj->logFmt(
			mrpt::system::LVL_DEBUG,
			"[thread_3D_viewer] Thread started (ID=0x%08lX)\n",
			std::this_thread::get_id());

		// --------------------------------------------
		//    The main loop
		//  Executes until termination is signaled
		// --------------------------------------------
		while (!obj->m_terminateThreads)
		{
			std::this_thread::sleep_for(100ms);
		};  // end while execute thread

		// Finish thread:
		// -------------------------
		MRPT_TODO("Fix thread times")
		// try { mrpt::system::getCurrentThreadTimes( timCreat,timExit,timCPU);
		// } catch(...) {};
		// obj->logFmt(mrpt::system::LVL_DEBUG,"[thread_3D_viewer] Thread
		// finished. CPU time used:%.06f secs \n",timCPU);
		obj->m_terminationFlag_3D_viewer = true;
	}
	catch (const std::exception& e)
	{
		obj->m_terminationFlag_3D_viewer = true;

		// Release semaphores:

		obj->logFmt(mrpt::system::LVL_ERROR, "%s", e.what());

		// DEBUG: Terminate application:
		obj->m_terminateThreads = true;
	}
	catch (...)
	{
		obj->m_terminationFlag_3D_viewer = true;

		obj->logFmt(
			mrpt::system::LVL_ERROR,
			"\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
			" In CHierarchicalMappingFramework::thread_3D_viewer. Unexpected "
			"runtime error!!\n");

		// DEBUG: Terminate application:
		obj->m_terminateThreads = true;
	}
}
Example #2
0
/*---------------------------------------------------------------

						CHMTSLAM_TBI

	Topological Bayesian Inference (TBI) process within HMT-SLAM

  ---------------------------------------------------------------*/
void CHMTSLAM::thread_TBI()
{
	CHMTSLAM	*obj = this;
	CTicTac							tictac;

	// Seems that must be called in each thread??
	if (obj->m_options.random_seed)
			randomGenerator.randomize( obj->m_options.random_seed );
	else	randomGenerator.randomize();

	try
	{
		// Start thread:
		// -------------------------
		obj->logFmt(mrpt::utils::LVL_DEBUG, "[thread_TBI] Thread started (ID=0x%08lX)\n", mrpt::system::getCurrentThreadId() );

		// --------------------------------------------
		//    The main loop
		//  Executes until termination is signaled
		// --------------------------------------------
		while ( !obj->m_terminateThreads )
		{
			mrpt::system::sleep(100);
		};	// end while execute thread

		// Finish thread:
		// -------------------------
		time_t timCreat,timExit; double timCPU=0;
		try { mrpt::system::getCurrentThreadTimes( timCreat,timExit,timCPU); } catch(...) {};
		obj->logFmt(mrpt::utils::LVL_DEBUG, "[thread_TBI] Thread finished. CPU time used:%.06f secs \n",timCPU);
		obj->m_terminationFlag_TBI = true;

	}
	catch(std::exception &e)
	{
		obj->m_terminationFlag_TBI = true;

		// Release semaphores:

		obj->logFmt(mrpt::utils::LVL_ERROR, "%s", e.what() );

		// DEBUG: Terminate application:
		obj->m_terminateThreads	= true;


	}
	catch(...)
	{
		obj->m_terminationFlag_TBI = true;

		obj->logFmt(mrpt::utils::LVL_ERROR, 
			"\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
			" In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime error!!\n");

		// Release semaphores:

		// DEBUG: Terminate application:
		obj->m_terminateThreads	= true;


	}

}
Example #3
0
// ------------------------------------------------------
//				TestMapping
// ------------------------------------------------------
void Run_HMT_SLAM()
{
	CHMTSLAM			mapping;

	CConfigFile			cfgFile( configFile );
	std::string			rawlogFileName;

	// wait for threads init. (Just to do not mix debug strings on console)
	sleep(100);

	// The rawlog file:
	// ----------------------------------------
	rawlogFileName = cfgFile.read_string("HMT-SLAM","rawlog_file",std::string("log.rawlog"));
	unsigned int	rawlog_offset = cfgFile.read_int("HMT-SLAM","rawlog_offset",0);

	mapping.printf_debug("RAWLOG FILE: \n%s\n",rawlogFileName.c_str());

	const std::string OUT_DIR =  cfgFile.read_string("HMT-SLAM","LOG_OUTPUT_DIR", "HMT_SLAM_OUTPUT");


	ASSERT_FILE_EXISTS_( rawlogFileName )
	CFileGZInputStream	 rawlogFile( rawlogFileName);

	mapping.printf_debug("---------------------------------------------------\n\n");


	// Set relative path for externally-stored images in rawlogs:
	string	rawlog_images_path = extractFileDirectory( rawlogFileName );
	rawlog_images_path+="/Images";
	CImage::IMAGES_PATH_BASE = rawlog_images_path;		// Set it.


	// Load the config options for mapping:
	// ----------------------------------------
	mapping.loadOptions( configFile );



	//			INITIALIZATION
	// ----------------------------------------
	//utils::CRandomGenerator::Randomize( );	// Not necesary (called inside the classes)
	mapping.initializeEmptyMap();

	// The main loop:
	// ---------------------------------------
	unsigned int			rawlogEntry = 0, step = 0;
	bool					finish=false;

	while  (!finish && !mapping.abortedDueToErrors() )
	{
		if (os::kbhit())
        {
			char	pushKey = os::getch();
			finish = 27 == pushKey;
        }

		// Load next object from the rawlog:
		// ----------------------------------------
		CSerializablePtr objFromRawlog;
		try
		{
			rawlogFile >> objFromRawlog;
			rawlogEntry++;
		}
		catch(std::exception &) { break; }
		catch(...) { printf("Untyped exception reading rawlog file!!\n");break;}

		if (rawlogEntry>=rawlog_offset)
		{

			// Process the action and observations:
			// --------------------------------------------
			if (IS_CLASS(objFromRawlog,CActionCollection))
			{
				mapping.pushAction( CActionCollectionPtr( objFromRawlog) ); // Memory will be freed in mapping class
			}
			else if (IS_CLASS(objFromRawlog,CSensoryFrame))
			{
				mapping.pushObservations( CSensoryFramePtr( objFromRawlog) ); // Memory will be freed in mapping class
			}
			else if (IS_CLASS(objFromRawlog,CObservation))
			{
				mapping.pushObservation( CObservationPtr( objFromRawlog) ); // Memory will be freed in mapping class
			}
			else THROW_EXCEPTION("Invalid object class from rawlog!!")


			// Wait for the mapping framework processed the data
			// ---------------------------------------------------
			if ((rawlogEntry % STEPS_BETWEEN_WAITING_FOR_QUEUE_EMPTY)==0)
				while (!mapping.isInputQueueEmpty() && !os::kbhit() && !mapping.abortedDueToErrors() )
				{
					sleep(2);
				}
		} // (rawlogEntry>=rawlog_offset)

		mapping.printf_debug("======== Rawlog entries processed: %i ========\n", rawlogEntry);

        step++;

	};	// end "while(1)"

	mapping.printf_debug("********* Application finished!! 3 seconds to exit... **********\n");
	sleep(1000);
	mapping.printf_debug("********* Application finished!! 2 seconds to exit... **********\n");
	sleep(1000);
	mapping.printf_debug("********* Application finished!! 1 second to exit... **********\n");
	sleep(1000);

	{
		string final_file = OUT_DIR+string("/final_map.hmtslam");
		mapping.printf_debug("\n Saving FINAL HMT-MAP to file: %s\n",final_file.c_str());
		CFileGZOutputStream	fil(final_file);
		mapping.saveState( fil );
	}
}
Example #4
0
/*---------------------------------------------------------------

						areaAbstraction

	Area Abstraction (AA) process within HMT-SLAM

  ---------------------------------------------------------------*/
CHMTSLAM::TMessageLSLAMfromAAPtr CHMTSLAM::areaAbstraction(
	CLocalMetricHypothesis	*LMH,
	const TPoseIDList		&newPoseIDs )
{
	MRPT_START

	ASSERT_( !newPoseIDs.empty() );
	ASSERT_(LMH);
	CHMTSLAM *obj = LMH->m_parent.get();
	ASSERT_(obj);

	// The output results:
	TMessageLSLAMfromAAPtr resMsg = TMessageLSLAMfromAAPtr(new TMessageLSLAMfromAA());


	// Process msg:
	THypothesisID			LMH_ID = LMH->m_ID;
	resMsg->hypothesisID = LMH_ID;

	for (TPoseIDList::const_iterator newID=newPoseIDs.begin();newID!=newPoseIDs.end();++newID)
	{
		// Add a new node to the graph:
		obj->printf_debug("[thread_AA] Processing new pose ID: %u\n", static_cast<unsigned>( *newID ) );

		// Get SF & pose pdf for the new pose.
		const CSensoryFrame			*sf;
		CPose3DPDFParticlesPtr		posePDF = CPose3DPDFParticles::Create();

		{
			// CCriticalSectionLocker	lock( & LMH->m_lock ); // We are already within the LMH's lock!

			// SF:
			std::map<TPoseID,CSensoryFrame>::const_iterator itSFs = LMH->m_SFs.find( *newID );
			ASSERT_(itSFs != LMH->m_SFs.end() );
			sf = &itSFs->second;

			// Pose PDF:
			LMH->getPoseParticles( *newID, *posePDF );
		} // end of LMH's critical section lock

		{
			synch::CCriticalSectionLocker locker ( &LMH->m_robotPosesGraph.lock );

			// Add to the graph partitioner:
			LMH->m_robotPosesGraph.partitioner.options = obj->m_options.AA_options;

			unsigned int newIdx = LMH->m_robotPosesGraph.partitioner.addMapFrame(
				CSensoryFramePtr(new CSensoryFrame(*sf)),
				posePDF );
			LMH->m_robotPosesGraph.idx2pose[ newIdx ] = *newID;
		} // end of critical section lock on "m_robotPosesGraph.lock"
	} // end for each new ID

	vector< vector_uint >   partitions;
	{
		synch::CCriticalSectionLocker locker ( &LMH->m_robotPosesGraph.lock );
		// Recompute partitions:
		LMH->m_robotPosesGraph.partitioner.markAllNodesForReconsideration(); // We will have always small local maps.
		LMH->m_robotPosesGraph.partitioner.updatePartitions( partitions );
	}

	// Send result to LSLAM
	resMsg->partitions.resize( partitions.size() );
	vector< TPoseIDList >::iterator  itDest;
	vector< vector_uint >::const_iterator itSrc;
	for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin(); itSrc!=partitions.end(); itSrc++, itDest++)
	{
		itDest->resize( itSrc->size() );
		vector_uint::const_iterator   it1;
		TPoseIDList::iterator	  it2;
		for (it1=itSrc->begin(), it2=itDest->begin(); it1!=itSrc->end(); it1++,it2++)
			*it2 = LMH->m_robotPosesGraph.idx2pose[ *it1 ];
	}

	resMsg->dumpToConsole( );

	return resMsg;

	MRPT_END
}