add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,463 @@
|
||||
#include <cstdlib>
|
||||
#include "carmenconfiguration.h"
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <assert.h>
|
||||
#include <sys/types.h>
|
||||
#include <sensor_odometry/odometrysensor.h>
|
||||
#include <sensor_range/rangesensor.h>
|
||||
|
||||
|
||||
#define LINEBUFFER_SIZE 10000
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
using namespace std;
|
||||
|
||||
istream& CarmenConfiguration::load(istream& is){
|
||||
clear();
|
||||
char buf[LINEBUFFER_SIZE];
|
||||
bool laseron=false;
|
||||
bool rlaseron=false;
|
||||
bool rlaser1=false;
|
||||
bool rlaser2=false;
|
||||
|
||||
string beams;
|
||||
string rbeams;
|
||||
|
||||
while (is){
|
||||
is.getline(buf, LINEBUFFER_SIZE);
|
||||
istringstream lis(buf);
|
||||
|
||||
string qualifier;
|
||||
string name;
|
||||
|
||||
if (lis)
|
||||
lis >> qualifier;
|
||||
else
|
||||
continue;
|
||||
//this is a workaround for carmen log files
|
||||
//the number lf laser beams should be specofoed in the config
|
||||
//part of the log
|
||||
if (qualifier=="FLASER"){
|
||||
laseron=true;
|
||||
lis >> beams;
|
||||
}
|
||||
if (qualifier=="RLASER"){
|
||||
rlaseron=true;
|
||||
lis >> rbeams;
|
||||
}
|
||||
if (qualifier=="ROBOTLASER1"){
|
||||
string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
|
||||
lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams;
|
||||
rlaser1=true;
|
||||
}
|
||||
if (qualifier=="ROBOTLASER2"){
|
||||
string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
|
||||
lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams;
|
||||
rlaser2=true;
|
||||
}
|
||||
if (qualifier!="PARAM")
|
||||
continue;
|
||||
if (lis)
|
||||
lis >> name;
|
||||
else continue;
|
||||
|
||||
|
||||
vector<string> v;
|
||||
while (lis){
|
||||
string cparm;
|
||||
lis >> cparm;
|
||||
if (lis)
|
||||
v.push_back(cparm);
|
||||
}
|
||||
insert(make_pair(name, v));
|
||||
}
|
||||
if (laseron || rlaser1){
|
||||
vector<string> v;
|
||||
v.push_back(beams);
|
||||
insert(make_pair("laser_beams", v));
|
||||
cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl;
|
||||
v.clear();
|
||||
v.push_back("on");
|
||||
insert(make_pair("robot_use_laser", v));
|
||||
}
|
||||
if (rlaseron || rlaser2){
|
||||
vector<string> v;
|
||||
v.push_back(rbeams);
|
||||
insert(make_pair("rear_laser_beams", v));
|
||||
cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl;
|
||||
v.clear();
|
||||
v.push_back("on");
|
||||
insert(make_pair("robot_use_rear_laser", v));
|
||||
}
|
||||
return is;
|
||||
}
|
||||
|
||||
SensorMap CarmenConfiguration::computeSensorMap() const{
|
||||
//this boring stuff is for retrieving the parameters from the loaded tokens
|
||||
|
||||
SensorMap smap;
|
||||
//odometry
|
||||
OdometrySensor* odometry=new OdometrySensor("ODOM");
|
||||
OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true);
|
||||
|
||||
smap.insert(make_pair(odometry->getName(), odometry));
|
||||
smap.insert(make_pair(truepos->getName(), truepos));
|
||||
//sonars
|
||||
const_iterator key=find("robot_use_sonar");
|
||||
if (key!=end() && key->second.front()=="on"){
|
||||
RangeSensor* sonar=new RangeSensor("SONAR");
|
||||
|
||||
//the center of the sonar is the center of the base
|
||||
sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0;
|
||||
|
||||
double maxrange=10.;
|
||||
key=find("robot_max_sonar");
|
||||
if (key!=end()){
|
||||
maxrange=atof(key->second.front().c_str());
|
||||
cerr << "max sonar:" << maxrange << endl;
|
||||
}
|
||||
|
||||
unsigned int sonar_num=0;
|
||||
key=find("robot_num_sonars");
|
||||
if (key!=end()){
|
||||
sonar_num=atoi(key->second.front().c_str());
|
||||
cerr << "robot_num_sonars" << sonar_num << endl;
|
||||
}
|
||||
|
||||
key=find("robot_sonar_offsets");
|
||||
if (key!=end()){
|
||||
const vector<string> & soff=key->second;
|
||||
|
||||
if( (soff.size()/3<sonar_num)){
|
||||
cerr << __PRETTY_FUNCTION__ << ": Error " << soff.size()
|
||||
<< " parameters for defining the sonar offsets"
|
||||
<< " while the specified number of sonars requires "
|
||||
<< sonar_num*3 << " parameters at least" << endl;
|
||||
} else {
|
||||
cerr << __PRETTY_FUNCTION__ << ": Ok " << soff.size() << " parameters for defining the sonar offsets of " << sonar_num << " devices" << endl;
|
||||
}
|
||||
|
||||
|
||||
RangeSensor::Beam beam;
|
||||
|
||||
for (unsigned int i=0; i<sonar_num*3; i+=3){
|
||||
beam.span=M_PI/180.*7.5;
|
||||
beam.pose.x=atof(soff[i].c_str());
|
||||
beam.pose.y=atof(soff[i+1].c_str());
|
||||
beam.pose.theta=atof(soff[i+2].c_str());
|
||||
beam.maxRange=maxrange;
|
||||
sonar->m_beams.push_back(beam);
|
||||
cerr << "beam_x" << beam.pose.x;
|
||||
cerr << " beam_y" << beam.pose.y;
|
||||
cerr << " beam_theta" << beam.pose.theta << endl;;
|
||||
}
|
||||
}
|
||||
sonar->updateBeamsLookup();
|
||||
smap.insert(make_pair(sonar->getName(), sonar));
|
||||
}
|
||||
|
||||
//laser
|
||||
key=find("robot_use_laser");
|
||||
|
||||
if (key!=end() && key->second.front()=="on"){
|
||||
RangeSensor* laser=new RangeSensor("FLASER");
|
||||
laser->newFormat=false;
|
||||
//by default the center of the robot is the center of the laser
|
||||
laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
|
||||
key=find("robot_frontlaser_offset");
|
||||
if (key!=end()){
|
||||
laser->m_pose.x=atof(key->second.front().c_str());
|
||||
cerr << "FRONT OFFSET= " << laser->m_pose.x << endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
RangeSensor::Beam beam;
|
||||
|
||||
//double angle=-.5*M_PI;
|
||||
unsigned int beam_no=180;
|
||||
|
||||
key=find("laser_beams");
|
||||
if (key!=end()){
|
||||
beam_no=atoi(key->second.front().c_str());
|
||||
cerr << "FRONT BEAMS="<< beam_no << endl;
|
||||
}
|
||||
|
||||
double maxrange=50;
|
||||
double resolution=1.;
|
||||
|
||||
|
||||
if (beam_no==180 || beam_no==181)
|
||||
resolution =1.;
|
||||
else if (beam_no==360 || beam_no==361)
|
||||
resolution =.5;
|
||||
else if (beam_no==540 || beam_no==541)
|
||||
resolution =.5;
|
||||
else if (beam_no==769) {
|
||||
resolution =360./1024.;
|
||||
maxrange = 4.1;
|
||||
}
|
||||
else if (beam_no==682) {
|
||||
resolution =360./1024.;
|
||||
maxrange = 4.1;
|
||||
}
|
||||
else if (beam_no==683) {
|
||||
resolution =360./1024.;
|
||||
maxrange = 5.5;
|
||||
}
|
||||
else {
|
||||
key=find("laser_front_laser_resolution");
|
||||
if (key!=end()){
|
||||
resolution=atof(key->second.front().c_str());
|
||||
cerr << "FRONT RES " << resolution << endl;
|
||||
}
|
||||
}
|
||||
|
||||
laser->m_beams.resize(beam_no);
|
||||
double center_beam=(double)beam_no/2.;
|
||||
uint low_index=(uint)floor(center_beam);
|
||||
uint up_index=(uint)ceil(center_beam);
|
||||
double step=resolution*M_PI/180.;
|
||||
double angle=beam_no%2?0:step;
|
||||
unsigned int i=beam_no%2?0:1;
|
||||
for (; i<low_index+1; i++, angle+=step){
|
||||
beam.span=0;
|
||||
beam.pose.x=0;
|
||||
beam.pose.y=0;
|
||||
beam.s = 1;
|
||||
beam.c = 1;
|
||||
beam.pose.theta=-angle;
|
||||
beam.maxRange=maxrange;
|
||||
laser->m_beams[low_index-i]=beam;
|
||||
beam.pose.theta=angle;
|
||||
laser->m_beams[up_index+i-1]=beam;
|
||||
}
|
||||
laser->updateBeamsLookup();
|
||||
smap.insert(make_pair(laser->getName(), laser));
|
||||
cerr << "front beams " << beam_no << endl;
|
||||
cerr << "maxrange " << maxrange << endl;
|
||||
}
|
||||
|
||||
|
||||
key=find("robot_use_laser");
|
||||
if (key!=end() && key->second.front()=="on"){
|
||||
RangeSensor* laser=new RangeSensor("ROBOTLASER1");
|
||||
laser->newFormat=true;
|
||||
cerr << "ROBOTLASER1 inserted" << endl;
|
||||
//by default the center of the robot is the center of the laser
|
||||
laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
|
||||
key=find("robot_frontlaser_offset");
|
||||
if (key!=end()){
|
||||
laser->m_pose.x=atof(key->second.front().c_str());
|
||||
cerr << "FRONT OFFSET=" << laser->m_pose.x << endl;
|
||||
}
|
||||
|
||||
RangeSensor::Beam beam;
|
||||
|
||||
//double angle=-.5*M_PI;
|
||||
unsigned int beam_no=180;
|
||||
|
||||
key=find("laser_beams");
|
||||
if (key!=end()){
|
||||
beam_no=atoi(key->second.front().c_str());
|
||||
cerr << "FRONT BEAMS="<< beam_no << endl;
|
||||
}
|
||||
|
||||
double maxrange=50;
|
||||
double resolution=1.;
|
||||
|
||||
|
||||
if (beam_no==180 || beam_no==181)
|
||||
resolution =1.;
|
||||
else if (beam_no==360 || beam_no==361)
|
||||
resolution =.5;
|
||||
else if (beam_no==540 || beam_no==541)
|
||||
resolution =.5;
|
||||
else if (beam_no==769)
|
||||
resolution =360./1024.;
|
||||
else if (beam_no==683) {
|
||||
resolution =360./1024.;
|
||||
maxrange=5.50;
|
||||
}
|
||||
else {
|
||||
key=find("laser_front_laser_resolution");
|
||||
if (key!=end()){
|
||||
resolution=atof(key->second.front().c_str());
|
||||
cerr << "FRONT RES" << resolution << endl;
|
||||
}
|
||||
}
|
||||
|
||||
laser->m_beams.resize(beam_no);
|
||||
double center_beam=(double)beam_no/2.;
|
||||
uint low_index=(uint)floor(center_beam);
|
||||
uint up_index=(uint)ceil(center_beam);
|
||||
double step=resolution*M_PI/180.;
|
||||
double angle=beam_no%2?0:step;
|
||||
unsigned int i=beam_no%2?0:1;
|
||||
for (; i<low_index+1; i++, angle+=step){
|
||||
beam.span=0;
|
||||
beam.pose.x=0;
|
||||
beam.pose.y=0;
|
||||
beam.s=0;
|
||||
beam.c=1;
|
||||
beam.pose.theta=-angle;
|
||||
beam.maxRange=maxrange;
|
||||
laser->m_beams[low_index-i]=beam;
|
||||
beam.pose.theta=angle;
|
||||
laser->m_beams[up_index+i-1]=beam;
|
||||
}
|
||||
laser->updateBeamsLookup();
|
||||
smap.insert(make_pair(laser->getName(), laser));
|
||||
cerr << "front beams" << beam_no << endl;
|
||||
}
|
||||
|
||||
|
||||
//vertical laser
|
||||
key=find("robot_use_rear_laser");
|
||||
|
||||
if (key!=end() && key->second.front()=="on"){
|
||||
RangeSensor* laser=new RangeSensor("RLASER");
|
||||
|
||||
//by default the center of the robot is the center of the laser
|
||||
laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
|
||||
laser->m_pose.theta=M_PI;
|
||||
key=find("robot_rearlaser_offset");
|
||||
if (key!=end()){
|
||||
laser->m_pose.x=atof(key->second.front().c_str());
|
||||
cerr << "REAR OFFSET = " << laser->m_pose.x << endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
RangeSensor::Beam beam;
|
||||
|
||||
//double angle=-.5*M_PI;
|
||||
unsigned int beam_no=180;
|
||||
|
||||
key=find("rear_laser_beams");
|
||||
if (key!=end()){
|
||||
beam_no=atoi(key->second.front().c_str());
|
||||
cerr << "REAR BEAMS="<< beam_no << endl;
|
||||
}
|
||||
|
||||
double maxrange=89;
|
||||
double resolution=1.;
|
||||
|
||||
|
||||
if (beam_no==180 || beam_no==181)
|
||||
resolution =1.;
|
||||
else if (beam_no==360 || beam_no==361)
|
||||
resolution =.5;
|
||||
else if (beam_no==540 || beam_no==541)
|
||||
resolution =.5;
|
||||
else if (beam_no==769)
|
||||
resolution =360./1024.;
|
||||
else {
|
||||
key=find("laser_rear_laser_resolution");
|
||||
if (key!=end()){
|
||||
resolution=atof(key->second.front().c_str());
|
||||
cerr << "REAR RES" << resolution << endl;
|
||||
}
|
||||
}
|
||||
|
||||
laser->m_beams.resize(beam_no);
|
||||
double center_beam=(double)beam_no/2.;
|
||||
uint low_index=(uint)floor(center_beam);
|
||||
uint up_index=(uint)ceil(center_beam);
|
||||
double step=resolution*M_PI/180.;
|
||||
double angle=beam_no%2?0:step;
|
||||
unsigned int i=beam_no%2?0:1;
|
||||
for (; i<low_index+1; i++, angle+=step){
|
||||
beam.span=0;
|
||||
beam.pose.x=0;
|
||||
beam.pose.y=0;
|
||||
beam.s=0;
|
||||
beam.c=1;
|
||||
beam.pose.theta=-angle;
|
||||
beam.maxRange=maxrange;
|
||||
laser->m_beams[low_index-i]=beam;
|
||||
beam.pose.theta=angle;
|
||||
laser->m_beams[up_index+i-1]=beam;
|
||||
}
|
||||
laser->updateBeamsLookup();
|
||||
smap.insert(make_pair(laser->getName(), laser));
|
||||
cerr<< "rear beams" << beam_no << endl;
|
||||
}
|
||||
|
||||
key=find("robot_use_rear_laser");
|
||||
if (key!=end() && key->second.front()=="on"){
|
||||
RangeSensor* laser=new RangeSensor("ROBOTLASER2");
|
||||
laser->newFormat=true;
|
||||
cerr << "ROBOTLASER2 inserted" << endl;
|
||||
//by default the center of the robot is the center of the laser
|
||||
laser->m_pose.x=laser->m_pose.y=0;
|
||||
laser->m_pose.theta=M_PI;
|
||||
key=find("robot_rearlaser_offset");
|
||||
if (key!=end()){
|
||||
// laser->m_pose.x==atof(key->second.front().c_str());
|
||||
cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
|
||||
}
|
||||
|
||||
RangeSensor::Beam beam;
|
||||
|
||||
//double angle=-.5*M_PI;
|
||||
unsigned int beam_no=180;
|
||||
|
||||
key=find("rear_laser_beams");
|
||||
if (key!=end()){
|
||||
beam_no=atoi(key->second.front().c_str());
|
||||
cerr << "REAR BEAMS="<< beam_no << endl;
|
||||
}
|
||||
|
||||
double maxrange=50;
|
||||
double resolution=1.;
|
||||
|
||||
|
||||
if (beam_no==180 || beam_no==181)
|
||||
resolution =1.;
|
||||
else if (beam_no==360 || beam_no==361)
|
||||
resolution =.5;
|
||||
else if (beam_no==540 || beam_no==541)
|
||||
resolution =.5;
|
||||
else if (beam_no==769)
|
||||
resolution =360./1024.;
|
||||
else {
|
||||
key=find("laser_rear_laser_resolution");
|
||||
if (key!=end()){
|
||||
resolution=atof(key->second.front().c_str());
|
||||
cerr << "REAR RES" << resolution << endl;
|
||||
}
|
||||
}
|
||||
|
||||
laser->m_beams.resize(beam_no);
|
||||
double center_beam=(double)beam_no/2.;
|
||||
uint low_index=(uint)floor(center_beam);
|
||||
uint up_index=(uint)ceil(center_beam);
|
||||
double step=resolution*M_PI/180.;
|
||||
double angle=beam_no%2?0:step;
|
||||
unsigned int i=beam_no%2?0:1;
|
||||
for (; i<low_index+1; i++, angle+=step){
|
||||
beam.span=0;
|
||||
beam.s=0;
|
||||
beam.c=1;
|
||||
beam.pose.x=0;
|
||||
beam.pose.y=0;
|
||||
beam.pose.theta=-angle;
|
||||
beam.maxRange=maxrange;
|
||||
laser->m_beams[low_index-i]=beam;
|
||||
beam.pose.theta=angle;
|
||||
laser->m_beams[up_index+i-1]=beam;
|
||||
}
|
||||
laser->updateBeamsLookup();
|
||||
smap.insert(make_pair(laser->getName(), laser));
|
||||
cerr << "rear beams" << beam_no << endl;
|
||||
}
|
||||
|
||||
|
||||
return smap;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user