diff --git a/gridfastslam/gridslamprocessor.cpp b/gridfastslam/gridslamprocessor.cpp index 51fcf52..72df1a9 100644 --- a/gridfastslam/gridslamprocessor.cpp +++ b/gridfastslam/gridslamprocessor.cpp @@ -17,16 +17,15 @@ const double m_distanceThresholdCheck = 20; using namespace std; - GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ - + GridSlamProcessor::GridSlamProcessor(std::ostream& infoStr, std::ostream& errStr): m_infoStream(infoStr), m_errStream(errStr){ period_ = 5.0; m_obsSigmaGain=1; m_resampleThreshold=0.5; m_minimumScore=0.; } - + GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) - :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){ + :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(gsp.m_infoStream), m_errStream(gsp.m_errStream){ period_ = 5.0; @@ -48,14 +47,15 @@ using namespace std; m_linearDistance=gsp.m_linearDistance; m_angularDistance=gsp.m_angularDistance; m_neff=gsp.m_neff; - - cerr << "FILTER COPY CONSTRUCTOR" << endl; - cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; PointerMap pmap; for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ @@ -116,17 +112,21 @@ using namespace std; } } } - cerr << __func__ << ": Number of allocated chunks" << pmap.size() << endl; + if (m_infoStream) + m_infoStream << __func__ << ": Number of allocated chunks" << pmap.size() << endl; for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) assert(it->first->shares==(unsigned int)it->second); - cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl; + if (m_infoStream) + m_infoStream << __func__ << ": SUCCESS, the error is somewhere else" << endl; # endif GridSlamProcessor* cloned=new GridSlamProcessor(*this); # ifdef MAP_CONSISTENCY_CHECK - cerr << __func__ << ": trajectories end" << endl; - cerr << __func__ << ": performing afterclone_fit_test" << endl; + if (m_infoStream) { + m_infoStream << __func__ << ": trajectories end" << endl; + m_infoStream << __func__ << ": performing afterclone_fit_test" << endl; + } ParticleVector::const_iterator jt=cloned->m_particles.begin(); for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ const ScanMatcherMap& m1(it->map); @@ -143,20 +143,24 @@ using namespace std; } } } - cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl; + if (m_infoStream) + m_infoStream << __func__ << ": SUCCESS, the error is somewhere else" << endl; # endif return cloned; } GridSlamProcessor::~GridSlamProcessor(){ - cerr << __func__ << ": Start" << endl; - cerr << __func__ << ": Deleting tree" << endl; + if (m_infoStream) { + m_infoStream << __func__ << ": Start" << endl; + m_infoStream << __func__ << ": Deleting tree" << endl; + } for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ #ifdef TREE_CONSISTENCY_CHECK TNode* node=it->node; while(node) node=node->parent; - cerr << "@" << endl; + if (m_infoStream) + m_infoStream << "@" << endl; #endif if (it->node) delete it->node; @@ -164,7 +168,8 @@ using namespace std; } # ifdef MAP_CONSISTENCY_CHECK - cerr << __func__ << ": performing predestruction_fit_test" << endl; + if (m_infoStream) + m_infoStream << __func__ << ": performing predestruction_fit_test" << endl; typedef std::map >::reference* const, int> PointerMap; PointerMap pmap; for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ @@ -183,10 +188,12 @@ using namespace std; } } } - cerr << __func__ << ": Number of allocated chunks" << pmap.size() << endl; + if (m_infoStream) + m_infoStream << __func__ << ": Number of allocated chunks" << pmap.size() << endl; for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) assert(it->first->shares>=(unsigned int)it->second); - cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl; + if (m_infoStream) + m_infoStream << __func__ << ": SUCCESS, the error is somewhere else" << endl; # endif } @@ -249,7 +256,8 @@ void GridSlamProcessor::setMotionModelParameters SensorMap::const_iterator laser_it=smap.find(std::string("FLASER")); if (laser_it==smap.end()){ - cerr << "Attempting to load the new carmen log format" << endl; + if (m_infoStream) + m_infoStream << "Attempting to load the new carmen log format" << endl; laser_it=smap.find(std::string("ROBOTLASER1")); assert(laser_it!=smap.end()); } @@ -359,19 +367,19 @@ void GridSlamProcessor::setMotionModelParameters m_angularDistance+=fabs(move.theta); // if the robot jumps throw a warning - if (m_linearDistance>m_distanceThresholdCheck){ - cerr << "***********************************************************************" << endl; - cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; - cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; - cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y + if (m_linearDistance>m_distanceThresholdCheck && m_errStream){ + m_errStream << "***********************************************************************" << endl; + m_errStream << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; + m_errStream << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; + m_errStream << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y << " " < #include #include +#include "gmapping/utils/nullstream.h" #include #include #include @@ -126,13 +127,12 @@ namespace GMapping { typedef std::vector ParticleVector; - /** Constructs a GridSlamProcessor, initialized with the default parameters */ - GridSlamProcessor(); - - /** Constructs a GridSlamProcessor, whose output is routed to a stream. - @param infoStr: the output stream + /** Constructs a GridSlamProcessor with default params, whose output is routed to a stream. + * To suppress outputs from this node, pass GMapping::g_null_stream for both arguments. + @param infoStr: (optional) the output stream, default cout + @param errStr: (optional) the error output stream, default cerr */ - GridSlamProcessor(std::ostream& infoStr); + GridSlamProcessor(std::ostream& infoStr = std::cout, std::ostream& errStr = std::cerr); /** @returns a deep copy of the grid slam processor with all the internal structures. */ @@ -305,6 +305,7 @@ namespace GMapping { // stream in which to write the messages std::ostream& m_infoStream; + std::ostream& m_errStream; // the functions below performs side effect on the internal structure, diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx b/include/gmapping/gridfastslam/gridslamprocessor.hxx index 8a8b7a4..8cb9a38 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx @@ -124,31 +124,37 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz deletedParticles.push_back(j); j++; } - // cerr << endl; - std::cerr << "Deleting Nodes:"; + if (m_infoStream) + m_infoStream << "Deleting Nodes:"; for (unsigned int i=0; isetWeight(0); m_matcher.invalidateActiveArea(); m_matcher.registerScan(it->map, it->pose, plainReading); m_particles.push_back(*it); } - std::cerr << " Done" < + +namespace GMapping { + +/** Null output buffer/stream support. + */ +class NullBuffer : public std::streambuf { + public: + int overflow(int c) { + return c; + } +}; +class NullStream : public std::ostream { + public: + NullStream() + : std::ostream(&m_sb) { + } + + private: + NullBuffer m_sb; +}; + +/** Null output stream that can be used to suppress outputs. + * Passing this into the GridSlamProcessor constructor as + * GridSlamProcessor(GMapping::g_null_stream, GMapping::g_null_stream) + * will effectively suppress all command line output from the class. + */ +static NullStream g_null_stream; + +} + +#endif