Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Generalize ostreams used for console output in GridSlamProcessor #45

Open
wants to merge 3 commits into
base: melodic-devel
Choose a base branch
from
Open

Generalize ostreams used for console output in GridSlamProcessor #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2f2e06e
Replace cerr for non-errors with member var m_infoStream.
kevin-robb Apr 24, 2024
c1d7dee
Add member var for generalized err stream.
kevin-robb Apr 24, 2024
a7777d0
Handle errstream in copy constructor.
kevin-robb Apr 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
130 changes: 69 additions & 61 deletions gridfastslam/gridslamprocessor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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 << " " <<m_odoPose.y << " " << m_odoPose.theta << endl;
cerr << "m_lastPartPose=" << m_lastPartPose.x << " " <<m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
cerr << "m_linearDistance=" << m_linearDistance << endl;
cerr << "m_angularDistance=" << m_linearDistance << endl;



if (m_infoStream) {
m_infoStream << "FILTER COPY CONSTRUCTOR" << endl;
m_infoStream << "m_odoPose=" << m_odoPose.x << " " << m_odoPose.y << " " << m_odoPose.theta << endl;
m_infoStream << "m_lastPartPose=" << m_lastPartPose.x << " " << m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
m_infoStream << "m_linearDistance=" << m_linearDistance << endl;
m_infoStream << "m_angularDistance=" << m_linearDistance << endl;
}

m_xmin=gsp.m_xmin;
m_ymin=gsp.m_ymin;
m_xmax=gsp.m_xmax;
Expand All @@ -71,33 +71,29 @@ using namespace std;
m_obsSigmaGain=gsp.m_obsSigmaGain;

#ifdef MAP_CONSISTENCY_CHECK
cerr << __func__ << ": trajectories copy.... ";
if (m_infoStream)
m_infoStream << __func__ << ": trajectories copy.... ";
#endif
TNodeVector v=gsp.getTrajectories();
for (unsigned int i=0; i<v.size(); i++){
m_particles[i].node=v[i];
}
#ifdef MAP_CONSISTENCY_CHECK
cerr << "end" << endl;
if (m_infoStream)
m_infoStream << "end" << endl;
#endif


cerr << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
if (m_infoStream)
m_infoStream << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
updateTreeWeights(false);
cerr << ".done!" <<endl;
}

GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
period_ = 5.0;
m_obsSigmaGain=1;
m_resampleThreshold=0.5;
m_minimumScore=0.;

if (m_infoStream)
m_infoStream << ".done!" <<endl;
}

GridSlamProcessor* GridSlamProcessor::clone() const {
# ifdef MAP_CONSISTENCY_CHECK
cerr << __func__ << ": performing preclone_fit_test" << endl;
if (m_infoStream)
m_infoStream << __func__ << ": performing preclone_fit_test" << endl;
typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
PointerMap pmap;
for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
Expand All @@ -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);
Expand All @@ -143,28 +143,33 @@ 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<Particle>::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;
//cout << "l=" << it->weight<< endl;
}

# 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<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
PointerMap pmap;
for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
Expand All @@ -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
}

Expand Down Expand Up @@ -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());
}
Expand Down Expand Up @@ -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
<< " " <<m_odoPose.theta << endl;
cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
m_errStream << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
<< " " <<relPose.theta << endl;
cerr << "***********************************************************************" << endl;
cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
cerr << "***********************************************************************" << endl;
m_errStream << "***********************************************************************" << endl;
m_errStream << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
m_errStream << "** odometry/laser input. We continue now, but the result is probably **" << endl;
m_errStream << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
m_errStream << "***********************************************************************" << endl;
}

m_odoPose=relPose;
Expand All @@ -391,16 +399,14 @@ void GridSlamProcessor::setMotionModelParameters
m_outputStream << " " << m_linearDistance;
m_outputStream << " " << m_angularDistance << endl;
}

if (m_infoStream)
m_infoStream << "update frame " << m_readingCount << endl
<< "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;


cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
<< " " << reading.getPose().theta << endl;



if (m_infoStream) {
m_infoStream << "update frame " << m_readingCount << endl
<< "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
m_infoStream << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
<< " " << reading.getPose().theta << endl;
}

//this is for converting the reading in a scan-matcher feedable form
assert(reading.size()==m_beams);
double * plainReading = new double[m_beams];
Expand Down Expand Up @@ -462,9 +468,11 @@ void GridSlamProcessor::setMotionModelParameters

}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
// if (m_infoStream)
// m_infoStream << "Tree: normalizing, resetting and propagating weights at the end..." ;
updateTreeWeights(false);
// cerr << ".done!" <<endl;
// if (m_infoStream)
// m_infoStream << ".done!" <<endl;

delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
Expand Down
13 changes: 7 additions & 6 deletions include/gmapping/gridfastslam/gridslamprocessor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <gmapping/particlefilter/particlefilter.h>
#include <gmapping/utils/point.h>
#include <gmapping/utils/macro_params.h>
#include "gmapping/utils/nullstream.h"
#include <gmapping/log/sensorlog.h>
#include <gmapping/sensor/sensor_range/rangesensor.h>
#include <gmapping/sensor/sensor_range/rangereading.h>
Expand Down Expand Up @@ -126,13 +127,12 @@ namespace GMapping {

typedef std::vector<Particle> 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.
*/
Expand Down Expand Up @@ -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,
Expand Down
27 changes: 17 additions & 10 deletions include/gmapping/gridfastslam/gridslamprocessor.hxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; i<deletedParticles.size(); i++){
std::cerr <<" " << deletedParticles[i];
m_infoStream <<" " << deletedParticles[i];
delete m_particles[deletedParticles[i]].node;
m_particles[deletedParticles[i]].node=0;
}
std::cerr << " Done" <<std::endl;
if (m_infoStream)
m_infoStream << " Done" <<std::endl;

//END: BUILDING TREE
std::cerr << "Deleting old particles..." ;
if (m_infoStream)
m_infoStream << "Deleting old particles..." ;
m_particles.clear();
std::cerr << "Done" << std::endl;
std::cerr << "Copying Particles and Registering scans...";
if (m_infoStream) {
m_infoStream << "Done" << std::endl;
m_infoStream << "Copying Particles and Registering scans...";
}
for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
it->setWeight(0);
m_matcher.invalidateActiveArea();
m_matcher.registerScan(it->map, it->pose, plainReading);
m_particles.push_back(*it);
}
std::cerr << " Done" <<std::endl;
if (m_infoStream)
m_infoStream << " Done" <<std::endl;
hasResampled = true;
} else {
int index=0;
std::cerr << "Registering Scans:";
if (m_infoStream)
m_infoStream << "Registering Scans:";
TNodeVector::iterator node_it=oldGeneration.begin();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
//create a new node in the particle tree and add it to the old tree
Expand All @@ -168,7 +174,8 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz
node_it++;

}
std::cerr << "Done" <<std::endl;
if (m_infoStream)
m_infoStream << "Done" <<std::endl;

}
//END: BUILDING TREE
Expand Down
35 changes: 35 additions & 0 deletions include/gmapping/utils/nullstream.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
#ifndef NULLSTREAM_H
#define NULLSTREAM_H

#include <iostream>

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