carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
carmen_robot_laser_message frontlaser;
frontlaser.num_readings=reading.size();
frontlaser.range = new float[frontlaser.num_readings];
frontlaser.tooclose=0;
frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
frontlaser.tv=frontlaser.rv=0;
frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
frontlaser.turn_axis=0;
frontlaser.timestamp=reading.getTime();
for (unsigned int i=0; i< reading.size(); i++){
frontlaser.range[i]=(float)reading[i];
}
return frontlaser;
}
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
/**retireve the position from the reading, and compute the odometry*/
OrientedPoint relPose=reading.getPose();
if (!m_count){
m_lastPartPose=m_odoPose=relPose;
}
//write the state of the reading and update all the particles using the motion model
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint& pose(it->pose);
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
}
// update the output file
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODOM ";
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
m_outputStream << reading.getTime();
m_outputStream << endl;
}
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint& pose(it->pose);
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << reading.getTime();
m_outputStream << endl;
}
//invoke the callback
onOdometryUpdate();
// accumulate the robot translation and rotation
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
m_linearDistance+=sqrt(move*move);
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
<< " " <<m_odoPose.theta << endl;
cerr << "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_odoPose=relPose;
bool processed=false;
// process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
if (! m_count
|| m_linearDistance>=m_linearThresholdDistance
|| m_angularDistance>=m_angularThresholdDistance
|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
last_update_time_ = reading.getTime();
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "FRAME " << m_readingCount;
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;
//this is for converting the reading in a scan-matcher feedable form
assert(reading.size()==m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i=0; i<m_beams; i++){
plainReading[i]=reading[i];
}
m_infoStream << "m_count " << m_count << endl;
RangeReading* reading_copy =
new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
if (m_count>0){
scanMatch(plainReading);
if (m_outputStream.is_open()){
m_outputStream << "LASER_READING "<< reading.size() << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(2);
for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
m_outputStream << *b << " ";
}
OrientedPoint p=reading.getPose();
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
const OrientedPoint& pose=it->pose;
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << endl;
}
onScanmatchUpdate();
updateTreeWeights(false);
if (m_infoStream){
m_infoStream << "neff= " << m_neff << endl;
}
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "NEFF " << m_neff << endl;
}
resample(plainReading, adaptParticles, reading_copy);
} else {
m_infoStream << "Registering First Scan"<< endl;
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
m_matcher.registerScan(it->map, it->pose, plainReading);
// cyr: not needed anymore, particles refer to the root in the beginning!
TNode* node=new TNode(it->pose, 0., it->node, 0);
//node->reading=0;
node->reading = reading_copy;
it->node=node;
}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
updateTreeWeights(false);
// cerr << ".done!" <<endl;
delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
m_linearDistance=0;
m_angularDistance=0;
m_count++;
processed=true;
//keep ready for the next step
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
it->previousPose=it->pose;
}
}
if (m_outputStream.is_open())
m_outputStream << flush;
m_readingCount++;
return processed;
}
int main (int argc, char** argv)
{
if (argc < 2)
{
cout << "usage log_test <filename>" << endl;
exit (-1);
}
ifstream is (argv[1]);
if (! is)
{
cout << "no file " << argv[1] << " found" << endl;
exit (-1);
}
CarmenConfiguration conf;
conf.load (is);
SensorMap m = conf.computeSensorMap();
//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
// cout << it->first << " " << it->second->getName() << endl;
SensorLog log (m);
is.close();
ifstream ls (argv[1]);
log.load (ls);
ls.close();
cerr << "log size" << log.size() << endl;
for (SensorLog::iterator it = log.begin(); it != log.end(); it++)
{
RangeReading* rr = dynamic_cast<RangeReading*> (*it);
if (rr)
{
//cerr << rr->getSensor()->getName() << " ";
//cerr << rr->size()<< " ";
//for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
// cerr << *it << " ";
//}
cout << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl;
}
}
}