Skip to content
Snippets Groups Projects
Commit a62a281d authored by Projekt Robbie's avatar Projekt Robbie
Browse files

maskMap disabled because not functional at the moment

parents 89453d11 790725db
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
homer_mapping/src/ParticleFilter/SlamFilter.cpp
+ 537
− 580
View file @ a62a281d
...@@ -8,663 +8,620 @@ const float MIN_TURN_DISTANCE2 = 0.01 * 0.01; ...@@ -8,663 +8,620 @@ const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
const float M_2PI = 2 * M_PI; const float M_2PI = 2 * M_PI;
SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( particleNum ) SlamFilter::SlamFilter(int particleNum)
{ : ParticleFilter<SlamParticle>(particleNum) {
m_OccupancyMap = new OccupancyMap();
m_OccupancyMap = new OccupancyMap(); // generate initial particles
// generate initial particles for (int i = 0; i < m_ParticleNum; i++) {
for ( int i = 0; i < m_ParticleNum; i++ ) m_CurrentList[i] = new SlamParticle();
{ m_LastList[i] = new SlamParticle();
m_CurrentList[i] = new SlamParticle(); }
m_LastList[i] = new SlamParticle();
} float rotationErrorRotating = 0.0;
ros::param::get("/particlefilter/error_values/rotation_error_rotating",
float rotationErrorRotating = 0.0; rotationErrorRotating);
ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating); float rotationErrorTranslating = 0.0;
float rotationErrorTranslating = 0.0; ros::param::get("/particlefilter/error_values/rotation_error_translating",
ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating); rotationErrorTranslating);
float translationErrorTranslating = 0.0; float translationErrorTranslating = 0.0;
ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating); ros::param::get(
float translationErrorRotating = 0.0; "/particlefilter/error_values/translation_error_translating",
ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating); translationErrorTranslating);
float moveJitterWhileTurning = 0.0; float translationErrorRotating = 0.0;
ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning); ros::param::get(
ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond); "/particlefilter/error_values/translation_error_translating",
translationErrorRotating);
int updateMinMoveAngleDegrees; float moveJitterWhileTurning = 0.0;
ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees); ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees); moveJitterWhileTurning);
ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance); ros::param::get("/particlefilter/max_rotation_per_second",
double maxUpdateInterval; m_MaxRotationPerSecond);
ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
m_MaxUpdateInterval = ros::Duration(maxUpdateInterval); int updateMinMoveAngleDegrees;
ros::param::get("/particlefilter/update_min_move_angle",
setRotationErrorRotating ( rotationErrorRotating ); updateMinMoveAngleDegrees);
setRotationErrorTranslating ( rotationErrorTranslating ); m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
setTranslationErrorTranslating ( translationErrorTranslating ); ros::param::get("/particlefilter/update_min_move_dist",
setTranslationErrorRotating ( translationErrorRotating ); m_UpdateMinMoveDistance);
setMoveJitterWhileTurning ( moveJitterWhileTurning ); double maxUpdateInterval;
ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
m_FirstRun = true; m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
m_DoMapping = true;
setRotationErrorRotating(rotationErrorRotating);
m_EffectiveParticleNum = m_ParticleNum; setRotationErrorTranslating(rotationErrorTranslating);
m_LastUpdateTime = ros::Time(0); setTranslationErrorTranslating(translationErrorTranslating);
m_LastMoveTime = ros::Time::now(); setTranslationErrorRotating(translationErrorRotating);
setMoveJitterWhileTurning(moveJitterWhileTurning);
m_FirstRun = true;
m_DoMapping = true;
m_EffectiveParticleNum = m_ParticleNum;
m_LastUpdateTime = ros::Time(0);
m_LastMoveTime = ros::Time::now();
} }
SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle> ( slamFilter.m_ParticleNum ) SlamFilter::SlamFilter(SlamFilter& slamFilter)
{ : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum) {
m_OccupancyMap = new OccupancyMap ( * ( slamFilter.m_OccupancyMap ) ); m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
// generate initial particles // generate initial particles
for ( int i = 0; i < m_ParticleNum; i++ ) for (int i = 0; i < m_ParticleNum; i++) {
{ if (slamFilter.m_CurrentList[i] == 0) {
if ( slamFilter.m_CurrentList[i] == 0 ) m_CurrentList[i] = 0;
{ } else {
m_CurrentList[i]=0; m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[i]));
} }
else if (slamFilter.m_LastList[i] == 0) {
{ m_LastList[i] = 0;
m_CurrentList[i] = new SlamParticle ( * ( slamFilter.m_CurrentList[i] ) ); } else {
} m_LastList[i] = new SlamParticle(*(slamFilter.m_LastList[i]));
if ( slamFilter.m_LastList[i] == 0 ) }
{
m_LastList[i]=0;
}
else
{
m_LastList[i] = new SlamParticle ( * ( slamFilter.m_LastList[i] ) );
} }
}
float rotationErrorRotating = 0.0;
ros::param::get("/particlefilter/error_values/rotation_error_rotating", rotationErrorRotating);
float rotationErrorTranslating = 0.0;
ros::param::get("/particlefilter/error_values/rotation_error_translating", rotationErrorTranslating);
float translationErrorTranslating = 0.0;
ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorTranslating);
float translationErrorRotating = 0.0;
ros::param::get("/particlefilter/error_values/translation_error_translating", translationErrorRotating);
float moveJitterWhileTurning = 0.0;
ros::param::get("/particlefilter/error_values/move_jitter_while_turning", moveJitterWhileTurning);
ros::param::get("/particlefilter/max_rotation_per_second", m_MaxRotationPerSecond);
int updateMinMoveAngleDegrees;
ros::param::get("/particlefilter/update_min_move_angle", updateMinMoveAngleDegrees);
m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
ros::param::get("/particlefilter/update_min_move_dist", m_UpdateMinMoveDistance);
double maxUpdateInterval;
ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
setRotationErrorRotating ( rotationErrorRotating );
setRotationErrorTranslating ( rotationErrorTranslating );
setTranslationErrorTranslating ( translationErrorTranslating );
setTranslationErrorRotating ( translationErrorRotating );
setMoveJitterWhileTurning ( moveJitterWhileTurning );
m_FirstRun = slamFilter.m_FirstRun;
m_DoMapping = slamFilter.m_DoMapping;
m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
m_LastUpdateTime = slamFilter.m_LastUpdateTime;
m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
}
float rotationErrorRotating = 0.0;
ros::param::get("/particlefilter/error_values/rotation_error_rotating",
rotationErrorRotating);
float rotationErrorTranslating = 0.0;
ros::param::get("/particlefilter/error_values/rotation_error_translating",
rotationErrorTranslating);
float translationErrorTranslating = 0.0;
ros::param::get(
"/particlefilter/error_values/translation_error_translating",
translationErrorTranslating);
float translationErrorRotating = 0.0;
ros::param::get(
"/particlefilter/error_values/translation_error_translating",
translationErrorRotating);
float moveJitterWhileTurning = 0.0;
ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
moveJitterWhileTurning);
ros::param::get("/particlefilter/max_rotation_per_second",
m_MaxRotationPerSecond);
int updateMinMoveAngleDegrees;
ros::param::get("/particlefilter/update_min_move_angle",
updateMinMoveAngleDegrees);
m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
ros::param::get("/particlefilter/update_min_move_dist",
m_UpdateMinMoveDistance);
double maxUpdateInterval;
ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
setRotationErrorRotating(rotationErrorRotating);
setRotationErrorTranslating(rotationErrorTranslating);
setTranslationErrorTranslating(translationErrorTranslating);
setTranslationErrorRotating(translationErrorRotating);
setMoveJitterWhileTurning(moveJitterWhileTurning);
m_FirstRun = slamFilter.m_FirstRun;
m_DoMapping = slamFilter.m_DoMapping;
m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
m_LastUpdateTime = slamFilter.m_LastUpdateTime;
m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
}
SlamFilter::~SlamFilter() SlamFilter::~SlamFilter() {
{ if (m_OccupancyMap) {
if ( m_OccupancyMap ) delete m_OccupancyMap;
{
delete m_OccupancyMap;
}
for ( int i = 0; i < m_ParticleNum; i++ )
{
if ( m_CurrentList[i] )
{
delete m_CurrentList[i];
m_CurrentList[i] = 0;
} }
if ( m_LastList[i] ) for (int i = 0; i < m_ParticleNum; i++) {
{ if (m_CurrentList[i]) {
delete m_LastList[i]; delete m_CurrentList[i];
m_LastList[i] = 0; m_CurrentList[i] = 0;
}
if (m_LastList[i]) {
delete m_LastList[i];
m_LastList[i] = 0;
}
} }
}
} }
void SlamFilter::setRotationErrorRotating(float percent) {
void SlamFilter::setRotationErrorRotating ( float percent ) m_Alpha1 = percent / 100.0;
{
m_Alpha1 = percent / 100.0;
} }
void SlamFilter::resetHigh() void SlamFilter::resetHigh() { m_OccupancyMap->resetHighSensitive(); }
{
m_OccupancyMap->resetHighSensitive();
}
void SlamFilter::setRotationErrorTranslating ( float degreePerMeter ) void SlamFilter::setRotationErrorTranslating(float degreePerMeter) {
{ m_Alpha2 = degreePerMeter / 180.0 * M_PI;
m_Alpha2 = degreePerMeter / 180.0 * M_PI;
} }
void SlamFilter::setTranslationErrorTranslating ( float percent ) void SlamFilter::setTranslationErrorTranslating(float percent) {
{ m_Alpha3 = percent / 100.0;
m_Alpha3 = percent / 100.0;
} }
void SlamFilter::setTranslationErrorRotating ( float mPerDegree ) void SlamFilter::setTranslationErrorRotating(float mPerDegree) {
{ m_Alpha4 = mPerDegree / 180.0 * M_PI;
m_Alpha4 = mPerDegree / 180.0 * M_PI;
} }
void SlamFilter::setMoveJitterWhileTurning ( float mPerDegree ) void SlamFilter::setMoveJitterWhileTurning(float mPerDegree) {
{ m_Alpha5 = mPerDegree / 180.0 * M_PI;
m_Alpha5 = mPerDegree / 180.0 * M_PI;
} }
void SlamFilter::setScanMatchingClusterSize ( float minClusterSize ) void SlamFilter::setScanMatchingClusterSize(float minClusterSize) {
{ minClusterSize = minClusterSize;
minClusterSize = minClusterSize;
} }
void SlamFilter::setMapping ( bool doMapping ) void SlamFilter::setMapping(bool doMapping) { m_DoMapping = doMapping; }
{
m_DoMapping = doMapping;
}
void SlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap ) void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap) {
{ // delete old
//delete old if (m_OccupancyMap) {
if ( m_OccupancyMap ) delete m_OccupancyMap;
{ }
delete m_OccupancyMap; // copy
} m_OccupancyMap = occupancyMap;
//copy
m_OccupancyMap = occupancyMap;
} }
vector<Pose>* SlamFilter::getParticlePoses() const {
vector<Pose>* SlamFilter::getParticlePoses() const vector<Pose>* particlePoses = new vector<Pose>();
{ for (int i = 0; i < m_ParticleNum; i++) {
vector<Pose>* particlePoses = new vector<Pose>(); float robotX, robotY, robotTheta;
for ( int i = 0; i < m_ParticleNum; i++ ) SlamParticle* particle = m_CurrentList[i];
{ particle->getRobotPose(robotX, robotY, robotTheta);
float robotX, robotY, robotTheta; particlePoses->push_back(Pose(robotX, robotY, robotTheta));
SlamParticle* particle = m_CurrentList[i]; }
particle->getRobotPose ( robotX, robotY, robotTheta ); return particlePoses;
particlePoses->push_back ( Pose ( robotX, robotY, robotTheta ) );
}
return particlePoses;
} }
vector<SlamParticle*>* SlamFilter::getParticles() const vector<SlamParticle*>* SlamFilter::getParticles() const {
{ vector<SlamParticle*>* particles = new vector<SlamParticle*>();
vector<SlamParticle*>* particles = new vector<SlamParticle*>(); for (int i = 0; i < m_ParticleNum; i++) {
for ( int i = 0; i < m_ParticleNum; i++ ) SlamParticle* particle = m_CurrentList[i];
{ particles->push_back(particle);
SlamParticle* particle = m_CurrentList[i]; }
particles->push_back ( particle ); return particles;
}
return particles;
} }
void SlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta ) void SlamFilter::setRobotPose(Pose pose, double scatterVarXY,
{ double scatterVarTheta) {
// set first particle to exact position // set first particle to exact position
m_CurrentList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() ); m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
m_LastList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() ); m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
// scatter remaining particles // scatter remaining particles
for ( int i = 1; i < m_ParticleNum; ++i ) for (int i = 1; i < m_ParticleNum; ++i) {
{ const double scatterX = randomGauss() * scatterVarXY;
const double scatterX = randomGauss() * scatterVarXY; const double scatterY = randomGauss() * scatterVarXY;
const double scatterY = randomGauss() * scatterVarXY; const double scatterTheta = randomGauss() * scatterVarTheta;
const double scatterTheta = randomGauss() * scatterVarTheta;
m_CurrentList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
m_CurrentList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta ); pose.theta() + scatterTheta);
m_LastList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta ); m_LastList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
} pose.theta() + scatterTheta);
}
} }
vector<float> SlamFilter::getParticleWeights() const vector<float> SlamFilter::getParticleWeights() const {
{ vector<float> particleWeights(m_ParticleNum);
vector<float> particleWeights ( m_ParticleNum ); for (int i = 0; i < m_ParticleNum; i++) {
for ( int i = 0; i < m_ParticleNum; i++ ) particleWeights[i] = m_CurrentList[i]->getWeight();
{ }
particleWeights[i] = m_CurrentList[i]->getWeight(); return particleWeights;
}
return particleWeights;
} }
double SlamFilter::randomGauss ( float variance ) const double SlamFilter::randomGauss(float variance) const {
{ if (variance < 0) {
if ( variance < 0 ) variance = -variance;
{ }
variance = -variance; double x1, x2, w, y1;
} do {
double x1, x2, w, y1; x1 = 2.0 * random01() - 1.0;
do x2 = 2.0 * random01() - 1.0;
{ w = x1 * x1 + x2 * x2;
x1 = 2.0 * random01() - 1.0; } while (w >= 1.0);
x2 = 2.0 * random01() - 1.0;
w = x1 * x1 + x2 * x2; w = sqrt((-2.0 * log(w)) / w);
} y1 = x1 * w;
while ( w >= 1.0 ); // now y1 is uniformly distributed
return sqrt(variance) * y1;
w = sqrt ( ( -2.0 * log ( w ) ) / w );
y1 = x1 * w;
// now y1 is uniformly distributed
return sqrt ( variance ) * y1;
} }
vector<float> SlamFilter::filterOutliers (sensor_msgs::LaserScanConstPtr rawData, float maxDiff ) vector<float> SlamFilter::filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
{ float maxDiff) {
if ( rawData->ranges.size() < 2 ) if (rawData->ranges.size() < 2) {
{ return rawData->ranges;
return rawData->ranges; }
} vector<float> filteredData = rawData->ranges;
vector<float> filteredData = rawData->ranges; for (unsigned int i = 1; i < filteredData.size() - 1; i++) {
for ( unsigned int i = 1; i < filteredData.size() - 1; i++ ) if (abs((float)(rawData->ranges[i - 1] - rawData->ranges[i] * 2 +
{ rawData->ranges[i + 1])) > maxDiff * 2) {
if ( abs ( ( float ) ( rawData->ranges[i-1] - rawData->ranges[i]*2 + rawData->ranges[i+1] ) ) > maxDiff*2 ) filteredData[i] = 0;
{ }
filteredData[i] = 0; }
if (fabs(rawData->ranges[0] - rawData->ranges[1]) > maxDiff) {
filteredData[0] = 0;
}
if (fabs(rawData->ranges[rawData->ranges.size() - 1] -
rawData->ranges[rawData->ranges.size() - 2]) > maxDiff) {
filteredData[rawData->ranges.size() - 1] = 0;
} }
}
if ( fabs ( rawData->ranges[0] - rawData->ranges[1] ) > maxDiff ) return filteredData;
{
filteredData[0] = 0;
}
if ( fabs ( rawData->ranges[ rawData->ranges.size()-1 ] - rawData->ranges[ rawData->ranges.size()-2 ] ) > maxDiff )
{
filteredData[ rawData->ranges.size()-1 ] = 0;
}
return filteredData;
} }
void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &FilterDuration) void SlamFilter::filter(Pose currentPose,
{ sensor_msgs::LaserScanConstPtr laserData,
// if first run, initialize data ros::Time measurementTime,
if ( m_FirstRun ) ros::Duration& FilterDuration) {
{ // if first run, initialize data
m_FirstRun = false; if (m_FirstRun) {
// only do mapping, save first pose as reference m_FirstRun = false;
if ( m_DoMapping ) // only do mapping, save first pose as reference
{ if (m_DoMapping) {
tf::Transform transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(0.0, 0.0, 0.0)); tf::Transform transform(tf::createQuaternionFromYaw(0.0),
m_OccupancyMap->insertLaserData ( laserData , transform); tf::Vector3(0.0, 0.0, 0.0));
m_OccupancyMap->insertLaserData(laserData, transform);
}
m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
*laserData); // copy const ptr to be able to change values; //test
m_ReferencePoseOdometry = currentPose;
m_ReferenceMeasurementTime = measurementTime;
measure();
ROS_INFO_STREAM("first run!");
normalize();
sort(0, m_ParticleNum - 1);
return;
} }
m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(*laserData); //copy const ptr to be able to change values; //test // m_CurrentLaserConfig = laserConf;
m_ReferencePoseOdometry = currentPose; m_CurrentPoseOdometry = currentPose;
m_ReferenceMeasurementTime = measurementTime; m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
*laserData); // copy const ptr to be able to change values
measure(); m_CurrentLaserData->ranges = filterOutliers(laserData, 0.3);
ROS_INFO_STREAM("first run!");
normalize(); Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
sort ( 0, m_ParticleNum - 1 );
return; bool moving = sqr(trans.x()) + sqr(trans.y()) > 0 || sqr(trans.theta()) > 0;
}
//m_CurrentLaserConfig = laserConf; // do not resample if move to small and last move is min 0.5 sec away
m_CurrentPoseOdometry = currentPose; // if (sqr(trans.x()) + sqr(trans.y()) < MIN_MOVE_DISTANCE2 &&
m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(*laserData); //copy const ptr to be able to change values // sqr(trans.theta()) < MIN_TURN_DISTANCE2 &&
m_CurrentLaserData->ranges = filterOutliers ( laserData, 0.3 ); //(ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
if (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry; // if(false)
// do not resample if move to small and last move is min 0.5 sec away
if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0 )
//if(false)
{
ROS_DEBUG_STREAM( "Move too small, will not resample." );
if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
{
resample();
ROS_INFO_STREAM( "Particles too scattered, resampling." );
// filter steps
drift();
measure();
normalize();
sort ( 0, m_ParticleNum - 1 );
}
}
else
{
if(!( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 ))
{
m_LastMoveTime = ros::Time::now();
}
resample();
// filter steps
drift();
measure();
normalize();
sort ( 0, m_ParticleNum - 1 );
}
Pose likeliestPose = getLikeliestPose(measurementTime); //test
Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
ostringstream stream;
stream.precision ( 2 );
stream << "Transformation since last update: angle=" << Math::rad2Deg ( transSinceLastUpdate.theta() ) << " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
bool update = ( fabs ( transSinceLastUpdate.theta() ) > m_UpdateMinMoveAngle ) ||
( transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance ) ||
( ( measurementTime - m_LastUpdateTime ) > m_MaxUpdateInterval );
if ( m_DoMapping && update )
{
stream << "Updating map.";
double elapsedSeconds = ( measurementTime - m_ReferenceMeasurementTime ).toSec();
double thetaPerSecond;
if(elapsedSeconds == 0.0)
{
thetaPerSecond = trans.theta();
}
else
{
thetaPerSecond = trans.theta() / elapsedSeconds;
}
float poseVarianceX, poseVarianceY;
getPoseVariances(50, poseVarianceX, poseVarianceY);
if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond && poseVarianceX < 0.05 && poseVarianceY < 0.05 )
{ {
updateMap(); ROS_DEBUG_STREAM("Move too small, will not resample.");
m_LastUpdatePose = likeliestPose; if (m_EffectiveParticleNum < m_ParticleNum / 10) {
m_LastUpdateTime = measurementTime; resample();
ROS_INFO_STREAM("Particles too scattered, resampling.");
// filter steps
drift();
measure();
normalize();
sort(0, m_ParticleNum - 1);
}
} else {
if (moving) {
m_LastMoveTime = ros::Time::now();
}
resample();
// filter steps
drift();
measure();
normalize();
sort(0, m_ParticleNum - 1);
} }
else
{ Pose likeliestPose = getLikeliestPose(measurementTime); // test
ROS_WARN_STREAM( "No mapping performed, rotation angle too big." ); Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
ostringstream stream;
stream.precision(2);
stream << "Transformation since last update: angle="
<< Math::rad2Deg(transSinceLastUpdate.theta())
<< " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
bool update =
(std::fabs(transSinceLastUpdate.theta()) > m_UpdateMinMoveAngle) ||
(transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance) ||
((measurementTime - m_LastUpdateTime) > m_MaxUpdateInterval);
if (m_DoMapping && update) {
stream << "Updating map.";
double elapsedSeconds =
(measurementTime - m_ReferenceMeasurementTime).toSec();
double thetaPerSecond;
if (elapsedSeconds == 0.0) {
thetaPerSecond = trans.theta();
} else {
thetaPerSecond = trans.theta() / elapsedSeconds;
}
float poseVarianceX, poseVarianceY;
getPoseVariances(50, poseVarianceX, poseVarianceY);
if (std::fabs(thetaPerSecond) < m_MaxRotationPerSecond &&
poseVarianceX < 0.05 && poseVarianceY < 0.05) {
updateMap();
m_LastUpdatePose = likeliestPose;
m_LastUpdateTime = measurementTime;
} else {
ROS_WARN_STREAM("No mapping performed - variance to high");
}
} else {
stream << "No map update performed.";
} }
} ROS_DEBUG_STREAM(stream.str());
else // safe last used pose and laserdata as reference
{
stream << "No map update performed."; m_ReferencePoseOdometry = m_CurrentPoseOdometry;
} m_ReferenceMeasurementTime = measurementTime;
ROS_DEBUG_STREAM( stream.str() );
// safe last used pose and laserdata as reference
m_ReferencePoseOdometry = m_CurrentPoseOdometry;
m_ReferenceMeasurementTime = measurementTime;
} }
/** /**
* For the probabilistic motion model of the robot we use the following three parameters: * For the probabilistic motion model of the robot we use the following three
* - When the robot starts, the initial orientation may have errors (a few degrees). (m_InitialOrientationError) * parameters:
* - The distance of the robot movement may be wrong (a percentage of the moved distance). (m_TranslationError) * - When the robot starts, the initial orientation may have errors (a few
* - The orientation of the robot when the motion was finished may be wrong (a percentage of the rotation) (m_RotationError). * degrees). (m_InitialOrientationError)
* [cf. "An Efficient FastSLAM Algorithm for Generating Maps of Large-Scale Cyclic Environments * - The distance of the robot movement may be wrong (a percentage of the moved
* distance). (m_TranslationError)
* - The orientation of the robot when the motion was finished may be wrong (a
* percentage of the rotation) (m_RotationError).
* [cf. "An Efficient FastSLAM Algorithm for Generating Maps of Large-Scale
* Cyclic Environments
* from Raw Laser Range Measurements", Dirk Haenelt et. al.] * from Raw Laser Range Measurements", Dirk Haenelt et. al.]
* We use Gaussian-Distributions to estimate the error. * We use Gaussian-Distributions to estimate the error.
* The expected value of the errors are zero. * The expected value of the errors are zero.
*/ */
void SlamFilter::drift() void SlamFilter::drift() {
{ float rx = m_ReferencePoseOdometry.x();
float ry = m_ReferencePoseOdometry.y();
float rx = m_ReferencePoseOdometry.x(); float rt = m_ReferencePoseOdometry.theta();
float ry = m_ReferencePoseOdometry.y(); float cx = m_CurrentPoseOdometry.x();
float rt = m_ReferencePoseOdometry.theta(); float cy = m_CurrentPoseOdometry.y();
float cx = m_CurrentPoseOdometry.x(); float ct = m_CurrentPoseOdometry.theta();
float cy = m_CurrentPoseOdometry.y();
float ct = m_CurrentPoseOdometry.theta(); Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry; // find out if driving forward or backward
bool backwardMove = false;
// find out if driving forward or backward float scalar = odoTrans.x() * cosf(rt) + odoTrans.y() * sinf(rt);
bool backwardMove = false; if (scalar <= 0) {
float scalar = odoTrans.x() * cosf ( rt ) + odoTrans.y() * sinf ( rt ); backwardMove = true;
if ( scalar <= 0 ) }
{ float distance = sqrt(sqr(odoTrans.x()) + sqr(odoTrans.y()));
backwardMove = true; float deltaRot1, deltaTrans, deltaRot2;
} if (distance < sqrt(MIN_MOVE_DISTANCE2)) {
float distance = sqrt ( sqr ( odoTrans.x() ) + sqr ( odoTrans.y() ) ); deltaRot1 = odoTrans.theta();
float deltaRot1, deltaTrans, deltaRot2; deltaTrans = 0.0;
if ( distance < sqrt ( MIN_MOVE_DISTANCE2 ) ) deltaRot2 = 0.0;
{ } else if (backwardMove) {
deltaRot1 = odoTrans.theta(); deltaRot1 = atan2(ry - cy, rx - cx) - rt;
deltaTrans = 0.0; deltaTrans = -distance;
deltaRot2 = 0.0; deltaRot2 = ct - rt - deltaRot1;
} } else {
else if ( backwardMove ) deltaRot1 = atan2(odoTrans.y(), odoTrans.x()) - rt;
{ deltaTrans = distance;
deltaRot1 = atan2 ( ry - cy, rx - cx ) - rt; deltaRot2 = ct - rt - deltaRot1;
deltaTrans = - distance; }
deltaRot2 = ct - rt - deltaRot1;
} while (deltaRot1 >= M_PI) deltaRot1 -= M_2PI;
else while (deltaRot1 < -M_PI) deltaRot1 += M_2PI;
{ while (deltaRot2 >= M_PI) deltaRot2 -= M_2PI;
deltaRot1 = atan2 ( odoTrans.y(), odoTrans.x() ) - rt; while (deltaRot2 < -M_PI) deltaRot2 += M_2PI;
deltaTrans = distance;
deltaRot2 = ct - rt - deltaRot1; // always leave one particle with pure displacement
} SlamParticle* particle = m_CurrentList[0];
while ( deltaRot1 >= M_PI ) deltaRot1 -= M_2PI;
while ( deltaRot1 < -M_PI ) deltaRot1 += M_2PI;
while ( deltaRot2 >= M_PI ) deltaRot2 -= M_2PI;
while ( deltaRot2 < -M_PI ) deltaRot2 += M_2PI;
// always leave one particle with pure displacement
SlamParticle* particle = m_CurrentList[0];
// get stored pose
float robotX, robotY, robotTheta;
particle->getRobotPose ( robotX, robotY, robotTheta );
Pose pose ( robotX, robotY, robotTheta );
// move pose
float posX = pose.x() + deltaTrans * cos ( pose.theta() + deltaRot1 );
float posY = pose.y() + deltaTrans * sin ( pose.theta() + deltaRot1 );
float theta = pose.theta() + deltaRot1 + deltaRot2;
while ( theta > M_PI ) theta -= M_2PI;
while ( theta <= -M_PI ) theta += M_2PI;
// save new pose
particle->setRobotPose ( posX, posY, theta );
int i = 1;
//calculating parallel on 8 threats
//TODO: ERROR ON RESET MAPS
// omp_set_num_threads(4);
// #pragma omp parallel for
for ( i = 1; i < m_ParticleNum; i++ )
{
SlamParticle* particle = m_CurrentList[i];
// get stored pose // get stored pose
float robotX, robotY, robotTheta; float robotX, robotY, robotTheta;
particle->getRobotPose ( robotX, robotY, robotTheta ); particle->getRobotPose(robotX, robotY, robotTheta);
Pose pose ( robotX, robotY, robotTheta ); Pose pose(robotX, robotY, robotTheta);
// move pose // move pose
float estDeltaRot1 = deltaRot1 - randomGauss ( m_Alpha1 * fabs ( deltaRot1 ) + m_Alpha2 * deltaTrans ); float posX = pose.x() + deltaTrans * cos(pose.theta() + deltaRot1);
float estDeltaTrans = deltaTrans - randomGauss ( m_Alpha3 * deltaTrans + m_Alpha4 * ( fabs ( deltaRot1 ) + fabs ( deltaRot2 ) ) ); float posY = pose.y() + deltaTrans * sin(pose.theta() + deltaRot1);
float estDeltaRot2 = deltaRot2 - randomGauss ( m_Alpha1 * fabs ( deltaRot2 ) + m_Alpha2 * deltaTrans ); float theta = pose.theta() + deltaRot1 + deltaRot2;
while (theta > M_PI) theta -= M_2PI;
float posX = pose.x() + estDeltaTrans * cos ( pose.theta() + estDeltaRot1 ) + randomGauss ( m_Alpha5 * fabs ( estDeltaRot1 + estDeltaRot2 ) ); while (theta <= -M_PI) theta += M_2PI;
float posY = pose.y() + estDeltaTrans * sin ( pose.theta() + estDeltaRot1 ) + randomGauss ( m_Alpha5 * fabs ( estDeltaRot1 + estDeltaRot2 ) );
float theta = pose.theta() + estDeltaRot1 + estDeltaRot2;
// save new pose // save new pose
while ( theta > M_PI ) theta -= M_2PI; particle->setRobotPose(posX, posY, theta);
while ( theta <= -M_PI ) theta += M_2PI; int i = 1;
particle->setRobotPose ( posX, posY, theta ); // calculating parallel on 8 threats
} // TODO: ERROR ON RESET MAPS
// omp_set_num_threads(4);
// #pragma omp parallel for
} for (i = 1; i < m_ParticleNum; i++) {
SlamParticle* particle = m_CurrentList[i];
// get stored pose
void SlamFilter::measure()
{
if ( m_OccupancyMap )
{
m_MeasurePoints = m_OccupancyMap->getMeasurePoints ( m_CurrentLaserData );
//calculating parallel on 8 threats
omp_set_num_threads(8);
int i = 0;
//#pragma omp parallel for
for (i = 0; i < m_ParticleNum; i++ )
{
SlamParticle* particle = m_CurrentList[i];
if ( !particle )
{
ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
}
else
{
// calculate weights
float robotX, robotY, robotTheta; float robotX, robotY, robotTheta;
particle->getRobotPose ( robotX, robotY, robotTheta ); particle->getRobotPose(robotX, robotY, robotTheta);
Pose pose ( robotX, robotY, robotTheta ); Pose pose(robotX, robotY, robotTheta);
float weight = m_OccupancyMap->computeScore ( pose, m_MeasurePoints ); // move pose
particle->setWeight ( weight ); float estDeltaRot1 =
} deltaRot1 -
randomGauss(m_Alpha1 * fabs(deltaRot1) + m_Alpha2 * deltaTrans);
float estDeltaTrans =
deltaTrans -
randomGauss(m_Alpha3 * deltaTrans +
m_Alpha4 * (fabs(deltaRot1) + fabs(deltaRot2)));
float estDeltaRot2 =
deltaRot2 -
randomGauss(m_Alpha1 * fabs(deltaRot2) + m_Alpha2 * deltaTrans);
float posX = pose.x() +
estDeltaTrans * cos(pose.theta() + estDeltaRot1) +
randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
float posY = pose.y() +
estDeltaTrans * sin(pose.theta() + estDeltaRot1) +
randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
float theta = pose.theta() + estDeltaRot1 + estDeltaRot2;
// save new pose
while (theta > M_PI) theta -= M_2PI;
while (theta <= -M_PI) theta += M_2PI;
particle->setRobotPose(posX, posY, theta);
} }
} }
m_EffectiveParticleNum = getEffectiveParticleNum2();
void SlamFilter::measure() {
if (m_OccupancyMap) {
m_MeasurePoints = m_OccupancyMap->getMeasurePoints(m_CurrentLaserData);
// calculating parallel on 8 threats
omp_set_num_threads(8);
int i = 0;
//#pragma omp parallel for
for (i = 0; i < m_ParticleNum; i++) {
SlamParticle* particle = m_CurrentList[i];
if (!particle) {
ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
} else {
// calculate weights
float robotX, robotY, robotTheta;
particle->getRobotPose(robotX, robotY, robotTheta);
Pose pose(robotX, robotY, robotTheta);
float weight =
m_OccupancyMap->computeScore(pose, m_MeasurePoints);
particle->setWeight(weight);
}
}
}
m_EffectiveParticleNum = getEffectiveParticleNum2();
} }
void SlamFilter::updateMap() void SlamFilter::updateMap() {
{ m_OccupancyMap->insertLaserData(m_CurrentLaserData, m_latestTransform);
m_OccupancyMap->insertLaserData ( m_CurrentLaserData, m_latestTransform);
} }
void SlamFilter::printParticles() const void SlamFilter::printParticles() const {
{ cout << endl << "### PARTICLE LIST ###" << endl;
cout << endl << "### PARTICLE LIST ###" << endl; cout << right << fixed;
cout << right << fixed; cout.width(5);
cout.width ( 5 ); for (int i = 0; i < m_ParticleNum; i++) {
for ( int i = 0; i < m_ParticleNum; i++ ) SlamParticle* p_particle = m_CurrentList[i];
{ if (p_particle) {
SlamParticle* p_particle = m_CurrentList[i]; float robotX, robotY, robotTheta;
if ( p_particle ) p_particle->getRobotPose(robotX, robotY, robotTheta);
{ cout << "Particle " << i << ": (" << robotX << "," << robotY << ","
float robotX, robotY, robotTheta; << robotTheta * 180.0 / M_PI << "), weight:\t"
p_particle->getRobotPose ( robotX, robotY, robotTheta ); << p_particle->getWeight() << endl;
cout << "Particle " << i << ": (" << robotX << "," << robotY << "," << robotTheta * 180.0 / M_PI << "), weight:\t" << p_particle->getWeight() << endl; }
} }
} cout << "### END OF LIST ###" << endl;
cout << "### END OF LIST ###" << endl;
} }
void SlamFilter::reduceParticleNumber(int newParticleNum) {
if (newParticleNum < m_ParticleNum) {
SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
SlamParticle** newLastList = new SlamParticle*[newParticleNum];
void SlamFilter::reduceParticleNumber ( int newParticleNum ) for (int i = 0; i < newParticleNum; i++) {
{ newCurrentList[i] = m_CurrentList[i];
if ( newParticleNum < m_ParticleNum ) newLastList[i] = m_LastList[i];
{ }
SlamParticle** newCurrentList = new SlamParticle*[newParticleNum]; for (int i = newParticleNum + 1; i < m_ParticleNum; i++) {
SlamParticle** newLastList = new SlamParticle*[newParticleNum]; delete m_CurrentList[i];
delete m_LastList[i];
}
delete[] m_CurrentList;
delete[] m_LastList;
for ( int i = 0; i < newParticleNum; i++ ) m_CurrentList = newCurrentList;
{ m_LastList = newLastList;
newCurrentList[i] = m_CurrentList[i];
newLastList[i] = m_LastList[i];
}
for ( int i = newParticleNum + 1; i < m_ParticleNum; i++ ) m_ParticleNum = newParticleNum;
{ normalize();
delete m_CurrentList[i];
delete m_LastList[i];
} }
delete[] m_CurrentList;
delete[] m_LastList;
m_CurrentList = newCurrentList;
m_LastList = newLastList;
m_ParticleNum = newParticleNum;
normalize();
}
} }
Pose SlamFilter::getLikeliestPose(ros::Time poseTime) Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
{ float percentage = 0.4; // TODO param? //test
float percentage = 0.4; //TODO param? //test float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0; int numParticles = static_cast<int>(percentage / 100 * m_ParticleNum);
int numParticles = static_cast<int> ( percentage / 100 * m_ParticleNum ); if (0 == numParticles) {
if ( 0 == numParticles ) numParticles = 1;
{ }
numParticles = 1; for (int i = 0; i < numParticles; i++) {
} float robotX, robotY, robotTheta;
for ( int i = 0; i < numParticles; i++ ) m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
{ sumX += robotX;
float robotX, robotY, robotTheta; sumY += robotY;
m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta ); // calculate sum of vectors in unit circle
sumX += robotX; sumDirX += cos(robotTheta);
sumY += robotY; sumDirY += sin(robotTheta);
// calculate sum of vectors in unit circle }
sumDirX += cos ( robotTheta ); float meanTheta = atan2(sumDirY, sumDirX);
sumDirY += sin ( robotTheta ); float meanX = sumX / numParticles;
} float meanY = sumY / numParticles;
float meanTheta = atan2 ( sumDirY, sumDirX ); // broadcast transform map -> base_link
float meanX = sumX / numParticles; tf::Transform transform(
float meanY = sumY / numParticles; tf::createQuaternionFromYaw(meanTheta),
//broadcast transform map -> base_link tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
tf::Transform transform(tf::createQuaternionFromYaw(meanTheta), tf::TransformBroadcaster tfBroadcaster;
tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0)); m_latestTransform = transform;
tf::TransformBroadcaster tfBroadcaster;
m_latestTransform = transform; tfBroadcaster.sendTransform(tf::StampedTransform(
m_latestTransform, poseTime, "/map", "/base_link"));
tfBroadcaster.sendTransform(tf::StampedTransform(m_latestTransform, poseTime, "/map", "/base_link")); return Pose(meanX, meanY, meanTheta);
return Pose ( meanX, meanY, meanTheta );
} }
OccupancyMap* SlamFilter::getLikeliestMap() const OccupancyMap* SlamFilter::getLikeliestMap() const { return m_OccupancyMap; }
{
return m_OccupancyMap;
}
void SlamFilter::getPoseVariances ( int particleNum, float& poseVarianceX, float& poseVarianceY ) void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
{ float& poseVarianceY) {
// the particles of m_CurrentList are sorted by their weights
// the particles of m_CurrentList are sorted by their weights if (particleNum > m_ParticleNum || particleNum <= 0) {
if ( particleNum > m_ParticleNum || particleNum <= 0 ) particleNum = m_ParticleNum;
{ }
particleNum = m_ParticleNum; // calculate average pose
} float averagePoseX = 0;
// calculate average pose float averagePoseY = 0;
float averagePoseX = 0; float robotX = 0.0;
float averagePoseY = 0; float robotY = 0.0;
float robotX = 0.0; float robotTheta = 0.0;
float robotY = 0.0; for (int i = 0; i < particleNum; i++) {
float robotTheta = 0.0; m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
for ( int i = 0; i < particleNum; i++ ) averagePoseX += robotX;
{ averagePoseY += robotY;
m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta ); }
averagePoseX += robotX; averagePoseX /= particleNum;
averagePoseY += robotY; averagePoseY /= particleNum;
}
averagePoseX /= particleNum; // calculate standard deviation of pose
averagePoseY /= particleNum; poseVarianceX = 0.0;
poseVarianceY = 0.0;
// calculate standard deviation of pose for (int i = 0; i < particleNum; i++) {
poseVarianceX = 0.0; m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
poseVarianceY = 0.0; poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
for ( int i = 0; i < particleNum; i++ ) poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
{ }
m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta ); poseVarianceX /= particleNum;
poseVarianceX += ( averagePoseX - robotX ) * ( averagePoseX - robotX ); poseVarianceY /= particleNum;
poseVarianceY += ( averagePoseY - robotY ) * ( averagePoseY - robotY );
}
poseVarianceX /= particleNum;
poseVarianceY /= particleNum;
} }
double SlamFilter::evaluateByContrast() double SlamFilter::evaluateByContrast() {
{ return m_OccupancyMap->evaluateByContrast();
return m_OccupancyMap->evaluateByContrast();
} }
void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg) void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
{
m_OccupancyMap->applyMasking(msg); m_OccupancyMap->applyMasking(msg);
} }
homer_mapping/src/slam_node.cpp
+ 303
− 304
View file @ a62a281d
#include <homer_mapping/slam_node.h> #include <homer_mapping/slam_node.h>
SlamNode::SlamNode(ros::NodeHandle* nh) SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
: m_HyperSlamFilter( 0 )
{
init(); init();
// subscribe to topics // subscribe to topics
m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>("/scan", 1, &SlamNode::callbackLaserScan, this); m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>("/odom", 1, &SlamNode::callbackOdometry, this); "/scan", 1, &SlamNode::callbackLaserScan, this);
m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>("/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this); m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this); "/odom", 1, &SlamNode::callbackOdometry, this);
m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>("/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this); m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this); "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this); m_InitialPoseSubscriber =
m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this); nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this); "/initialpose", 1, &SlamNode::callbackInitialPose, this);
m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
"/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
"/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
"/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
"/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
"/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
// advertise topics // advertise topics
m_PoseStampedPublisher = nh->advertise<geometry_msgs::PoseStamped>("/pose", 2); m_PoseStampedPublisher =
m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1); nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
m_SLAMMapPublisher =
nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time(0); geometry_msgs::PoseStamped poseMsg;
poseMsg.header.frame_id = "map"; poseMsg.header.stamp = ros::Time(0);
poseMsg.pose.position.x = 0.0; poseMsg.header.frame_id = "map";
poseMsg.pose.position.y = 0.0; poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.z = 0.0; poseMsg.pose.position.y = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0); poseMsg.pose.position.z = 0.0;
geometry_msgs::Quaternion quatMsg; tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion quatMsg;
poseMsg.pose.orientation = quatMsg; tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
m_PoseStampedPublisher.publish(poseMsg); // to
poseMsg.pose.orientation = quatMsg;
//// //broadcast transform map -> base_link m_PoseStampedPublisher.publish(poseMsg);
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link")); //// //broadcast transform map -> base_link
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation)); tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); m_tfBroadcaster.sendTransform(tf::StampedTransform(
transform, poseMsg.header.stamp, "map", "base_link"));
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
tf::getYaw(poseMsg.pose.orientation));
m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::init() void SlamNode::init() {
{
double waitTime; double waitTime;
ros::param::get("/particlefilter/wait_time", waitTime); ros::param::get("/particlefilter/wait_time", waitTime);
m_WaitDuration = ros::Duration(waitTime); m_WaitDuration = ros::Duration(waitTime);
ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY); ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta); ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
m_DoMapping = true; m_DoMapping = true;
int particleNum; int particleNum;
ros::param::get("/particlefilter/particle_num", particleNum); ros::param::get("/particlefilter/particle_num", particleNum);
int particleFilterNum; int particleFilterNum;
ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum); ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum ); particleFilterNum);
m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
m_ReferenceOdometryTime = ros::Time(0); m_ReferenceOdometryTime = ros::Time(0);
m_LastMapSendTime = ros::Time(0); m_LastMapSendTime = ros::Time(0);
m_LastPositionSendTime = ros::Time(0); m_LastPositionSendTime = ros::Time(0);
m_LastMappingTime = ros::Time(0); m_LastMappingTime = ros::Time(0);
m_ReferenceOdometryTime = ros::Time(0); m_ReferenceOdometryTime = ros::Time(0);
m_laser_queue.clear(); m_laser_queue.clear();
m_odom_queue.clear(); m_odom_queue.clear();
} }
SlamNode::~SlamNode() SlamNode::~SlamNode() { delete m_HyperSlamFilter; }
{
delete m_HyperSlamFilter;
}
void SlamNode::resetMaps() void SlamNode::resetMaps() {
{ ROS_INFO("Resetting maps..");
ROS_INFO( "Resetting maps.." );
delete m_HyperSlamFilter;
m_HyperSlamFilter = 0;
init();
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time::now();
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.y = 0.0;
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
m_LastLikeliestPose.set(0.0, 0.0);
m_LastLikeliestPose.setTheta(0.0f);
//// //broadcast transform map -> base_link
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation));
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
// sendMapDataMessage();
}
void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) delete m_HyperSlamFilter;
{ m_HyperSlamFilter = 0;
m_HyperSlamFilter->resetHigh();
init();
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time::now();
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.y = 0.0;
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
// to
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
m_LastLikeliestPose.set(0.0, 0.0);
m_LastLikeliestPose.setTheta(0.0f);
//// //broadcast transform map -> base_link
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(
transform, poseMsg.header.stamp, "map", "base_link"));
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
tf::getYaw(poseMsg.pose.orientation));
m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
// sendMapDataMessage();
} }
void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) {
m_HyperSlamFilter->resetHigh();
}
void SlamNode::sendMapDataMessage(ros::Time mapTime) void SlamNode::sendMapDataMessage(ros::Time mapTime) {
{ std::vector<int8_t> mapData;
std::vector<int8_t> mapData; nav_msgs::MapMetaData metaData;
nav_msgs::MapMetaData metaData;
OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap(); OccupancyMap* occMap =
occMap->getOccupancyProbabilityImage (mapData, metaData); m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
occMap->getOccupancyProbabilityImage(mapData, metaData);
//if ( width != height )
//{
//ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
//}
//else
{
nav_msgs::OccupancyGrid mapMsg; nav_msgs::OccupancyGrid mapMsg;
std_msgs::Header header; std_msgs::Header header;
header.stamp = mapTime; header.stamp = mapTime;
...@@ -133,249 +137,244 @@ void SlamNode::sendMapDataMessage(ros::Time mapTime) ...@@ -133,249 +137,244 @@ void SlamNode::sendMapDataMessage(ros::Time mapTime)
mapMsg.data = mapData; mapMsg.data = mapData;
m_SLAMMapPublisher.publish(mapMsg); m_SLAMMapPublisher.publish(mapMsg);
}
} }
void SlamNode::callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg ) void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg) {
{ Pose userdef_pose(msg->position.x, msg->position.y,
Pose userdef_pose(msg->position.x, msg->position.y, tf::getYaw(msg->orientation)); tf::getYaw(msg->orientation));
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) void SlamNode::callbackInitialPose(
{ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(msg->pose.pose.orientation)); Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); tf::getYaw(msg->pose.pose.orientation));
m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) {
{ m_laser_queue.push_back(msg);
m_laser_queue.push_back(msg); if (m_laser_queue.size() > 5) // Todo param
if(m_laser_queue.size() > 5) //Todo param {
{ m_laser_queue.erase(m_laser_queue.begin());
m_laser_queue.erase(m_laser_queue.begin()); }
}
} }
void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg) void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
{ m_odom_queue.push_back(msg);
m_odom_queue.push_back(msg); static int last_i = 0;
static int last_i = 0; if (m_odom_queue.size() > 5) // Todo param
if(m_odom_queue.size() > 5) //Todo param {
{ last_i--;
last_i--; if (last_i < 0) {
if(last_i < 0) last_i = 0;
{ }
last_i = 0; m_odom_queue.erase(m_odom_queue.begin());
} }
m_odom_queue.erase(m_odom_queue.begin());
} static Pose last_interpolatedPose(0.0, 0.0, 0.0);
ros::Time currentOdometryTime = msg->header.stamp;
static Pose last_interpolatedPose(0.0, 0.0, 0.0); if ((ros::Time::now() - m_LastMappingTime > m_WaitDuration) &&
ros::Time currentOdometryTime = msg->header.stamp; m_odom_queue.size() > 1 && m_laser_queue.size() > 0) {
if( ( ros::Time::now() - m_LastMappingTime > m_WaitDuration ) && m_odom_queue.size() > 1 && m_laser_queue.size() > 0) int i, j;
{ bool got_match = false;
int i, j;
bool got_match = false; for (i = m_odom_queue.size() - 1; i > 0; i--) {
// Check if we would repeat a calculation which is already done but
for( i = m_odom_queue.size()-1; i > 0 ; i--) // still in the queue
{ if (m_odom_queue.at(i - 1)->header.stamp ==
//Check if we would repeat a calculation which is already done but still in the queue m_ReferenceOdometryTime) {
if(m_odom_queue.at(i-1)->header.stamp == m_ReferenceOdometryTime) break;
{ }
break;
} for (j = m_laser_queue.size() - 1; j > -1; j--) {
// find a laserscan in between two odometry readings (or at
for(j = m_laser_queue.size()-1; j > -1; j--) // the same time)
{ if ((m_laser_queue.at(j)->header.stamp >=
// find a laserscan in between two odometry readings (or at the same time) m_odom_queue.at(i - 1)->header.stamp) &&
if( (m_odom_queue.at(i)->header.stamp >=
(m_laser_queue.at(j)->header.stamp >= m_odom_queue.at(i-1)->header.stamp) m_laser_queue.at(j)->header.stamp)) {
&& got_match = true;
(m_odom_queue.at(i)->header.stamp >= m_laser_queue.at(j)->header.stamp) break;
) }
{ }
got_match = true; if (got_match) {
break; break;
} }
} }
if(got_match)
{ if (got_match) {
break; last_i = i;
} m_LastLaserData = m_laser_queue.at(j);
} m_LastMappingTime = m_LastLaserData->header.stamp;
if(got_match) float odoX = m_odom_queue.at(i)->pose.pose.position.x;
{ float odoY = m_odom_queue.at(i)->pose.pose.position.y;
last_i = i; float odoTheta =
m_LastLaserData = m_laser_queue.at(j); tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
m_LastMappingTime = m_LastLaserData->header.stamp; Pose currentOdometryPose(odoX, odoY, odoTheta);
currentOdometryTime = m_odom_queue.at(i)->header.stamp;
float odoX = m_odom_queue.at(i)->pose.pose.position.x;
float odoY = m_odom_queue.at(i)->pose.pose.position.y; odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation); odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
Pose currentOdometryPose ( odoX, odoY, odoTheta ); odoTheta =
currentOdometryTime = m_odom_queue.at(i)->header.stamp; tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
Pose lastOdometryPose(odoX, odoY, odoTheta);
odoX = m_odom_queue.at(i-1)->pose.pose.position.x; m_ReferenceOdometryPose = lastOdometryPose;
odoY = m_odom_queue.at(i-1)->pose.pose.position.y; m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
odoTheta = tf::getYaw(m_odom_queue.at(i-1)->pose.pose.orientation);
Pose lastOdometryPose ( odoX, odoY, odoTheta ); ros::Time startTime = ros::Time::now();
m_ReferenceOdometryPose = lastOdometryPose; // laserscan in between current odometry reading and
m_ReferenceOdometryTime = m_odom_queue.at(i-1)->header.stamp; // m_ReferenceOdometry
// -> calculate pose of robot during laser scan
ros::Duration d1 =
ros::Time startTime = ros::Time::now(); m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
// laserscan in between current odometry reading and m_ReferenceOdometry ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
// -> calculate pose of robot during laser scan
ros::Duration d1 = m_LastLaserData->header.stamp - m_ReferenceOdometryTime; float timeFactor;
ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime; if (d1.toSec() == 0.0) {
timeFactor = 0.0f;
float timeFactor; } else if (d2.toSec() == 0.0) {
if(d1.toSec() == 0.0) timeFactor = 1.0f;
{ } else {
timeFactor = 0.0f; timeFactor = d1.toSec() / d2.toSec();
} }
else if(d2.toSec() == 0.0) ros::Duration duration = ros::Duration(0);
{
timeFactor = 1.0f; last_interpolatedPose = m_ReferenceOdometryPose.interpolate(
} currentOdometryPose, timeFactor);
else m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
{ m_LastLaserData->header.stamp, duration);
timeFactor = d1.toSec() / d2.toSec(); ros::Time finishTime = ros::Time::now();
}
ros::Duration duration = ros::Duration(0); m_LastLikeliestPose =
m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
last_interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor ); m_LastLaserData->header.stamp);
m_HyperSlamFilter->filter( last_interpolatedPose, m_LastLaserData, m_LastLaserData->header.stamp, duration);
ros::Time finishTime = ros::Time::now(); // TODO löschen
m_LastPositionSendTime = m_LastLaserData->header.stamp;
m_LastLikeliestPose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(m_LastLaserData->header.stamp); // send map max. every 500 ms
if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() >
//TODO löschen 0.5) {
m_LastPositionSendTime = m_LastLaserData->header.stamp; sendMapDataMessage(m_LastLaserData->header.stamp);
// send map max. every 500 ms m_LastMapSendTime = m_LastLaserData->header.stamp;
if ( (m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 ) }
{
sendMapDataMessage(m_LastLaserData->header.stamp); ROS_DEBUG_STREAM("Pos. data calc time: "
m_LastMapSendTime = m_LastLaserData->header.stamp; << (finishTime - startTime).toSec() << "s");
} ROS_DEBUG_STREAM("Map send Interval: "
<< (finishTime - m_LastPositionSendTime).toSec()
ROS_DEBUG_STREAM("Pos. data calc time: " << (finishTime - startTime).toSec() << "s" ); << "s");
ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" ); }
} }
} Pose currentPose = m_LastLikeliestPose;
Pose currentPose = m_LastLikeliestPose; // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
//currentPose.setX(currentPose.x() - last_interpolatedPose.x()); // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
//currentPose.setY(currentPose.y() - last_interpolatedPose.y()); // currentPose.setTheta(currentPose.theta() -
//currentPose.setTheta(currentPose.theta() - last_interpolatedPose.theta()); // last_interpolatedPose.theta());
for(int i = (last_i-1<0?0:last_i-1); i < m_odom_queue.size(); i++) for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size();
{ i++) {
currentPose.setX(currentPose.x() + m_odom_queue.at(i)->twist.twist.linear.x); currentPose.setX(currentPose.x() +
currentPose.setY(currentPose.y() + m_odom_queue.at(i)->twist.twist.linear.y); m_odom_queue.at(i)->twist.twist.linear.x);
currentPose.setTheta(currentPose.theta() + m_odom_queue.at(i)->twist.twist.angular.z); currentPose.setY(currentPose.y() +
} m_odom_queue.at(i)->twist.twist.linear.y);
currentPose.setTheta(currentPose.theta() +
geometry_msgs::PoseStamped poseMsg; m_odom_queue.at(i)->twist.twist.angular.z);
//header }
poseMsg.header.stamp = msg->header.stamp;
poseMsg.header.frame_id = "map"; geometry_msgs::PoseStamped poseMsg;
// header
//position and orientation poseMsg.header.stamp = msg->header.stamp;
poseMsg.pose.position.x = currentPose.x(); poseMsg.header.frame_id = "map";
poseMsg.pose.position.y = currentPose.y();
poseMsg.pose.position.z = 0.0; // position and orientation
tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta()); poseMsg.pose.position.x = currentPose.x();
geometry_msgs::Quaternion quatMsg; poseMsg.pose.position.y = currentPose.y();
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion poseMsg.pose.position.z = 0.0;
poseMsg.pose.orientation = quatMsg; tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
m_PoseStampedPublisher.publish(poseMsg); geometry_msgs::Quaternion quatMsg;
//broadcast transform map -> base_link tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
//tf::Transform transform(quatTF,tf::Vector3(currentPose.x(), currentPose.y(), 0.0)); // to geometry_msgs::Quaternion
//m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "map", "base_link")); poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
// broadcast transform map -> base_link
// tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
// currentPose.y(), 0.0));
// m_tfBroadcaster.sendTransform(tf::StampedTransform(transform,
// msg->header.stamp, "map", "base_link"));
} }
void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr &msg) void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg) {
{
m_DoMapping = msg->data; m_DoMapping = msg->data;
m_HyperSlamFilter->setMapping ( m_DoMapping ); m_HyperSlamFilter->setMapping(m_DoMapping);
ROS_INFO_STREAM( "Do mapping is set to " << ( m_DoMapping ) ); ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
} }
void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr &msg) void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg) {
{
resetMaps(); resetMaps();
} }
void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
{
float res = msg->info.resolution; float res = msg->info.resolution;
int height = msg->info.height; // cell size int height = msg->info.height; // cell size
int width = msg->info.width; //cell size int width = msg->info.width; // cell size
//if(height!=width) { // if(height!=width) {
//ROS_ERROR("Height != width in loaded map"); // ROS_ERROR("Height != width in loaded map");
//return; // return;
//} //}
//convert map vector from ros format to robbie probability array // convert map vector from ros format to robbie probability array
float* map = new float[msg->data.size()]; float* map = new float[msg->data.size()];
//generate exploredRegion // generate exploredRegion
int minX = INT_MIN; int minX = INT_MIN;
int minY = INT_MIN; int minY = INT_MIN;
int maxX = INT_MAX; int maxX = INT_MAX;
int maxY = INT_MAX; int maxY = INT_MAX;
for(size_t y = 0; y < msg->info.height; y++) for (size_t y = 0; y < msg->info.height; y++) {
{ int yOffset = msg->info.width * y;
int yOffset = msg->info.width * y; for (size_t x = 0; x < msg->info.width; x++) {
for(size_t x = 0; x < msg->info.width; x++) int i = yOffset + x;
{ if (msg->data[i] == -1)
int i = yOffset + x; map[i] = 0.5;
if(msg->data[i] == -1 ) else
map[i] = 0.5; map[i] = msg->data[i] / 100.0;
else
map[i] = msg->data[i]/100.0; if (map[i] != 0.5) {
if (minX == INT_MIN || minX > (int)x) minX = (int)x;
if(map[i]!=0.5) { if (minY == INT_MIN || minY > (int)y) minY = (int)y;
if(minX==INT_MIN || minX > (int)x) if (maxX == INT_MAX || maxX < (int)x) maxX = (int)x;
minX = (int)x; if (maxY == INT_MAX || maxY < (int)y) maxY = (int)y;
if(minY==INT_MIN || minY > (int)y) }
minY = (int)y; }
if(maxX==INT_MAX || maxX < (int)x) }
maxX = (int)x; Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
if(maxY==INT_MAX || maxY < (int)y) OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
maxY = (int)y; height, exploredRegion);
} m_HyperSlamFilter->setOccupancyMap(occMap);
} m_HyperSlamFilter->setMapping(
} false); // is this already done by gui message?
Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY ); ROS_INFO_STREAM("Replacing occupancy map");
OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, height, exploredRegion);
m_HyperSlamFilter->setOccupancyMap( occMap );
m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
ROS_INFO_STREAM( "Replacing occupancy map" );
} }
void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg) void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
{
m_HyperSlamFilter->applyMasking(msg); m_HyperSlamFilter->applyMasking(msg);
} }
/** /**
* @brief main function * @brief main function
*/ */
int main(int argc, char** argv) int main(int argc, char** argv) {
{
ros::init(argc, argv, "homer_mapping"); ros::init(argc, argv, "homer_mapping");
ros::NodeHandle nh; ros::NodeHandle nh;
SlamNode slamNode(&nh); SlamNode slamNode(&nh);
ros::Rate loop_rate(160); ros::Rate loop_rate(160);
while (ros::ok()) {
while(ros::ok())
{
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
return 0; return 0;
} }
homer_navigation/include/homer_navigation/homer_navigation_node.h
+ 3
− 3
View file @ a62a281d
...@@ -2,10 +2,10 @@ ...@@ -2,10 +2,10 @@
#define FastNavigationModule_H #define FastNavigationModule_H
#include <cmath> #include <cmath>
#include <regex>
#include <sstream> #include <sstream>
#include <string> #include <string>
#include <vector> #include <vector>
#include <regex>
#include <ros/package.h> #include <ros/package.h>
#include <ros/ros.h> #include <ros/ros.h>
...@@ -66,7 +66,6 @@ class HomerNavigationNode { ...@@ -66,7 +66,6 @@ class HomerNavigationNode {
void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg); void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg);
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg); void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg); void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void startNavigationCallback( void startNavigationCallback(
const homer_mapnav_msgs::StartNavigation::ConstPtr& msg); const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
void moveBaseSimpleGoalCallback( void moveBaseSimpleGoalCallback(
...@@ -85,12 +84,13 @@ class HomerNavigationNode { ...@@ -85,12 +84,13 @@ class HomerNavigationNode {
virtual void init(); virtual void init();
void initNewTarget(); void initNewTarget();
void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
private: private:
/** @brief Start navigation to m_Target on last_map_data_ */ /** @brief Start navigation to m_Target on last_map_data_ */
void startNavigation(); void startNavigation();
geometry_msgs::Point calculateMeanPoint(
const std::vector<geometry_msgs::Point>& points);
/** @brief Check if obstacles are blocking the way in last_map_data_ */ /** @brief Check if obstacles are blocking the way in last_map_data_ */
bool obstacleOnPath(); bool obstacleOnPath();
......
homer_navigation/src/homer_navigation_node.cpp
+ 102
− 85
View file @ a62a281d
...@@ -11,7 +11,7 @@ HomerNavigationNode::HomerNavigationNode() { ...@@ -11,7 +11,7 @@ HomerNavigationNode::HomerNavigationNode() {
m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>( m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
"/scan", 1, &HomerNavigationNode::laserDataCallback, this); "/scan", 1, &HomerNavigationNode::laserDataCallback, this);
m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>( m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
"/front_scan", 1, &HomerNavigationNode::downlaserDataCallback, this); "/front_scan", 1, &HomerNavigationNode::laserDataCallback, this);
m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>( m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
"/homer_navigation/start_navigation", 1, "/homer_navigation/start_navigation", 1,
&HomerNavigationNode::startNavigationCallback, this); &HomerNavigationNode::startNavigationCallback, this);
...@@ -34,8 +34,9 @@ HomerNavigationNode::HomerNavigationNode() { ...@@ -34,8 +34,9 @@ HomerNavigationNode::HomerNavigationNode() {
m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>( m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
"/homer_navigation/max_depth_move_distance", 1, "/homer_navigation/max_depth_move_distance", 1,
&HomerNavigationNode::maxDepthMoveDistanceCallback, this); &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
m_ignore_laser_sub = nh.subscribe<std_msgs::String>("/homer_navigation/ignore_laser",1,&HomerNavigationNode::ignoreLaserCallback ,this); m_ignore_laser_sub = nh.subscribe<std_msgs::String>(
"/homer_navigation/ignore_laser", 1,
&HomerNavigationNode::ignoreLaserCallback, this);
m_cmd_vel_pub = m_cmd_vel_pub =
nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3); nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3);
...@@ -156,9 +157,9 @@ void HomerNavigationNode::init() { ...@@ -156,9 +157,9 @@ void HomerNavigationNode::init() {
m_ignore_scan = ""; m_ignore_scan = "";
m_obstacle_on_path = false; m_obstacle_on_path = false;
m_obstacle_position.x = 0; m_obstacle_position.x = 0;
m_obstacle_position.y = 0; m_obstacle_position.y = 0;
m_obstacle_position.z = 0; m_obstacle_position.z = 0;
nav_msgs::OccupancyGrid tmp_map; nav_msgs::OccupancyGrid tmp_map;
tmp_map.header.frame_id = "/map"; tmp_map.header.frame_id = "/map";
...@@ -209,8 +210,8 @@ void HomerNavigationNode::idleProcess() { ...@@ -209,8 +210,8 @@ void HomerNavigationNode::idleProcess() {
} }
} }
bool HomerNavigationNode::isInIgnoreList(std::string frame_id){ bool HomerNavigationNode::isInIgnoreList(std::string frame_id) {
std::regex reg("(^|\\s)"+frame_id+"(\\s|$)"); std::regex reg("(^|\\s)" + frame_id + "(\\s|$)");
return regex_match(m_ignore_scan, reg); return regex_match(m_ignore_scan, reg);
} }
...@@ -218,21 +219,24 @@ void HomerNavigationNode::setExplorerMap() { ...@@ -218,21 +219,24 @@ void HomerNavigationNode::setExplorerMap() {
// adding lasers to map // adding lasers to map
nav_msgs::OccupancyGrid temp_map = *m_map; nav_msgs::OccupancyGrid temp_map = *m_map;
for (const auto& scan : m_scan_map) { for (const auto& scan : m_scan_map) {
if(!isInIgnoreList(scan.second->header.frame_id)){ if (!isInIgnoreList(scan.second->header.frame_id)) {
std::vector<geometry_msgs::Point> scan_points; std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points( scan_points = map_tools::laser_msg_to_points(
scan.second, m_transform_listener, "/map"); scan.second, m_transform_listener, "/map");
for (const auto& point : scan_points) { for (const auto& point : scan_points) {
Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map); Eigen::Vector2i map_point =
map_tools::toMapCoords(point, m_map);
int index = map_point.y() * m_map->info.width + map_point.x(); int index = map_point.y() * m_map->info.width + map_point.x();
if (index > 0 && index < m_map->info.width * m_map->info.height) { if (index > 0 &&
index < m_map->info.width * m_map->info.height) {
temp_map.data[index] = (int8_t)100; temp_map.data[index] = (int8_t)100;
} }
} }
} }
} }
if (m_fast_path_planning) { if (m_fast_path_planning) {
maskMap(temp_map); //TODO check why not functional
//maskMap(temp_map);
} }
m_explorer->setOccupancyMap( m_explorer->setOccupancyMap(
boost::make_shared<nav_msgs::OccupancyGrid>(temp_map)); boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
...@@ -420,39 +424,61 @@ bool HomerNavigationNode::isTargetPositionReached() { ...@@ -420,39 +424,61 @@ bool HomerNavigationNode::isTargetPositionReached() {
} }
} }
geometry_msgs::Point HomerNavigationNode::calculateMeanPoint(
const std::vector<geometry_msgs::Point>& points) {
geometry_msgs::Point mean_point = geometry_msgs::Point();
for (auto const& point : points) {
mean_point.x += point.x;
mean_point.y += point.y;
}
if (points.size() > 0) {
mean_point.x /= points.size();
mean_point.y /= points.size();
}
return mean_point;
}
bool HomerNavigationNode::obstacleOnPath() { bool HomerNavigationNode::obstacleOnPath() {
m_last_check_path_time = ros::Time::now(); m_last_check_path_time = ros::Time::now();
if (m_pixel_path.size() != 0) { if (m_pixel_path.size() == 0) {
for (auto const& scan : m_scan_map) { ROS_DEBUG_STREAM("no path found for finding an obstacle");
if(!isInIgnoreList(scan.second->header.frame_id)){ return false;
std::vector<geometry_msgs::Point> scan_points; }
scan_points = map_tools::laser_msg_to_points( std::vector<geometry_msgs::Point> obstacle_vec;
scan.second, m_transform_listener, "/map");
for (auto const& scan : m_scan_map) {
for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) { if (!isInIgnoreList(scan.second->header.frame_id)) {
geometry_msgs::Point lp = std::vector<geometry_msgs::Point> scan_points;
map_tools::fromMapCoords(m_pixel_path.at(i-1), m_map); scan_points = map_tools::laser_msg_to_points(
geometry_msgs::Point p = scan.second, m_transform_listener, "/map");
map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
if (map_tools::distance(m_robot_pose.position, p) > for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
m_check_path_max_distance * 2) { geometry_msgs::Point lp =
map_tools::fromMapCoords(m_pixel_path.at(i - 1), m_map);
geometry_msgs::Point p =
map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
if (map_tools::distance(m_robot_pose.position, p) >
m_check_path_max_distance * 2) {
if (obstacle_vec.size() > 0) {
m_obstacle_position = calculateMeanPoint(obstacle_vec);
ROS_DEBUG_STREAM(
"found obstacle at: " << m_obstacle_position);
return true;
} else {
return false; return false;
} }
for(int k = 0; k < 4; k++) }
{ for (int k = 0; k < 4; k++) {
geometry_msgs::Point t; geometry_msgs::Point t;
t.x = lp.x + (p.x - lp.x) * k/ 4.0; t.x = lp.x + (p.x - lp.x) * k / 4.0;
t.y = lp.y + (p.y - lp.y) * k/ 4.0; t.y = lp.y + (p.y - lp.y) * k / 4.0;
for(const auto &sp: scan_points) for (const auto& sp : scan_points) {
{ if (map_tools::distance(sp, t) <
if (map_tools::distance(sp, t) < m_AllowedObstacleDistance.first -
m_AllowedObstacleDistance.first - m_map->info.resolution) { m_map->info.resolution) {
if(map_tools::distance(m_robot_pose.position, sp) < m_check_path_max_distance) if (map_tools::distance(m_robot_pose.position, sp) <
{ m_check_path_max_distance) {
m_obstacle_position = t; obstacle_vec.push_back(sp);
ROS_INFO_STREAM("Found obstacle");
return true;
}
} }
} }
} }
...@@ -460,7 +486,13 @@ bool HomerNavigationNode::obstacleOnPath() { ...@@ -460,7 +486,13 @@ bool HomerNavigationNode::obstacleOnPath() {
} }
} }
} }
return false; if (obstacle_vec.size() > 0) {
m_obstacle_position = calculateMeanPoint(obstacle_vec);
ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
return true;
} else {
return false;
}
} }
void HomerNavigationNode::performNextMove() { void HomerNavigationNode::performNextMove() {
...@@ -473,14 +505,11 @@ void HomerNavigationNode::performNextMove() { ...@@ -473,14 +505,11 @@ void HomerNavigationNode::performNextMove() {
(ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) { (ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
if (obstacleOnPath()) { if (obstacleOnPath()) {
if (m_seen_obstacle_before) { if (m_seen_obstacle_before) {
if(!m_obstacle_on_path) if (!m_obstacle_on_path) {
{
stopRobot(); stopRobot();
calculatePath(); calculatePath();
return; return;
} } else {
else
{
calculatePath(); calculatePath();
} }
} }
...@@ -594,18 +623,18 @@ void HomerNavigationNode::performNextMove() { ...@@ -594,18 +623,18 @@ void HomerNavigationNode::performNextMove() {
} }
} else if (fabs(angle) > m_max_drive_angle) { } else if (fabs(angle) > m_max_drive_angle) {
m_act_speed = 0.0; m_act_speed = 0.0;
} else if (m_obstacle_on_path && map_tools::distance(m_robot_pose.position, m_obstacle_position) < 1.0 ) } else if (m_obstacle_on_path &&
{ map_tools::distance(m_robot_pose.position,
m_obstacle_position) < 1.0) {
m_act_speed = 0.0; m_act_speed = 0.0;
float angleToWaypoint2 = angleToPointDeg(m_obstacle_position); float angleToWaypoint2 = angleToPointDeg(m_obstacle_position);
if (angleToWaypoint2 < -180) { if (angleToWaypoint2 < -180) {
angleToWaypoint2 += 360; angleToWaypoint2 += 360;
} }
angle = deg2Rad(angleToWaypoint2); angle = deg2Rad(angleToWaypoint2);
if(std::fabs(angle) < m_min_turn_angle) if (std::fabs(angle) < m_min_turn_angle) {
{ ROS_INFO_STREAM("angle = " << angle);
ROS_INFO_STREAM("angle = "<<angle);
m_avoided_collision = true; m_avoided_collision = true;
m_initial_path_reaches_target = true; m_initial_path_reaches_target = true;
m_stop_before_obstacle = true; m_stop_before_obstacle = true;
...@@ -644,10 +673,12 @@ void HomerNavigationNode::performNextMove() { ...@@ -644,10 +673,12 @@ void HomerNavigationNode::performNextMove() {
m_max_move_speed * obstacleMapDistance * m_map_speed_factor; m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
float max_waypoint_speed = 1; float max_waypoint_speed = 1;
if(m_waypoints.size() > 1) if (m_waypoints.size() > 1) {
{ float angleToNextWaypoint =
float angleToNextWaypoint = angleToPointDeg(m_waypoints[1].pose.position); angleToPointDeg(m_waypoints[1].pose.position);
max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) * distanceToWaypoint * m_waypoint_speed_factor; max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) *
distanceToWaypoint *
m_waypoint_speed_factor;
} }
m_act_speed = std::min( m_act_speed = std::min(
...@@ -1028,10 +1059,8 @@ void HomerNavigationNode::poseCallback( ...@@ -1028,10 +1059,8 @@ void HomerNavigationNode::poseCallback(
} }
void HomerNavigationNode::calcMaxMoveDist() { void HomerNavigationNode::calcMaxMoveDist() {
m_max_move_distance = 99; m_max_move_distance = 99;
for(auto d: m_max_move_distances) for (auto d : m_max_move_distances) {
{
m_max_move_distance = std::min(m_max_move_distance, d.second); m_max_move_distance = std::min(m_max_move_distance, d.second);
} }
if (m_max_move_distance <= m_collision_distance && if (m_max_move_distance <= m_collision_distance &&
...@@ -1047,8 +1076,9 @@ void HomerNavigationNode::calcMaxMoveDist() { ...@@ -1047,8 +1076,9 @@ void HomerNavigationNode::calcMaxMoveDist() {
} }
} }
void HomerNavigationNode::ignoreLaserCallback(const std_msgs::String::ConstPtr& msg){ void HomerNavigationNode::ignoreLaserCallback(
ROS_INFO_STREAM("changed ignore laser to: "<<msg->data); const std_msgs::String::ConstPtr& msg) {
ROS_INFO_STREAM("changed ignore laser to: " << msg->data);
m_ignore_scan = msg->data; m_ignore_scan = msg->data;
} }
...@@ -1058,35 +1088,22 @@ void HomerNavigationNode::maxDepthMoveDistanceCallback( ...@@ -1058,35 +1088,22 @@ void HomerNavigationNode::maxDepthMoveDistanceCallback(
calcMaxMoveDist(); calcMaxMoveDist();
} }
void HomerNavigationNode::processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) void HomerNavigationNode::laserDataCallback(
{ const sensor_msgs::LaserScan::ConstPtr& msg) {
m_scan_map[msg->header.frame_id] = msg; m_scan_map[msg->header.frame_id] = msg;
m_last_laser_time = ros::Time::now(); m_last_laser_time = ros::Time::now();
if(m_MainMachine.state() != IDLE) if (m_MainMachine.state() != IDLE) {
{ if (!isInIgnoreList(msg->header.frame_id)) {
if(!isInIgnoreList(msg->header.frame_id)) m_max_move_distances[msg->header.frame_id] =
{ map_tools::get_max_move_distance(
m_max_move_distances[msg->header.frame_id] = map_tools::get_max_move_distance( map_tools::laser_msg_to_points(msg, m_transform_listener,
map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"), "/base_link"),
m_min_x, m_min_y); m_min_x, m_min_y);
} } else {
else
{
m_max_move_distances[msg->header.frame_id] = 99; m_max_move_distances[msg->header.frame_id] = 99;
} }
calcMaxMoveDist(); calcMaxMoveDist();
} }
}
void HomerNavigationNode::laserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
processLaserScan(msg);
}
void HomerNavigationNode::downlaserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
processLaserScan(msg);
} }
void HomerNavigationNode::initNewTarget() { void HomerNavigationNode::initNewTarget() {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment