/*--------------------------------------------------------------- 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; } }
/*--------------------------------------------------------------- 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; } }
// ------------------------------------------------------ // 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 ); } }
/*--------------------------------------------------------------- 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 }