diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 1e58a7278480b6d6b9054463e2d50bdcb08f905a..2f6508c36aec1c280dcdd75624d45dc05a0f42fc 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -8,663 +8,620 @@ const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
const float M_2PI = 2 * M_PI;
-SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( particleNum )
-{
-
- m_OccupancyMap = new OccupancyMap();
- // generate initial particles
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- m_CurrentList[i] = new SlamParticle();
- m_LastList[i] = new SlamParticle();
- }
-
- 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 = true;
- m_DoMapping = true;
-
- m_EffectiveParticleNum = m_ParticleNum;
- m_LastUpdateTime = ros::Time(0);
- m_LastMoveTime = ros::Time::now();
+SlamFilter::SlamFilter(int particleNum)
+ : ParticleFilter<SlamParticle>(particleNum) {
+ m_OccupancyMap = new OccupancyMap();
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++) {
+ m_CurrentList[i] = new SlamParticle();
+ m_LastList[i] = new SlamParticle();
+ }
+
+ 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 = 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 )
-{
- m_OccupancyMap = new OccupancyMap ( * ( slamFilter.m_OccupancyMap ) );
- // generate initial particles
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- if ( slamFilter.m_CurrentList[i] == 0 )
- {
- m_CurrentList[i]=0;
- }
- else
- {
- m_CurrentList[i] = new SlamParticle ( * ( slamFilter.m_CurrentList[i] ) );
- }
- if ( slamFilter.m_LastList[i] == 0 )
- {
- m_LastList[i]=0;
- }
- else
- {
- m_LastList[i] = new SlamParticle ( * ( slamFilter.m_LastList[i] ) );
+SlamFilter::SlamFilter(SlamFilter& slamFilter)
+ : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum) {
+ m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++) {
+ if (slamFilter.m_CurrentList[i] == 0) {
+ m_CurrentList[i] = 0;
+ } else {
+ m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[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()
-{
- if ( 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;
+SlamFilter::~SlamFilter() {
+ if (m_OccupancyMap) {
+ delete m_OccupancyMap;
}
- if ( m_LastList[i] )
- {
- delete m_LastList[i];
- m_LastList[i] = 0;
+ for (int i = 0; i < m_ParticleNum; i++) {
+ if (m_CurrentList[i]) {
+ delete m_CurrentList[i];
+ m_CurrentList[i] = 0;
+ }
+ if (m_LastList[i]) {
+ delete m_LastList[i];
+ m_LastList[i] = 0;
+ }
}
- }
}
-
-void SlamFilter::setRotationErrorRotating ( float percent )
-{
- m_Alpha1 = percent / 100.0;
+void SlamFilter::setRotationErrorRotating(float percent) {
+ m_Alpha1 = percent / 100.0;
}
-void SlamFilter::resetHigh()
-{
- m_OccupancyMap->resetHighSensitive();
-}
+void SlamFilter::resetHigh() { m_OccupancyMap->resetHighSensitive(); }
-void SlamFilter::setRotationErrorTranslating ( float degreePerMeter )
-{
- m_Alpha2 = degreePerMeter / 180.0 * M_PI;
+void SlamFilter::setRotationErrorTranslating(float degreePerMeter) {
+ m_Alpha2 = degreePerMeter / 180.0 * M_PI;
}
-void SlamFilter::setTranslationErrorTranslating ( float percent )
-{
- m_Alpha3 = percent / 100.0;
+void SlamFilter::setTranslationErrorTranslating(float percent) {
+ m_Alpha3 = percent / 100.0;
}
-void SlamFilter::setTranslationErrorRotating ( float mPerDegree )
-{
- m_Alpha4 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setTranslationErrorRotating(float mPerDegree) {
+ m_Alpha4 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
-{
- m_Alpha5 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setMoveJitterWhileTurning(float mPerDegree) {
+ m_Alpha5 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setScanMatchingClusterSize ( float minClusterSize )
-{
- minClusterSize = minClusterSize;
+void SlamFilter::setScanMatchingClusterSize(float minClusterSize) {
+ minClusterSize = minClusterSize;
}
-void SlamFilter::setMapping ( bool doMapping )
-{
- m_DoMapping = doMapping;
-}
+void SlamFilter::setMapping(bool doMapping) { m_DoMapping = doMapping; }
-void SlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap )
-{
- //delete old
- if ( m_OccupancyMap )
- {
- delete m_OccupancyMap;
- }
- //copy
- m_OccupancyMap = occupancyMap;
+void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap) {
+ // delete old
+ if (m_OccupancyMap) {
+ delete m_OccupancyMap;
+ }
+ // copy
+ m_OccupancyMap = occupancyMap;
}
-
-vector<Pose>* SlamFilter::getParticlePoses() const
-{
- vector<Pose>* particlePoses = new vector<Pose>();
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- float robotX, robotY, robotTheta;
- SlamParticle* particle = m_CurrentList[i];
- particle->getRobotPose ( robotX, robotY, robotTheta );
- particlePoses->push_back ( Pose ( robotX, robotY, robotTheta ) );
- }
- return particlePoses;
+vector<Pose>* SlamFilter::getParticlePoses() const {
+ vector<Pose>* particlePoses = new vector<Pose>();
+ for (int i = 0; i < m_ParticleNum; i++) {
+ float robotX, robotY, robotTheta;
+ SlamParticle* particle = m_CurrentList[i];
+ particle->getRobotPose(robotX, robotY, robotTheta);
+ particlePoses->push_back(Pose(robotX, robotY, robotTheta));
+ }
+ return particlePoses;
}
-vector<SlamParticle*>* SlamFilter::getParticles() const
-{
- vector<SlamParticle*>* particles = new vector<SlamParticle*>();
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- SlamParticle* particle = m_CurrentList[i];
- particles->push_back ( particle );
- }
- return particles;
+vector<SlamParticle*>* SlamFilter::getParticles() const {
+ vector<SlamParticle*>* particles = new vector<SlamParticle*>();
+ for (int i = 0; i < m_ParticleNum; i++) {
+ SlamParticle* particle = m_CurrentList[i];
+ particles->push_back(particle);
+ }
+ return particles;
}
-void SlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta )
-{
- // set first particle to exact position
- m_CurrentList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() );
- m_LastList[0]->setRobotPose ( pose.x(), pose.y(), pose.theta() );
- // scatter remaining particles
- for ( int i = 1; i < m_ParticleNum; ++i )
- {
- const double scatterX = randomGauss() * scatterVarXY;
- const double scatterY = randomGauss() * scatterVarXY;
- const double scatterTheta = randomGauss() * scatterVarTheta;
-
- m_CurrentList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta );
- m_LastList[i]->setRobotPose ( pose.x()+scatterX, pose.y()+scatterY, pose.theta()+scatterTheta );
- }
+void SlamFilter::setRobotPose(Pose pose, double scatterVarXY,
+ double scatterVarTheta) {
+ // set first particle to exact position
+ m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ // scatter remaining particles
+ for (int i = 1; i < m_ParticleNum; ++i) {
+ const double scatterX = randomGauss() * scatterVarXY;
+ const double scatterY = randomGauss() * scatterVarXY;
+ const double scatterTheta = randomGauss() * scatterVarTheta;
+
+ m_CurrentList[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> particleWeights ( m_ParticleNum );
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- particleWeights[i] = m_CurrentList[i]->getWeight();
- }
- return particleWeights;
+vector<float> SlamFilter::getParticleWeights() const {
+ vector<float> particleWeights(m_ParticleNum);
+ for (int i = 0; i < m_ParticleNum; i++) {
+ particleWeights[i] = m_CurrentList[i]->getWeight();
+ }
+ return particleWeights;
}
-double SlamFilter::randomGauss ( float variance ) const
-{
- if ( variance < 0 )
- {
- variance = -variance;
- }
- double x1, x2, w, y1;
- do
- {
- x1 = 2.0 * random01() - 1.0;
- x2 = 2.0 * random01() - 1.0;
- w = x1 * x1 + x2 * x2;
- }
- while ( w >= 1.0 );
-
- w = sqrt ( ( -2.0 * log ( w ) ) / w );
- y1 = x1 * w;
- // now y1 is uniformly distributed
- return sqrt ( variance ) * y1;
+double SlamFilter::randomGauss(float variance) const {
+ if (variance < 0) {
+ variance = -variance;
+ }
+ double x1, x2, w, y1;
+ do {
+ x1 = 2.0 * random01() - 1.0;
+ x2 = 2.0 * random01() - 1.0;
+ w = x1 * x1 + x2 * x2;
+ } while (w >= 1.0);
+
+ 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 )
-{
- if ( rawData->ranges.size() < 2 )
- {
- return rawData->ranges;
- }
- vector<float> filteredData = rawData->ranges;
- 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 )
- {
- filteredData[i] = 0;
+vector<float> SlamFilter::filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
+ float maxDiff) {
+ if (rawData->ranges.size() < 2) {
+ return rawData->ranges;
+ }
+ vector<float> filteredData = rawData->ranges;
+ 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) {
+ 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 )
- {
- 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;
+
+ return filteredData;
}
-void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &FilterDuration)
-{
- // if first run, initialize data
- if ( m_FirstRun )
- {
- m_FirstRun = false;
- // 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));
- m_OccupancyMap->insertLaserData ( laserData , transform);
+void SlamFilter::filter(Pose currentPose,
+ sensor_msgs::LaserScanConstPtr laserData,
+ ros::Time measurementTime,
+ ros::Duration& FilterDuration) {
+ // if first run, initialize data
+ if (m_FirstRun) {
+ m_FirstRun = false;
+ // 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));
+ 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_ReferencePoseOdometry = currentPose;
- m_ReferenceMeasurementTime = measurementTime;
-
- measure();
- ROS_INFO_STREAM("first run!");
- normalize();
- sort ( 0, m_ParticleNum - 1 );
- return;
- }
- //m_CurrentLaserConfig = laserConf;
- m_CurrentPoseOdometry = currentPose;
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(*laserData); //copy const ptr to be able to change values
- m_CurrentLaserData->ranges = filterOutliers ( laserData, 0.3 );
-
- Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- // 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 )
+ // m_CurrentLaserConfig = laserConf;
+ m_CurrentPoseOdometry = currentPose;
+ m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
+ *laserData); // copy const ptr to be able to change values
+ m_CurrentLaserData->ranges = filterOutliers(laserData, 0.3);
+
+ Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
+
+ bool moving = sqr(trans.x()) + sqr(trans.y()) > 0 || sqr(trans.theta()) > 0;
+
+ // 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 (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
+ // if(false)
{
- updateMap();
- m_LastUpdatePose = likeliestPose;
- m_LastUpdateTime = measurementTime;
+ 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 (moving) {
+ m_LastMoveTime = ros::Time::now();
+ }
+ resample();
+ // filter steps
+ drift();
+ measure();
+ normalize();
+
+ sort(0, m_ParticleNum - 1);
}
- else
- {
- ROS_WARN_STREAM( "No mapping performed, rotation angle too big." );
+
+ 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 =
+ (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.";
}
- }
- else
- {
- stream << "No map update performed.";
- }
- ROS_DEBUG_STREAM( stream.str() );
- // safe last used pose and laserdata as reference
-
- 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:
- * - When the robot starts, the initial orientation may have errors (a few degrees). (m_InitialOrientationError)
- * - 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
+ * For the probabilistic motion model of the robot we use the following three
+ * parameters:
+ * - When the robot starts, the initial orientation may have errors (a few
+ * degrees). (m_InitialOrientationError)
+ * - 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.]
* We use Gaussian-Distributions to estimate the error.
* The expected value of the errors are zero.
*/
-void SlamFilter::drift()
-{
-
- float rx = m_ReferencePoseOdometry.x();
- float ry = m_ReferencePoseOdometry.y();
- float rt = m_ReferencePoseOdometry.theta();
- float cx = m_CurrentPoseOdometry.x();
- float cy = m_CurrentPoseOdometry.y();
- float ct = m_CurrentPoseOdometry.theta();
-
- Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- // find out if driving forward or backward
- bool backwardMove = false;
- float scalar = odoTrans.x() * cosf ( rt ) + odoTrans.y() * sinf ( rt );
- if ( scalar <= 0 )
- {
- backwardMove = true;
- }
- float distance = sqrt ( sqr ( odoTrans.x() ) + sqr ( odoTrans.y() ) );
- float deltaRot1, deltaTrans, deltaRot2;
- if ( distance < sqrt ( MIN_MOVE_DISTANCE2 ) )
- {
- deltaRot1 = odoTrans.theta();
- deltaTrans = 0.0;
- deltaRot2 = 0.0;
- }
- else if ( backwardMove )
- {
- deltaRot1 = atan2 ( ry - cy, rx - cx ) - rt;
- deltaTrans = - distance;
- deltaRot2 = ct - rt - deltaRot1;
- }
- else
- {
- deltaRot1 = atan2 ( odoTrans.y(), odoTrans.x() ) - rt;
- deltaTrans = distance;
- deltaRot2 = ct - rt - deltaRot1;
- }
-
- 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];
+void SlamFilter::drift() {
+ float rx = m_ReferencePoseOdometry.x();
+ float ry = m_ReferencePoseOdometry.y();
+ float rt = m_ReferencePoseOdometry.theta();
+ float cx = m_CurrentPoseOdometry.x();
+ float cy = m_CurrentPoseOdometry.y();
+ float ct = m_CurrentPoseOdometry.theta();
+
+ Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
+
+ // find out if driving forward or backward
+ bool backwardMove = false;
+ float scalar = odoTrans.x() * cosf(rt) + odoTrans.y() * sinf(rt);
+ if (scalar <= 0) {
+ backwardMove = true;
+ }
+ float distance = sqrt(sqr(odoTrans.x()) + sqr(odoTrans.y()));
+ float deltaRot1, deltaTrans, deltaRot2;
+ if (distance < sqrt(MIN_MOVE_DISTANCE2)) {
+ deltaRot1 = odoTrans.theta();
+ deltaTrans = 0.0;
+ deltaRot2 = 0.0;
+ } else if (backwardMove) {
+ deltaRot1 = atan2(ry - cy, rx - cx) - rt;
+ deltaTrans = -distance;
+ deltaRot2 = ct - rt - deltaRot1;
+ } else {
+ deltaRot1 = atan2(odoTrans.y(), odoTrans.x()) - rt;
+ deltaTrans = distance;
+ deltaRot2 = ct - rt - deltaRot1;
+ }
+
+ 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 );
+ particle->getRobotPose(robotX, robotY, robotTheta);
+ Pose pose(robotX, robotY, robotTheta);
// move pose
- 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;
-
+ 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
- while ( theta > M_PI ) theta -= M_2PI;
- while ( theta <= -M_PI ) theta += M_2PI;
-
- particle->setRobotPose ( posX, posY, theta );
- }
-
-
-}
-
-
-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
+ 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
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 );
- }
+ particle->getRobotPose(robotX, robotY, robotTheta);
+ Pose pose(robotX, robotY, robotTheta);
+ // move pose
+ 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()
-{
- m_OccupancyMap->insertLaserData ( m_CurrentLaserData, m_latestTransform);
+void SlamFilter::updateMap() {
+ m_OccupancyMap->insertLaserData(m_CurrentLaserData, m_latestTransform);
}
-void SlamFilter::printParticles() const
-{
- cout << endl << "### PARTICLE LIST ###" << endl;
- cout << right << fixed;
- cout.width ( 5 );
- for ( int i = 0; i < m_ParticleNum; i++ )
- {
- SlamParticle* p_particle = m_CurrentList[i];
- if ( p_particle )
- {
- float robotX, robotY, robotTheta;
- p_particle->getRobotPose ( robotX, robotY, robotTheta );
- cout << "Particle " << i << ": (" << robotX << "," << robotY << "," << robotTheta * 180.0 / M_PI << "), weight:\t" << p_particle->getWeight() << endl;
+void SlamFilter::printParticles() const {
+ cout << endl << "### PARTICLE LIST ###" << endl;
+ cout << right << fixed;
+ cout.width(5);
+ for (int i = 0; i < m_ParticleNum; i++) {
+ SlamParticle* p_particle = m_CurrentList[i];
+ if (p_particle) {
+ float robotX, robotY, robotTheta;
+ p_particle->getRobotPose(robotX, robotY, robotTheta);
+ 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 )
-{
- if ( newParticleNum < m_ParticleNum )
- {
+ for (int i = 0; i < newParticleNum; i++) {
+ newCurrentList[i] = m_CurrentList[i];
+ newLastList[i] = m_LastList[i];
+ }
- SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
- SlamParticle** newLastList = new SlamParticle*[newParticleNum];
+ for (int i = newParticleNum + 1; i < m_ParticleNum; i++) {
+ delete m_CurrentList[i];
+ delete m_LastList[i];
+ }
+ delete[] m_CurrentList;
+ delete[] m_LastList;
- for ( int i = 0; i < newParticleNum; i++ )
- {
- newCurrentList[i] = m_CurrentList[i];
- newLastList[i] = m_LastList[i];
- }
+ m_CurrentList = newCurrentList;
+ m_LastList = newLastList;
- for ( int i = newParticleNum + 1; i < m_ParticleNum; i++ )
- {
- delete m_CurrentList[i];
- delete m_LastList[i];
+ m_ParticleNum = newParticleNum;
+ normalize();
}
- delete[] m_CurrentList;
- delete[] m_LastList;
-
- m_CurrentList = newCurrentList;
- m_LastList = newLastList;
-
- m_ParticleNum = newParticleNum;
- normalize();
- }
}
-Pose SlamFilter::getLikeliestPose(ros::Time poseTime)
-{
- float percentage = 0.4; //TODO param? //test
- float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
- int numParticles = static_cast<int> ( percentage / 100 * m_ParticleNum );
- if ( 0 == numParticles )
- {
- numParticles = 1;
- }
- for ( int i = 0; i < numParticles; i++ )
- {
- float robotX, robotY, robotTheta;
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- sumX += robotX;
- sumY += robotY;
- // calculate sum of vectors in unit circle
- sumDirX += cos ( robotTheta );
- sumDirY += sin ( robotTheta );
- }
- float meanTheta = atan2 ( sumDirY, sumDirX );
- float meanX = sumX / numParticles;
- float meanY = sumY / numParticles;
- //broadcast transform map -> base_link
- tf::Transform transform(tf::createQuaternionFromYaw(meanTheta),
- tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
- tf::TransformBroadcaster tfBroadcaster;
- m_latestTransform = transform;
-
- tfBroadcaster.sendTransform(tf::StampedTransform(m_latestTransform, poseTime, "/map", "/base_link"));
- return Pose ( meanX, meanY, meanTheta );
+Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
+ float percentage = 0.4; // TODO param? //test
+ float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
+ int numParticles = static_cast<int>(percentage / 100 * m_ParticleNum);
+ if (0 == numParticles) {
+ numParticles = 1;
+ }
+ for (int i = 0; i < numParticles; i++) {
+ float robotX, robotY, robotTheta;
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ sumX += robotX;
+ sumY += robotY;
+ // calculate sum of vectors in unit circle
+ sumDirX += cos(robotTheta);
+ sumDirY += sin(robotTheta);
+ }
+ float meanTheta = atan2(sumDirY, sumDirX);
+ float meanX = sumX / numParticles;
+ float meanY = sumY / numParticles;
+ // broadcast transform map -> base_link
+ tf::Transform transform(
+ tf::createQuaternionFromYaw(meanTheta),
+ tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
+ tf::TransformBroadcaster tfBroadcaster;
+ m_latestTransform = transform;
+
+ tfBroadcaster.sendTransform(tf::StampedTransform(
+ m_latestTransform, poseTime, "/map", "/base_link"));
+ return Pose(meanX, meanY, meanTheta);
}
-OccupancyMap* SlamFilter::getLikeliestMap() const
-{
- return m_OccupancyMap;
-}
+OccupancyMap* SlamFilter::getLikeliestMap() const { return m_OccupancyMap; }
-void SlamFilter::getPoseVariances ( int particleNum, float& poseVarianceX, float& poseVarianceY )
-{
-
- // the particles of m_CurrentList are sorted by their weights
- if ( particleNum > m_ParticleNum || particleNum <= 0 )
- {
- particleNum = m_ParticleNum;
- }
- // calculate average pose
- float averagePoseX = 0;
- float averagePoseY = 0;
- float robotX = 0.0;
- float robotY = 0.0;
- float robotTheta = 0.0;
- for ( int i = 0; i < particleNum; i++ )
- {
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- averagePoseX += robotX;
- averagePoseY += robotY;
- }
- averagePoseX /= particleNum;
- averagePoseY /= particleNum;
-
- // calculate standard deviation of pose
- poseVarianceX = 0.0;
- poseVarianceY = 0.0;
- for ( int i = 0; i < particleNum; i++ )
- {
- m_CurrentList[i]->getRobotPose ( robotX, robotY, robotTheta );
- poseVarianceX += ( averagePoseX - robotX ) * ( averagePoseX - robotX );
- poseVarianceY += ( averagePoseY - robotY ) * ( averagePoseY - robotY );
- }
- poseVarianceX /= particleNum;
- poseVarianceY /= particleNum;
+void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
+ float& poseVarianceY) {
+ // the particles of m_CurrentList are sorted by their weights
+ if (particleNum > m_ParticleNum || particleNum <= 0) {
+ particleNum = m_ParticleNum;
+ }
+ // calculate average pose
+ float averagePoseX = 0;
+ float averagePoseY = 0;
+ float robotX = 0.0;
+ float robotY = 0.0;
+ float robotTheta = 0.0;
+ for (int i = 0; i < particleNum; i++) {
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ averagePoseX += robotX;
+ averagePoseY += robotY;
+ }
+ averagePoseX /= particleNum;
+ averagePoseY /= particleNum;
+
+ // calculate standard deviation of pose
+ poseVarianceX = 0.0;
+ poseVarianceY = 0.0;
+ for (int i = 0; i < particleNum; i++) {
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
+ poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
+ }
+ poseVarianceX /= particleNum;
+ poseVarianceY /= particleNum;
}
-double SlamFilter::evaluateByContrast()
-{
- return m_OccupancyMap->evaluateByContrast();
+double SlamFilter::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);
}
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 6153613d4187a5118758adc7b4e36345e8f43b30..86014344a5cbb18a80f0138c86dbac93a133d0fc 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -1,129 +1,133 @@
#include <homer_mapping/slam_node.h>
-SlamNode::SlamNode(ros::NodeHandle* nh)
- : m_HyperSlamFilter( 0 )
-{
+SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
init();
// subscribe to topics
- m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>("/scan", 1, &SlamNode::callbackLaserScan, this);
- m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>("/odom", 1, &SlamNode::callbackOdometry, this);
- m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>("/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
- m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/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);
+ m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
+ "/scan", 1, &SlamNode::callbackLaserScan, this);
+ m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
+ "/odom", 1, &SlamNode::callbackOdometry, this);
+ m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
+ "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
+ m_InitialPoseSubscriber =
+ nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
+ "/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
- m_PoseStampedPublisher = 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);
- 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);
-
-//// //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 );
-
+ m_PoseStampedPublisher =
+ 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);
+ 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);
+
+ //// //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);
}
-void SlamNode::init()
-{
+void SlamNode::init() {
double waitTime;
- ros::param::get("/particlefilter/wait_time", waitTime);
+ ros::param::get("/particlefilter/wait_time", waitTime);
m_WaitDuration = ros::Duration(waitTime);
- ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
- ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
+ ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
+ ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
m_DoMapping = true;
int particleNum;
- ros::param::get("/particlefilter/particle_num", particleNum);
+ ros::param::get("/particlefilter/particle_num", particleNum);
int particleFilterNum;
- ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
- m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum );
+ ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
+ particleFilterNum);
+ m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
m_ReferenceOdometryTime = ros::Time(0);
- m_LastMapSendTime = ros::Time(0);
- m_LastPositionSendTime = ros::Time(0);
- m_LastMappingTime = ros::Time(0);
+ m_LastMapSendTime = ros::Time(0);
+ m_LastPositionSendTime = ros::Time(0);
+ m_LastMappingTime = ros::Time(0);
m_ReferenceOdometryTime = ros::Time(0);
-
+
m_laser_queue.clear();
m_odom_queue.clear();
}
-SlamNode::~SlamNode()
-{
- delete m_HyperSlamFilter;
-}
+SlamNode::~SlamNode() { delete m_HyperSlamFilter; }
-void SlamNode::resetMaps()
-{
- 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::resetMaps() {
+ ROS_INFO("Resetting maps..");
-void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
-{
- m_HyperSlamFilter->resetHigh();
+ 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) {
+ m_HyperSlamFilter->resetHigh();
+}
-void SlamNode::sendMapDataMessage(ros::Time mapTime)
-{
- std::vector<int8_t> mapData;
- nav_msgs::MapMetaData metaData;
+void SlamNode::sendMapDataMessage(ros::Time mapTime) {
+ std::vector<int8_t> mapData;
+ nav_msgs::MapMetaData metaData;
- OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
- occMap->getOccupancyProbabilityImage (mapData, metaData);
+ OccupancyMap* occMap =
+ 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;
std_msgs::Header header;
header.stamp = mapTime;
@@ -133,249 +137,244 @@ void SlamNode::sendMapDataMessage(ros::Time mapTime)
mapMsg.data = mapData;
m_SLAMMapPublisher.publish(mapMsg);
- }
}
-void SlamNode::callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg )
-{
- Pose userdef_pose(msg->position.x, msg->position.y, tf::getYaw(msg->orientation));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
+void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg) {
+ Pose userdef_pose(msg->position.x, msg->position.y,
+ tf::getYaw(msg->orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-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));
- m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
+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));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_laser_queue.push_back(msg);
- if(m_laser_queue.size() > 5) //Todo param
- {
- m_laser_queue.erase(m_laser_queue.begin());
- }
+void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) {
+ m_laser_queue.push_back(msg);
+ if (m_laser_queue.size() > 5) // Todo param
+ {
+ m_laser_queue.erase(m_laser_queue.begin());
+ }
}
-void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg)
-{
- m_odom_queue.push_back(msg);
- static int last_i = 0;
- if(m_odom_queue.size() > 5) //Todo param
- {
- last_i--;
- if(last_i < 0)
- {
- last_i = 0;
- }
- m_odom_queue.erase(m_odom_queue.begin());
- }
-
- static Pose last_interpolatedPose(0.0, 0.0, 0.0);
- ros::Time currentOdometryTime = msg->header.stamp;
- 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;
-
- for( i = m_odom_queue.size()-1; i > 0 ; i--)
- {
- //Check if we would repeat a calculation which is already done but still in the queue
- if(m_odom_queue.at(i-1)->header.stamp == m_ReferenceOdometryTime)
- {
- break;
- }
-
- for(j = m_laser_queue.size()-1; j > -1; j--)
- {
- // find a laserscan in between two odometry readings (or at the same time)
- if(
- (m_laser_queue.at(j)->header.stamp >= m_odom_queue.at(i-1)->header.stamp)
- &&
- (m_odom_queue.at(i)->header.stamp >= m_laser_queue.at(j)->header.stamp)
- )
- {
- got_match = true;
- break;
- }
- }
- if(got_match)
- {
- break;
- }
- }
-
- if(got_match)
- {
- last_i = i;
- m_LastLaserData = m_laser_queue.at(j);
- m_LastMappingTime = m_LastLaserData->header.stamp;
-
- float odoX = m_odom_queue.at(i)->pose.pose.position.x;
- float odoY = m_odom_queue.at(i)->pose.pose.position.y;
- float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
- Pose currentOdometryPose ( odoX, odoY, odoTheta );
- currentOdometryTime = m_odom_queue.at(i)->header.stamp;
-
- odoX = m_odom_queue.at(i-1)->pose.pose.position.x;
- odoY = m_odom_queue.at(i-1)->pose.pose.position.y;
- odoTheta = tf::getYaw(m_odom_queue.at(i-1)->pose.pose.orientation);
- Pose lastOdometryPose ( odoX, odoY, odoTheta );
- m_ReferenceOdometryPose = lastOdometryPose;
- m_ReferenceOdometryTime = m_odom_queue.at(i-1)->header.stamp;
-
-
- ros::Time startTime = ros::Time::now();
- // laserscan in between current odometry reading and m_ReferenceOdometry
- // -> calculate pose of robot during laser scan
- ros::Duration d1 = m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
- ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
-
- float timeFactor;
- if(d1.toSec() == 0.0)
- {
- timeFactor = 0.0f;
- }
- else if(d2.toSec() == 0.0)
- {
- timeFactor = 1.0f;
- }
- else
- {
- timeFactor = d1.toSec() / d2.toSec();
- }
- ros::Duration duration = ros::Duration(0);
-
- last_interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor );
- m_HyperSlamFilter->filter( last_interpolatedPose, m_LastLaserData, m_LastLaserData->header.stamp, duration);
- ros::Time finishTime = ros::Time::now();
-
- m_LastLikeliestPose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(m_LastLaserData->header.stamp);
-
- //TODO löschen
- m_LastPositionSendTime = m_LastLaserData->header.stamp;
- // send map max. every 500 ms
- if ( (m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 )
- {
- sendMapDataMessage(m_LastLaserData->header.stamp);
- m_LastMapSendTime = m_LastLaserData->header.stamp;
- }
-
- ROS_DEBUG_STREAM("Pos. data calc time: " << (finishTime - startTime).toSec() << "s" );
- ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" );
- }
- }
- Pose currentPose = m_LastLikeliestPose;
- //currentPose.setX(currentPose.x() - last_interpolatedPose.x());
- //currentPose.setY(currentPose.y() - last_interpolatedPose.y());
- //currentPose.setTheta(currentPose.theta() - last_interpolatedPose.theta());
- 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.setY(currentPose.y() + m_odom_queue.at(i)->twist.twist.linear.y);
- currentPose.setTheta(currentPose.theta() + m_odom_queue.at(i)->twist.twist.angular.z);
- }
-
- geometry_msgs::PoseStamped poseMsg;
- //header
- poseMsg.header.stamp = msg->header.stamp;
- poseMsg.header.frame_id = "map";
-
- //position and orientation
- poseMsg.pose.position.x = currentPose.x();
- poseMsg.pose.position.y = currentPose.y();
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion
- 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::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
+ m_odom_queue.push_back(msg);
+ static int last_i = 0;
+ if (m_odom_queue.size() > 5) // Todo param
+ {
+ last_i--;
+ if (last_i < 0) {
+ last_i = 0;
+ }
+ m_odom_queue.erase(m_odom_queue.begin());
+ }
+
+ static Pose last_interpolatedPose(0.0, 0.0, 0.0);
+ ros::Time currentOdometryTime = msg->header.stamp;
+ 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;
+
+ for (i = m_odom_queue.size() - 1; i > 0; i--) {
+ // Check if we would repeat a calculation which is already done but
+ // still in the queue
+ if (m_odom_queue.at(i - 1)->header.stamp ==
+ m_ReferenceOdometryTime) {
+ break;
+ }
+
+ for (j = m_laser_queue.size() - 1; j > -1; j--) {
+ // find a laserscan in between two odometry readings (or at
+ // the same time)
+ if ((m_laser_queue.at(j)->header.stamp >=
+ m_odom_queue.at(i - 1)->header.stamp) &&
+ (m_odom_queue.at(i)->header.stamp >=
+ m_laser_queue.at(j)->header.stamp)) {
+ got_match = true;
+ break;
+ }
+ }
+ if (got_match) {
+ break;
+ }
+ }
+
+ if (got_match) {
+ last_i = i;
+ m_LastLaserData = m_laser_queue.at(j);
+ m_LastMappingTime = m_LastLaserData->header.stamp;
+
+ float odoX = m_odom_queue.at(i)->pose.pose.position.x;
+ float odoY = m_odom_queue.at(i)->pose.pose.position.y;
+ float odoTheta =
+ tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
+ Pose currentOdometryPose(odoX, odoY, odoTheta);
+ currentOdometryTime = m_odom_queue.at(i)->header.stamp;
+
+ odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
+ odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
+ odoTheta =
+ tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
+ Pose lastOdometryPose(odoX, odoY, odoTheta);
+ m_ReferenceOdometryPose = lastOdometryPose;
+ m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
+
+ ros::Time startTime = ros::Time::now();
+ // laserscan in between current odometry reading and
+ // m_ReferenceOdometry
+ // -> calculate pose of robot during laser scan
+ ros::Duration d1 =
+ m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
+ ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
+
+ float timeFactor;
+ if (d1.toSec() == 0.0) {
+ timeFactor = 0.0f;
+ } else if (d2.toSec() == 0.0) {
+ timeFactor = 1.0f;
+ } else {
+ timeFactor = d1.toSec() / d2.toSec();
+ }
+ ros::Duration duration = ros::Duration(0);
+
+ last_interpolatedPose = m_ReferenceOdometryPose.interpolate(
+ currentOdometryPose, timeFactor);
+ m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
+ m_LastLaserData->header.stamp, duration);
+ ros::Time finishTime = ros::Time::now();
+
+ m_LastLikeliestPose =
+ m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
+ m_LastLaserData->header.stamp);
+
+ // TODO löschen
+ m_LastPositionSendTime = m_LastLaserData->header.stamp;
+ // send map max. every 500 ms
+ if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() >
+ 0.5) {
+ sendMapDataMessage(m_LastLaserData->header.stamp);
+ m_LastMapSendTime = m_LastLaserData->header.stamp;
+ }
+
+ ROS_DEBUG_STREAM("Pos. data calc time: "
+ << (finishTime - startTime).toSec() << "s");
+ ROS_DEBUG_STREAM("Map send Interval: "
+ << (finishTime - m_LastPositionSendTime).toSec()
+ << "s");
+ }
+ }
+ Pose currentPose = m_LastLikeliestPose;
+ // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
+ // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
+ // currentPose.setTheta(currentPose.theta() -
+ // last_interpolatedPose.theta());
+ 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.setY(currentPose.y() +
+ m_odom_queue.at(i)->twist.twist.linear.y);
+ currentPose.setTheta(currentPose.theta() +
+ m_odom_queue.at(i)->twist.twist.angular.z);
+ }
+
+ geometry_msgs::PoseStamped poseMsg;
+ // header
+ poseMsg.header.stamp = msg->header.stamp;
+ poseMsg.header.frame_id = "map";
+
+ // position and orientation
+ poseMsg.pose.position.x = currentPose.x();
+ poseMsg.pose.position.y = currentPose.y();
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to geometry_msgs::Quaternion
+ 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_HyperSlamFilter->setMapping ( m_DoMapping );
- ROS_INFO_STREAM( "Do mapping is set to " << ( m_DoMapping ) );
+ m_HyperSlamFilter->setMapping(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();
}
-void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
-{
+void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
float res = msg->info.resolution;
- int height = msg->info.height; // cell size
- int width = msg->info.width; //cell size
- //if(height!=width) {
- //ROS_ERROR("Height != width in loaded map");
- //return;
+ int height = msg->info.height; // cell size
+ int width = msg->info.width; // cell size
+ // if(height!=width) {
+ // ROS_ERROR("Height != width in loaded map");
+ // 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()];
- //generate exploredRegion
- int minX = INT_MIN;
- int minY = INT_MIN;
- int maxX = INT_MAX;
- int maxY = INT_MAX;
- for(size_t y = 0; y < msg->info.height; y++)
- {
- int yOffset = msg->info.width * y;
- for(size_t x = 0; x < msg->info.width; x++)
- {
- int i = yOffset + x;
- if(msg->data[i] == -1 )
- map[i] = 0.5;
- else
- map[i] = msg->data[i]/100.0;
-
- if(map[i]!=0.5) {
- if(minX==INT_MIN || minX > (int)x)
- minX = (int)x;
- if(minY==INT_MIN || minY > (int)y)
- minY = (int)y;
- if(maxX==INT_MAX || maxX < (int)x)
- maxX = (int)x;
- if(maxY==INT_MAX || maxY < (int)y)
- maxY = (int)y;
- }
- }
- }
- Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY );
- 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" );
+ // generate exploredRegion
+ int minX = INT_MIN;
+ int minY = INT_MIN;
+ int maxX = INT_MAX;
+ int maxY = INT_MAX;
+ for (size_t y = 0; y < msg->info.height; y++) {
+ int yOffset = msg->info.width * y;
+ for (size_t x = 0; x < msg->info.width; x++) {
+ int i = yOffset + x;
+ if (msg->data[i] == -1)
+ map[i] = 0.5;
+ else
+ map[i] = msg->data[i] / 100.0;
+
+ if (map[i] != 0.5) {
+ if (minX == INT_MIN || minX > (int)x) minX = (int)x;
+ if (minY == INT_MIN || minY > (int)y) minY = (int)y;
+ if (maxX == INT_MAX || maxX < (int)x) maxX = (int)x;
+ if (maxY == INT_MAX || maxY < (int)y) maxY = (int)y;
+ }
+ }
+ }
+ Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
+ 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);
}
/**
* @brief main function
*/
-int main(int argc, char** argv)
-{
+int main(int argc, char** argv) {
ros::init(argc, argv, "homer_mapping");
ros::NodeHandle nh;
SlamNode slamNode(&nh);
- ros::Rate loop_rate(160);
-
- while(ros::ok())
- {
+ ros::Rate loop_rate(160);
+ while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
-
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index 5c594333bc2906b9d686d899f74b2dd04ff4ef10..5765fae5f6d3415c4b29078493c363d9e63b698c 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -2,10 +2,10 @@
#define FastNavigationModule_H
#include <cmath>
+#include <regex>
#include <sstream>
#include <string>
#include <vector>
-#include <regex>
#include <ros/package.h>
#include <ros/ros.h>
@@ -66,7 +66,6 @@ class HomerNavigationNode {
void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg);
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
- void downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void startNavigationCallback(
const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
void moveBaseSimpleGoalCallback(
@@ -85,12 +84,13 @@ class HomerNavigationNode {
virtual void init();
void initNewTarget();
- void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
private:
/** @brief Start navigation to m_Target on last_map_data_ */
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_ */
bool obstacleOnPath();
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 66a8372c5b0643f3bc3f410d5417742e4b03ad3f..89646bc6eb563f6889758be044a7c37c61cc30b2 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -11,7 +11,7 @@ HomerNavigationNode::HomerNavigationNode() {
m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
"/scan", 1, &HomerNavigationNode::laserDataCallback, this);
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>(
"/homer_navigation/start_navigation", 1,
&HomerNavigationNode::startNavigationCallback, this);
@@ -34,8 +34,9 @@ HomerNavigationNode::HomerNavigationNode() {
m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
"/homer_navigation/max_depth_move_distance", 1,
&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 =
nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3);
@@ -156,9 +157,9 @@ void HomerNavigationNode::init() {
m_ignore_scan = "";
m_obstacle_on_path = false;
- m_obstacle_position.x = 0;
- m_obstacle_position.y = 0;
- m_obstacle_position.z = 0;
+ m_obstacle_position.x = 0;
+ m_obstacle_position.y = 0;
+ m_obstacle_position.z = 0;
nav_msgs::OccupancyGrid tmp_map;
tmp_map.header.frame_id = "/map";
@@ -209,8 +210,8 @@ void HomerNavigationNode::idleProcess() {
}
}
-bool HomerNavigationNode::isInIgnoreList(std::string frame_id){
- std::regex reg("(^|\\s)"+frame_id+"(\\s|$)");
+bool HomerNavigationNode::isInIgnoreList(std::string frame_id) {
+ std::regex reg("(^|\\s)" + frame_id + "(\\s|$)");
return regex_match(m_ignore_scan, reg);
}
@@ -218,21 +219,24 @@ void HomerNavigationNode::setExplorerMap() {
// adding lasers to map
nav_msgs::OccupancyGrid temp_map = *m_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;
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) {
- 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();
- 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;
}
}
}
}
if (m_fast_path_planning) {
- maskMap(temp_map);
+ //TODO check why not functional
+ //maskMap(temp_map);
}
m_explorer->setOccupancyMap(
boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
@@ -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() {
m_last_check_path_time = ros::Time::now();
- if (m_pixel_path.size() != 0) {
- for (auto const& scan : m_scan_map) {
- if(!isInIgnoreList(scan.second->header.frame_id)){
- std::vector<geometry_msgs::Point> scan_points;
- scan_points = map_tools::laser_msg_to_points(
- scan.second, m_transform_listener, "/map");
-
- for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
- 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 (m_pixel_path.size() == 0) {
+ ROS_DEBUG_STREAM("no path found for finding an obstacle");
+ return false;
+ }
+ std::vector<geometry_msgs::Point> obstacle_vec;
+
+ for (auto const& scan : m_scan_map) {
+ if (!isInIgnoreList(scan.second->header.frame_id)) {
+ std::vector<geometry_msgs::Point> scan_points;
+ scan_points = map_tools::laser_msg_to_points(
+ scan.second, m_transform_listener, "/map");
+
+ for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
+ 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;
}
- for(int k = 0; k < 4; k++)
- {
- geometry_msgs::Point t;
- t.x = lp.x + (p.x - lp.x) * k/ 4.0;
- t.y = lp.y + (p.y - lp.y) * k/ 4.0;
- for(const auto &sp: scan_points)
- {
- if (map_tools::distance(sp, t) <
- m_AllowedObstacleDistance.first - m_map->info.resolution) {
- if(map_tools::distance(m_robot_pose.position, sp) < m_check_path_max_distance)
- {
- m_obstacle_position = t;
- ROS_INFO_STREAM("Found obstacle");
- return true;
- }
+ }
+ for (int k = 0; k < 4; k++) {
+ geometry_msgs::Point t;
+ t.x = lp.x + (p.x - lp.x) * k / 4.0;
+ t.y = lp.y + (p.y - lp.y) * k / 4.0;
+ for (const auto& sp : scan_points) {
+ if (map_tools::distance(sp, t) <
+ m_AllowedObstacleDistance.first -
+ m_map->info.resolution) {
+ if (map_tools::distance(m_robot_pose.position, sp) <
+ m_check_path_max_distance) {
+ obstacle_vec.push_back(sp);
}
}
}
@@ -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() {
@@ -473,14 +505,11 @@ void HomerNavigationNode::performNextMove() {
(ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
if (obstacleOnPath()) {
if (m_seen_obstacle_before) {
- if(!m_obstacle_on_path)
- {
+ if (!m_obstacle_on_path) {
stopRobot();
calculatePath();
return;
- }
- else
- {
+ } else {
calculatePath();
}
}
@@ -594,18 +623,18 @@ void HomerNavigationNode::performNextMove() {
}
} else if (fabs(angle) > m_max_drive_angle) {
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;
float angleToWaypoint2 = angleToPointDeg(m_obstacle_position);
if (angleToWaypoint2 < -180) {
angleToWaypoint2 += 360;
}
angle = deg2Rad(angleToWaypoint2);
-
- if(std::fabs(angle) < m_min_turn_angle)
- {
- ROS_INFO_STREAM("angle = "<<angle);
+
+ if (std::fabs(angle) < m_min_turn_angle) {
+ ROS_INFO_STREAM("angle = " << angle);
m_avoided_collision = true;
m_initial_path_reaches_target = true;
m_stop_before_obstacle = true;
@@ -644,10 +673,12 @@ void HomerNavigationNode::performNextMove() {
m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
float max_waypoint_speed = 1;
- if(m_waypoints.size() > 1)
- {
- float angleToNextWaypoint = angleToPointDeg(m_waypoints[1].pose.position);
- max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) * distanceToWaypoint * m_waypoint_speed_factor;
+ if (m_waypoints.size() > 1) {
+ float angleToNextWaypoint =
+ angleToPointDeg(m_waypoints[1].pose.position);
+ max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) *
+ distanceToWaypoint *
+ m_waypoint_speed_factor;
}
m_act_speed = std::min(
@@ -1028,10 +1059,8 @@ void HomerNavigationNode::poseCallback(
}
void HomerNavigationNode::calcMaxMoveDist() {
-
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);
}
if (m_max_move_distance <= m_collision_distance &&
@@ -1047,8 +1076,9 @@ void HomerNavigationNode::calcMaxMoveDist() {
}
}
-void HomerNavigationNode::ignoreLaserCallback(const std_msgs::String::ConstPtr& msg){
- ROS_INFO_STREAM("changed ignore laser to: "<<msg->data);
+void HomerNavigationNode::ignoreLaserCallback(
+ const std_msgs::String::ConstPtr& msg) {
+ ROS_INFO_STREAM("changed ignore laser to: " << msg->data);
m_ignore_scan = msg->data;
}
@@ -1058,35 +1088,22 @@ void HomerNavigationNode::maxDepthMoveDistanceCallback(
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_last_laser_time = ros::Time::now();
- if(m_MainMachine.state() != IDLE)
- {
- if(!isInIgnoreList(msg->header.frame_id))
- {
- m_max_move_distances[msg->header.frame_id] = map_tools::get_max_move_distance(
- map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"),
+ if (m_MainMachine.state() != IDLE) {
+ if (!isInIgnoreList(msg->header.frame_id)) {
+ m_max_move_distances[msg->header.frame_id] =
+ map_tools::get_max_move_distance(
+ map_tools::laser_msg_to_points(msg, m_transform_listener,
+ "/base_link"),
m_min_x, m_min_y);
- }
- else
- {
+ } else {
m_max_move_distances[msg->header.frame_id] = 99;
}
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() {