crsm_slam: crsm_slam.cpp Source File

Go to the documentation of this file.
00001 /*
00002  *      This file is part of CrsmSlam.
00003     CrsmSlam is free software: you can redistribute it and/or modify 
00004     it under the terms of the GNU General Public License as published by
00005     the Free Software Foundation, either version 3 of the License, or
00006     (at your option) any later version.
00007 
00008     CrsmSlam is distributed in the hope that it will be useful,
00009     but WITHOUT ANY WARRANTY; without even the implied warranty of
00010     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011     GNU General Public License for more details.
00012 
00013     You should have received a copy of the GNU General Public License
00014     along with CrsmSlam.  If not, see <http://www.gnu.org/licenses/>.
00015 * 
00016 * Author : Manos Tsardoulias, etsardou@gmail.com
00017 * Organization : AUTH, PANDORA Robotics Team
00018 * */
00019 
00020 #include "crsm_slam/crsm_slam.h"
00021 
00022 using namespace std;
00023 
00024 namespace crsm_slam{
00025 
00032 CrsmSlam::CrsmSlam(int argc, char **argv){
00033 
00034         robotPose.x=0;
00035         robotPose.y=0;
00036         robotPose.theta=0;
00037         
00038         updateParameters();
00039 
00040         bestTransformation.dx=bestTransformation.dy=bestTransformation.dth=0;
00041         map=CrsmMap(slamParams.map_size);
00042         int initialPatchWidth=slamParams.robot_width/slamParams.ocgd/2;
00043         int initialPatchLength=slamParams.robot_length/slamParams.ocgd/2;
00044         for(int i=-initialPatchLength;i<initialPatchLength;i++)
00045                 for(int j=-initialPatchWidth;j<initialPatchWidth;j++)
00046                         map.p[i+map.info.originx-(int)(slamParams.dx_laser_robotCenter/slamParams.ocgd)][j+map.info.originy]=200;
00047 
00048         _pathPublishingTimer = n.createTimer(ros::Duration(1.0/slamParams.trajectory_freq),&CrsmSlam::publishTrajectory,this,false,false);
00049         
00050         _robotPosePublishingTimer = n.createTimer(ros::Duration(1.0/slamParams.robot_pose_tf_freq),&CrsmSlam::publishRobotPoseTf,this,false,false);
00051         _robotPosePublishingTimer.start();
00052         
00053         _mapPublishingTimer = n.createTimer(ros::Duration(1.0/slamParams.occupancy_grid_map_freq),&CrsmSlam::publishOGM,this,false,false);
00054         
00055         expansion.expansions.insert(std::pair<CrsmDirection,int>(RIGHT,0));
00056         expansion.expansions.insert(std::pair<CrsmDirection,int>(LEFT,0));
00057         expansion.expansions.insert(std::pair<CrsmDirection,int>(UP,0));
00058         expansion.expansions.insert(std::pair<CrsmDirection,int>(DOWN,0));
00059 }
00060 
00065 void CrsmSlam::updateParameters(void){
00066 
00067         if (n.hasParam("/crsm_slam/occupancy_grid_publish_topic")) 
00068                 n.getParam("/crsm_slam/occupancy_grid_publish_topic", slamParams.occupancy_grid_publish_topic);
00069         else {
00070                 ROS_WARN("[CrsmSlam] : Parameter occupancy_grid_publish_topic not found. Using Default");
00071                 slamParams.occupancy_grid_publish_topic = "/crsm_slam/map" ;
00072         }
00073         
00074         if (n.hasParam("/crsm_slam/robot_trajectory_publish_topic")) 
00075                 n.getParam("/crsm_slam/robot_trajectory_publish_topic", slamParams.robot_trajectory_publish_topic);
00076         else {
00077                 ROS_WARN("[CrsmSlam] : Parameter robot_trajectory_publish_topic not found. Using Default");
00078                 slamParams.robot_trajectory_publish_topic = "/crsm_slam/trajectory" ;
00079         }
00080         
00081         if (n.hasParam("/crsm_slam/trajectory_publisher_frame_id")) 
00082                 n.getParam("/crsm_slam/trajectory_publisher_frame_id", slamParams.trajectory_publisher_frame_id);
00083         else {
00084                 ROS_WARN("[CrsmSlam] : Parameter trajectory_publisher_frame_id not found. Using Default");
00085                 slamParams.trajectory_publisher_frame_id = "map" ;
00086         }
00087         
00088         if (n.hasParam("/crsm_slam/laser_subscriber_topic")) 
00089                 n.getParam("/crsm_slam/laser_subscriber_topic", slamParams.laser_subscriber_topic);
00090         else {
00091                 ROS_WARN("[CrsmSlam] : Parameter laser_subscriber_topic not found. Using Default");
00092                 slamParams.laser_subscriber_topic = "/crsm_slam/laser_scan" ;
00093         }
00094         
00095         if (n.hasParam("/crsm_slam/world_frame")) 
00096                 n.getParam("/crsm_slam/world_frame", slamParams.world_frame);
00097         else {
00098                 ROS_WARN("[CrsmSlam] : Parameter world_frame not found. Using Default");
00099                 slamParams.world_frame = "world" ;
00100         }
00101         
00102         if (n.hasParam("/crsm_slam/base_footprint_frame")) 
00103                 n.getParam("/crsm_slam/base_footprint_frame", slamParams.base_footprint_frame);
00104         else {
00105                 ROS_WARN("[CrsmSlam] : Parameter base_footprint_frame not found. Using Default");
00106                 slamParams.base_footprint_frame = "base_footprint_link" ;
00107         }
00108         
00109         if (n.hasParam("/crsm_slam/base_frame")) 
00110                 n.getParam("/crsm_slam/base_frame", slamParams.base_frame);
00111         else {
00112                 ROS_WARN("[CrsmSlam] : Parameter base_frame not found. Using Default");
00113                 slamParams.base_frame = "base_link" ;
00114         }
00115         
00116         if (n.hasParam("/crsm_slam/map_frame")) 
00117                 n.getParam("/crsm_slam/map_frame", slamParams.map_frame);
00118         else {
00119                 ROS_WARN("[CrsmSlam] : Parameter map_frame not found. Using Default");
00120                 slamParams.map_frame = "map" ;
00121         }
00122         
00123         if (n.hasParam("/crsm_slam/laser_frame")) 
00124                 n.getParam("/crsm_slam/laser_frame", slamParams.laser_frame);
00125         else {
00126                 ROS_WARN("[CrsmSlam] : Parameter laser_frame not found. Using Default");
00127                 slamParams.laser_frame = "laser_link" ;
00128         }
00129         
00130         if (n.hasParam("/crsm_slam/hill_climbing_disparity")) 
00131                 n.getParam("/crsm_slam/hill_climbing_disparity", slamParams.disparity);
00132         else {
00133                 ROS_WARN("[CrsmSlam] : Parameter hill_climbing_disparity not found. Using Default");
00134                 slamParams.disparity = 40 ;
00135         }
00136         
00137         if (n.hasParam("/crsm_slam/slam_container_size")) 
00138                 n.getParam("/crsm_slam/slam_container_size", slamParams.map_size);
00139         else {
00140                 ROS_WARN("[CrsmSlam] : Parameter slam_container_size not found. Using Default");
00141                 slamParams.map_size = 500 ;
00142         }
00143         
00144         if (n.hasParam("/crsm_slam/slam_occupancy_grid_dimentionality")) 
00145                 n.getParam("/crsm_slam/slam_occupancy_grid_dimentionality", slamParams.ocgd);
00146         else {
00147                 ROS_WARN("[CrsmSlam] : Parameter slam_occupancy_grid_dimentionality not found. Using Default");
00148                 slamParams.ocgd = 0.02 ;
00149         }
00150         
00151         if (n.hasParam("/crsm_slam/map_update_density")) 
00152                 n.getParam("/crsm_slam/map_update_density", slamParams.density);
00153         else {
00154                 ROS_WARN("[CrsmSlam] : Parameter map_update_density not found. Using Default");
00155                 slamParams.density = 30.0 ;
00156         }
00157         
00158         if (n.hasParam("/crsm_slam/map_update_obstacle_density")) 
00159                 n.getParam("/crsm_slam/map_update_obstacle_density", slamParams.obstacle_density);
00160         else {
00161                 ROS_WARN("[CrsmSlam] : Parameter map_update_obstacle_density not found. Using Default");
00162                 slamParams.obstacle_density = 3.0 ;
00163         }
00164         
00165         if (n.hasParam("/crsm_slam/scan_density_lower_boundary")) 
00166                 n.getParam("/crsm_slam/scan_density_lower_boundary", slamParams.scan_selection_meters);
00167         else {
00168                 ROS_WARN("[CrsmSlam] : Parameter scan_density_lower_boundary not found. Using Default");
00169                 slamParams.scan_selection_meters = 0.3 ;
00170         }
00171         
00172         if (n.hasParam("/crsm_slam/max_hill_climbing_iterations")) 
00173                 n.getParam("/crsm_slam/max_hill_climbing_iterations", slamParams.max_hill_climbing_iterations);
00174         else {
00175                 ROS_WARN("[CrsmSlam] : Parameter max_hill_climbing_iterations not found. Using Default");
00176                 slamParams.max_hill_climbing_iterations = 40000 ;
00177         }
00178         
00179         if (n.hasParam("/crsm_slam/occupancy_grid_map_freq")) 
00180                 n.getParam("/crsm_slam/occupancy_grid_map_freq", slamParams.occupancy_grid_map_freq);
00181         else {
00182                 ROS_WARN("[CrsmSlam] : Parameter occupancy_grid_map_freq not found. Using Default");
00183                 slamParams.occupancy_grid_map_freq = 1.0 ;
00184         }
00185         
00186         if (n.hasParam("/crsm_slam/robot_pose_tf_freq")) 
00187                 n.getParam("/crsm_slam/robot_pose_tf_freq", slamParams.robot_pose_tf_freq);
00188         else {
00189                 ROS_WARN("[CrsmSlam] : Parameter robot_pose_tf_freq not found. Using Default");
00190                 slamParams.robot_pose_tf_freq = 5.0 ;
00191         }
00192         
00193         if (n.hasParam("/crsm_slam/trajectory_freq")) 
00194                 n.getParam("/crsm_slam/trajectory_freq", slamParams.trajectory_freq);
00195         else {
00196                 ROS_WARN("[CrsmSlam] : Parameter trajectory_freq not found. Using Default");
00197                 slamParams.trajectory_freq = 1.0 ;
00198         }
00199 
00200         // Try to find distance between laser and robot center, else initialize to zero
00201         tf::StampedTransform tfTransform;
00202         try {
00203                 _listener.waitForTransform(slamParams.base_frame, slamParams.laser_frame, ros::Time(0), ros::Duration(1));
00204                 _listener.lookupTransform(slamParams.base_frame, slamParams.laser_frame, ros::Time(0), tfTransform);
00205                 tf::Vector3 origin = tfTransform.getOrigin(); 
00206                 slamParams.dx_laser_robotCenter=origin[0];
00207         }
00208         catch (tf::TransformException ex) {
00209                 ROS_ERROR("[CrsmSlam] Error in tf : %s", ex.what());
00210                 slamParams.dx_laser_robotCenter=0.2;
00211         }
00212         
00213         if (n.hasParam("/crsm_slam/desired_number_of_picked_rays")) 
00214                 n.getParam("/crsm_slam/desired_number_of_picked_rays", slamParams.desired_number_of_picked_rays);
00215         else {
00216                 ROS_WARN("[CrsmSlam] : Parameter desired_number_of_picked_rays not found. Using Default");
00217                 slamParams.desired_number_of_picked_rays = 40 ;
00218         }
00219         
00220         if (n.hasParam("/crsm_slam/robot_width")) 
00221                 n.getParam("/crsm_slam/robot_width", slamParams.robot_width);
00222         else {
00223                 ROS_WARN("[CrsmSlam] : Parameter robot_width not found. Using Default");
00224                 slamParams.robot_width = 0.5 ;
00225         }
00226         
00227         if (n.hasParam("/crsm_slam/robot_length")) 
00228                 n.getParam("/crsm_slam/robot_length", slamParams.robot_length);
00229         else {
00230                 ROS_WARN("[CrsmSlam] : Parameter robot_length not found. Using Default");
00231                 slamParams.robot_length = 0.6 ;
00232         }
00233 }
00234 
00239 void CrsmSlam::findTransformation(void){
00240         
00241         
00242         bestFitness=0;
00243         bestTransformation.dx=bestTransformation.dy=bestTransformation.dth=0;
00244         
00245         int tempx,tempy;
00246         float sinth,costh,tttx,ttty;
00247         CrsmTransformation temp;
00248         CrsmHillClimbingPerson trier;
00249         unsigned int counter=0;
00250         
00251         trier.fitness=0;
00252         trier.t.dx=0;
00253         trier.t.dy=0;
00254         trier.t.dth=0;
00255         
00256         bool isDone=false;
00257         bestFitness=0;
00258 
00259         while(!isDone) {
00260                 temp.dx=robotPose.x+trier.t.dx;
00261                 temp.dy=robotPose.y+trier.t.dy;
00262                 temp.dth=robotPose.theta+trier.t.dth;   
00263                 trier.fitness=0;
00264                 float tempFitness=0;
00265                 for(set<int>::iterator j=scanSelections.begin();j != scanSelections.end();j++){
00266                         tempx=laser.scan.p[*j].x;
00267                         tempy=laser.scan.p[*j].y; 
00268                         sinth=sin(temp.dth);
00269                         costh=cos(temp.dth);
00270                         tttx=tempx*costh-tempy*sinth+temp.dx+map.info.originx;
00271                         ttty=tempx*sinth+tempy*costh+temp.dy+map.info.originy;  
00272                 
00273                         if(checkExpansion(tttx,ttty,false)) continue;
00274                                                         
00275                         if(map.p[(unsigned int)tttx][(unsigned int)ttty]==127) continue;
00276                         tempFitness+=((255-map.p[(unsigned int)tttx][(unsigned int)ttty])*10+
00277                                                 (255-map.p[(unsigned int)tttx-1][(unsigned int)ttty])+
00278                                                 (255-map.p[(unsigned int)tttx+1][(unsigned int)ttty])+
00279                                                 (255-map.p[(unsigned int)tttx][(unsigned int)ttty-1])+
00280                                                 (255-map.p[(unsigned int)tttx][(unsigned int)ttty+1]))/255.0;
00281                 }
00282                 tempFitness/=(14.0*scanSelections.size());
00283                 trier.fitness=tempFitness;
00284                 if(trier.fitness>bestFitness){
00285                         bestFitness=trier.fitness;
00286                         bestTransformation=trier.t;
00287                         trier.t.dx+=rand()%slamParams.disparity/4-slamParams.disparity/8;
00288                         trier.t.dy+=rand()%slamParams.disparity/4-slamParams.disparity/8;
00289                         trier.t.dth+=(rand()%slamParams.disparity-slamParams.disparity/2.0)/90.0;
00290                 }
00291                 else{
00292                         trier.t.dx=rand()%slamParams.disparity/2-slamParams.disparity/4;
00293                         trier.t.dy=rand()%slamParams.disparity/2-slamParams.disparity/4;
00294                         trier.t.dth=(rand()%slamParams.disparity-slamParams.disparity/2.0)/45.0;
00295                 }
00296                 if(counter>slamParams.max_hill_climbing_iterations) break;
00297                 counter++;
00298         }
00299 }
00300 
00305 void CrsmSlam::calculateCriticalRays(void){
00306         meanDensity=0;
00307         float maxDensity=0;
00308         float maxRay=0;
00309         for(unsigned int i=0;i<laser.info.laserRays-1;i++){
00310                 if(laser.scan.distance[i]>maxRay) maxRay=laser.scan.distance[i];
00311                 laser.scan.density[i]=fabs(laser.scan.distance[i]-laser.scan.distance[i+1]);
00312                 meanDensity+=laser.scan.density[i];
00313                 if(maxDensity<laser.scan.density[i] && laser.scan.density[i]<slamParams.scan_selection_meters)
00314                         maxDensity=laser.scan.density[i];
00315         }
00316         if(laser.scan.distance[laser.info.laserRays-1]>maxRay) maxRay=laser.scan.distance[laser.info.laserRays-1];
00317         meanDensity/=(laser.info.laserRays-1);
00318         static float parameter = 100;
00319         scanSelections.clear();
00320         bigChanges.clear();
00321         for(unsigned int i=0;i<laser.info.laserRays;i++){
00322                 if(laser.scan.distance[i]<(laser.info.laserMax-0.05)){
00323                         bigChanges.insert(i);
00324                         break;
00325                 }
00326         }
00327         unsigned int counterLaserRays=0;
00328         while(counterLaserRays<laser.info.laserRays){
00329                 unsigned int a=0,b=0; 
00330                 if(laser.scan.density[counterLaserRays]>slamParams.scan_selection_meters){
00331                         if(laser.scan.distance[counterLaserRays]<(laser.info.laserMax-0.05)){
00332                                 bigChanges.insert(counterLaserRays);
00333                                 a=1;
00334                         }
00335                         if(laser.scan.distance[counterLaserRays+1]<(laser.info.laserMax-0.05)){
00336                                 bigChanges.insert(counterLaserRays+1);
00337                                 b=1;
00338                         }
00339                 }
00340                 if((a+b)==0)
00341                         counterLaserRays++;
00342                 else  
00343                         counterLaserRays+=a+b;
00344         }
00345         for(unsigned int i=laser.info.laserRays-1;i>0;i--){ 
00346                 if( laser.scan.distance[i]<(laser.info.laserMax-0.05) ){
00347                         bigChanges.insert(i);
00348                         break;
00349                 }
00350         }
00351         scanSelections=bigChanges;
00352         unsigned int start,end;
00353         int count=0;
00354         for(set<int>::const_iterator j=bigChanges.begin();j != bigChanges.end();){
00355                 float sumDensity=0;
00356                 start= *j;
00357                 j++;
00358                 if(j==bigChanges.end()) break;
00359                 end= *j;
00360                 if(laser.scan.distance[start+1]>(laser.info.laserMax)-0.5)continue;
00361                 for(unsigned int i=start;i<end-1;i++)
00362                         sumDensity+=laser.scan.density[i];
00363                 float step = sumDensity /(end-start);
00364                 float localPos=0;
00365                 for(unsigned int i=start;i<end;i++){
00366                         localPos+=laser.scan.density[i];
00367                         if( localPos > (parameter*step/exp(laser.scan.distance[i]/sqrt(maxRay)*3.0)) ){ 
00368                                 scanSelections.insert(i);       
00369                                 count++;
00370                                 localPos=0;
00371                         }
00372                 }
00373         }
00374         if(count > slamParams.desired_number_of_picked_rays*1.25)
00375                 parameter += (count - slamParams.desired_number_of_picked_rays);
00376         else if(count < slamParams.desired_number_of_picked_rays*0.75)
00377                 parameter -= (slamParams.desired_number_of_picked_rays - count);
00378                 
00379         bool isFinished=false;
00380         while(!isFinished){
00381                 isFinished=true;
00382                 std::vector<int> tete;
00383                 for(std::set<int>::const_iterator it=scanSelections.begin();it!=scanSelections.end();it++)
00384                         tete.push_back(*it);
00385                 for(unsigned int i=0;i<tete.size()-1;i++)
00386                         if((tete[i+1]-tete[i])>25){
00387                                 isFinished=false;
00388                                 scanSelections.insert((tete[i+1]+tete[i])/2);
00389                         }       
00390         }
00391 }
00392 
00393 
00399 void CrsmSlam::fixNewScans(const sensor_msgs::LaserScanConstPtr& msg){
00400         if(!laser.initialized)
00401                 laser.initialize(msg);
00402         
00403         static int raysPicked=0;
00404         static float meanFitness=0;
00405         static int counter=0;
00406         float laserMean=0;
00407         for(unsigned int j=0;j<laser.info.laserRays;j++) {
00408                 laser.scan.distance[j]=msg->ranges[j];
00409                 laserMean+=laser.scan.distance[j];
00410                 
00412     if( ! (laser.scan.distance[j] >= 0.1 && laser.scan.distance[j] <= laser.info.laserMax))
00413       laser.scan.distance[j]=0;
00414                 
00415                 laser.scan.p[j].theta= msg->angle_min + ( j*msg->angle_increment );
00416                 laser.scan.p[j].x=laser.scan.distance[j]/slamParams.ocgd*cos(laser.scan.p[j].theta);
00417                 laser.scan.p[j].y=laser.scan.distance[j]/slamParams.ocgd*sin(laser.scan.p[j].theta);
00418         }
00419         
00420         calculateCriticalRays();
00421 
00422         std::vector< set<int>::iterator > toBeErased;
00423         for(set<int>::const_iterator it=scanSelections.begin();it!=scanSelections.end();it++)
00424                 if(laser.scan.distance[*it]==0 || laser.scan.distance[*it]==laser.info.laserMax)
00425                         toBeErased.push_back(it);
00426         for(unsigned int i=0;i<toBeErased.size();i++){
00427                 scanSelections.erase(toBeErased[i]);
00428         }
00429         
00430         raysPicked+=scanSelections.size();
00431 
00432         findTransformation();
00433 
00434         meanFitness+=bestFitness;
00435         
00436         robotPose.x+=bestTransformation.dx;
00437         robotPose.y+=bestTransformation.dy;
00438         robotPose.theta+=bestTransformation.dth;
00439         if(robotPose.theta>pi) robotPose.theta-=pi_double;
00440         if(robotPose.theta<-pi) robotPose.theta+=pi_double;
00441         
00442         if(counter<40){
00443                 robotPose.x=0;
00444                 robotPose.y=0;
00445                 robotPose.theta=-(laser.info.laserAngleBegin+laser.info.laserAngleEnd)/2.0;
00446         }
00447 
00448         //---robot trajectory---
00449         CrsmPose trajectoryTemp;
00450         if (    robotTrajectory.size()==0 || 
00451                         robotTrajectory[robotTrajectory.size()-1].x != (robotPose.x-cos(robotPose.theta)*slamParams.dx_laser_robotCenter/slamParams.ocgd) || 
00452                         robotTrajectory[robotTrajectory.size()-1].y != (robotPose.y-sin(robotPose.theta)*slamParams.dx_laser_robotCenter/slamParams.ocgd)){
00453                                 
00454                 trajectoryTemp.x = robotPose.x-cos(robotPose.theta)*slamParams.dx_laser_robotCenter/slamParams.ocgd;
00455                 trajectoryTemp.y = robotPose.y-sin(robotPose.theta)*slamParams.dx_laser_robotCenter/slamParams.ocgd;
00456                 trajectoryTemp.theta = robotPose.theta;
00457                 robotTrajectory.push_back( trajectoryTemp );
00458         }
00459 
00460         if(counter<10){
00461                         meanDensity=0.5;
00462         }
00463         updateMapProbabilities();
00464         counter++;
00465 }
00466 
00467 
00468 bool CrsmSlam::checkExpansion(int x,int y,bool update){
00469         bool changed=false;
00470         if(x<0){
00471                 if(update && (abs(x))>expansion.expansions[LEFT]){
00472                         expansion.expansions[LEFT]=abs(x)+50;
00473                         ROS_INFO("x %d",x);
00474                 }
00475                 changed=true;
00476         }
00477         if(x>=(int)map.info.width){
00478                 if(update && (abs(x-map.info.width))>expansion.expansions[RIGHT]){
00479                         expansion.expansions[RIGHT]=abs(x-map.info.width)+50;
00480                         ROS_INFO("x %d",x);
00481                 }
00482                 changed=true;
00483         }
00484         if(y<0){
00485                 if(update && (abs(y))>expansion.expansions[UP]){
00486                         expansion.expansions[UP]=abs(y)+50;
00487                         ROS_INFO("y %d",y);
00488                 }
00489                 changed=true;
00490         }
00491         if(y>=(int)map.info.height){
00492                 if(update && (abs(y-map.info.height))>expansion.expansions[DOWN]){
00493                         expansion.expansions[DOWN]=abs(y-map.info.height)+50;
00494                         ROS_INFO("y %d",y);
00495                 }
00496                 changed=true;
00497         }
00498         return changed;
00499 }
00500 
00501 void CrsmSlam::expandMap(void){
00502         if(     expansion.expansions[LEFT]==0 && 
00503                 expansion.expansions[RIGHT]==0 &&
00504                 expansion.expansions[UP]==0 &&
00505                 expansion.expansions[DOWN]==0 ) return;
00506 
00507         map.expandMap(expansion);
00508 }
00509 
00510 
00515 void CrsmSlam::updateMapProbabilities(void){
00516         int R=0;
00517         int dMeasure;
00518         if(meanDensity>0.2) meanDensity=0.2;
00519         if(meanDensity<0.05) meanDensity=0.05;
00520         std::set<long> prevPoints;
00521         
00522         expansion.expansions[RIGHT]=0;
00523         expansion.expansions[LEFT]=0;
00524         expansion.expansions[UP]=0;
00525         expansion.expansions[DOWN]=0;
00526         
00527         checkExpansion( robotPose.x+map.info.originx-laser.info.laserMax/slamParams.ocgd,
00528                                         robotPose.y+map.info.originy-laser.info.laserMax/slamParams.ocgd,true);
00529         checkExpansion( robotPose.x+map.info.originx+laser.info.laserMax/slamParams.ocgd,
00530                                         robotPose.y+map.info.originy+laser.info.laserMax/slamParams.ocgd,true);
00531         expandMap();
00532         
00533         //      Fix map size according to the laser scans
00534         for(unsigned int i=0;i<laser.info.laserRays;i++){
00535                         float xPoint,yPoint;
00536                         xPoint=laser.scan.distance[i]/slamParams.ocgd*cos(robotPose.theta+(float)i/(float)laser.info.laserRays*laser.info.laserAngle+laser.info.laserAngleBegin) + robotPose.x+map.info.originx;
00537                         yPoint=laser.scan.distance[i]/slamParams.ocgd*sin(robotPose.theta+(float)i/(float)laser.info.laserRays*laser.info.laserAngle+laser.info.laserAngleBegin) + robotPose.y+map.info.originy;
00538         }
00539 
00540         //      Set the map's colorization
00541         for(unsigned int measid=0;measid<laser.info.laserRays;measid+=1){
00542                 int xt,yt,xtt,ytt;
00543                 if(laser.scan.distance[measid]==0) continue;
00544                 xt=laser.scan.p[measid].x;
00545                 yt=laser.scan.p[measid].y;
00546                 
00547                 if(prevPoints.find(xt*map.info.width+yt)!=prevPoints.end()) continue;
00548                 prevPoints.insert(xt*map.info.width+yt);
00549 
00550                 dMeasure=(int)((float)laser.scan.distance[measid]/slamParams.ocgd);
00551                 while(R<laser.info.laserMax/slamParams.ocgd-3){
00552                         float xPoint,yPoint;
00553                         xPoint=R*cos(robotPose.theta+laser.angles[measid]) + robotPose.x+map.info.originx;
00554                         yPoint=R*sin(robotPose.theta+laser.angles[measid]) + robotPose.y+map.info.originy;
00555                         if(checkExpansion((int)xPoint,(int)yPoint,false)) break;
00556                         int tt=map.p[(unsigned int)xPoint][(unsigned int)yPoint];
00557                         float diff=fabs(tt-127.0)/128.0;
00558                         if(dMeasure>R || (xt==0 && yt==0))
00559                                 tt+=(1-diff)*meanDensity*slamParams.density;
00560                         if( dMeasure+1>R && dMeasure-2<R )
00561                                 tt-=(1-diff)*meanDensity*slamParams.obstacle_density*slamParams.density;
00562                         map.p[(unsigned int)xPoint][(unsigned int)yPoint]=tt;
00563                         R++;
00564                 }
00565                 R=1;
00566         }
00567 }
00568 
00573 void CrsmSlam::startLaserSubscriber(){
00574         
00575         clientLaserValues = n.subscribe( slamParams.laser_subscriber_topic.c_str() , 1 , &CrsmSlam::fixNewScans , this );
00576 }
00577 
00582 void CrsmSlam::stopLaserSubscriber(){
00583         clientLaserValues.shutdown();
00584 }
00585 
00590 void CrsmSlam::startOGMPublisher(){
00591         _occupancyGridPublisher = n.advertise<nav_msgs::OccupancyGrid>(slamParams.occupancy_grid_publish_topic.c_str(),1);
00592         _mapPublishingTimer.start();
00593 }
00594 
00599 void CrsmSlam::stopOGMPublisher(void){
00600         _mapPublishingTimer.stop();
00601         _occupancyGridPublisher.shutdown();
00602 }
00603 
00609 void CrsmSlam::publishOGM(const ros::TimerEvent& e){
00610         int width=map.info.width;
00611     int height=map.info.height;
00612     
00613     nav_msgs::OccupancyGrid grid;
00614     
00615     grid.header.stamp = ros::Time::now(); 
00616     grid.header.frame_id = slamParams.map_frame; 
00617     
00618     grid.info.resolution = slamParams.ocgd;
00619     grid.info.width = width;
00620     grid.info.height = height;
00621 
00622     grid.info.origin.position.x = -(map.info.originx*slamParams.ocgd);
00623     grid.info.origin.position.y = -(map.info.originy*slamParams.ocgd);
00624     
00625     grid.data.resize(width*height) ;
00626     for(int i=0;i<width;i++){
00627                 for(int j=0;j<height;j++){
00628                         grid.data[j*width+i]= 100.0-(int) (map.p[i][j]*100.0/255.0);
00629                 }
00630         }
00631     _occupancyGridPublisher.publish(grid);
00632 }
00633 
00640 char CrsmSlam::getMapProbability(int x,int y){ 
00641         return map.p[x][y]; 
00642 }
00643 
00648 CrsmMapInfo CrsmSlam::getMapInfo(void){
00649         return map.info;
00650 }
00651 
00656 CrsmPose CrsmSlam::getRobotPose(void){
00657         return robotPose;
00658 }
00659 
00664 CrsmLaserInfo CrsmSlam::getLaserInfo(void){
00665         return laser.info;
00666 }
00667 
00672 std::vector<CrsmPose> CrsmSlam::getTrajectory(void){
00673         return robotTrajectory;
00674 }
00675 
00681 void CrsmSlam::publishRobotPoseTf(const ros::TimerEvent& e){
00682         tf::Vector3 translation( (robotPose.x-cos(robotPose.theta)*(slamParams.dx_laser_robotCenter/slamParams.ocgd))* slamParams.ocgd, (robotPose.y-sin(robotPose.theta)*(slamParams.dx_laser_robotCenter/slamParams.ocgd))* slamParams.ocgd, 0);
00683         tf::Quaternion rotation;
00684         rotation.setRPY(0,0,robotPose.theta);
00685 
00686         tf::Transform transform(rotation,translation);
00687         _slamFrameBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00688                                 slamParams.map_frame, slamParams.base_footprint_frame));                                
00689 }
00690 
00695 void CrsmSlam::startTrajectoryPublisher(){
00696         _pathPublisher = n.advertise<nav_msgs::Path>(slamParams.robot_trajectory_publish_topic.c_str(),1);
00697         _pathPublishingTimer.start();   
00698 }
00699         
00704 void CrsmSlam::stopTrajectoryPublisher(void){
00705         _pathPublishingTimer.stop();    
00706         _pathPublisher.shutdown();
00707 }
00708 
00714 void CrsmSlam::publishTrajectory(const ros::TimerEvent& e){
00715         nav_msgs::Path pathForViz;
00716         geometry_msgs::PoseStamped pathPoint;
00717         
00718         for (int i=0; i<robotTrajectory.size();i++){
00719                 pathPoint.pose.position.x = robotTrajectory[i].x*slamParams.ocgd;
00720                 pathPoint.pose.position.y = robotTrajectory[i].y*slamParams.ocgd;
00721                 pathForViz.poses.push_back(pathPoint);
00722         }
00723         
00724         pathForViz.header.stamp = ros::Time::now();
00725         pathForViz.header.frame_id = slamParams.trajectory_publisher_frame_id;
00726                 
00727         _pathPublisher.publish(pathForViz);
00728 }
00729 
00730 //---------------------- Setters for slamParameters ----------------------------//
00736 void CrsmSlam::setDisparity(int disparity){
00737         slamParams.disparity=disparity;
00738 }
00739 
00745 void CrsmSlam::setInitialMapSize(int size){
00746         slamParams.map_size=size;
00747 }
00748 
00754 void CrsmSlam::setOcgd(double ocgd){
00755         slamParams.ocgd=ocgd;
00756 }
00757 
00763 void CrsmSlam::setDensity(double density){
00764         slamParams.density=density;
00765 }
00766 
00772 void CrsmSlam::setObstacleDensity(double ob_density){
00773         slamParams.obstacle_density=ob_density;
00774 }
00775 
00781 void CrsmSlam::setScanSelectionMeters(double scan_selection_meters){
00782         slamParams.scan_selection_meters=scan_selection_meters;
00783 }
00784 
00790 void CrsmSlam::setMaxHillClimbingIterations(int iterations){
00791         slamParams.max_hill_climbing_iterations=iterations;
00792 }
00793 
00799 void CrsmSlam::setDxLaserRobotCenter(double dx){
00800         slamParams.dx_laser_robotCenter=dx;
00801 }
00802 
00808 void CrsmSlam::setOccupancyGridMapFreq(double freq){
00809         slamParams.occupancy_grid_map_freq=freq;
00810 }
00811 
00817 void CrsmSlam::setRobotPoseTfFreq(double freq){
00818         slamParams.robot_pose_tf_freq=freq;
00819 }
00820 
00826 void CrsmSlam::setTrajectoryFreq(double freq){
00827         slamParams.trajectory_freq=freq;
00828 }
00829 
00835 void CrsmSlam::setDesiredNumberOfPickedRays(int rays){
00836         slamParams.desired_number_of_picked_rays=rays;
00837 }
00838 
00844 void CrsmSlam::setRobotWidth(double width){
00845         slamParams.robot_width=width;
00846 }
00847 
00853 void CrsmSlam::setRobotLength(double length){
00854         slamParams.robot_length=length;
00855 }
00856 
00862 void CrsmSlam::setOccupancyGridPublishTopic(std::string topic){
00863         slamParams.occupancy_grid_publish_topic=topic;
00864 }
00865 
00871 void CrsmSlam::setRobotTrajectoryPublishTopic(std::string topic){
00872         slamParams.robot_trajectory_publish_topic=topic;
00873 }
00874 
00880 void CrsmSlam::setTrajectoryPublisherFrameId(std::string frame_id){
00881         slamParams.trajectory_publisher_frame_id=frame_id;
00882 }
00883 
00889 void CrsmSlam::setLaserSubscriberTopic(std::string topic){
00890         slamParams.laser_subscriber_topic=topic;
00891 }
00892 
00898 void CrsmSlam::setWorldFrame(std::string frame){
00899         slamParams.world_frame=frame;
00900 }
00901 
00907 void CrsmSlam::setBaseFootprintFrame(std::string frame){
00908         slamParams.base_footprint_frame=frame;
00909 }
00910 
00916 void CrsmSlam::setBaseFrame(std::string frame){
00917         slamParams.base_frame=frame;
00918 }
00919 
00925 void CrsmSlam::setMapFrame(std::string frame){
00926         slamParams.map_frame=frame;
00927 }
00928 
00934 void CrsmSlam::setLaserFrame(std::string frame){
00935         slamParams.laser_frame=frame;
00936 }
00937 
00938 //------------------- Getters for slamParameters ----------------------//
00939 
00944 int CrsmSlam::getDisparity(void){
00945         return slamParams.disparity;
00946 }
00947 
00952 int CrsmSlam::getInitialMapSize(void){
00953         return slamParams.map_size;
00954 }
00955 
00960 double CrsmSlam::getOcgd(void){
00961         return slamParams.ocgd;
00962 }
00963 
00968 double CrsmSlam::getDensity(void){
00969         return slamParams.density;
00970 }
00971 
00976 double CrsmSlam::getObstacleDensity(void){
00977         return slamParams.obstacle_density;
00978 }
00979 
00984 double CrsmSlam::getScanSelectionMeters(void){
00985         return slamParams.scan_selection_meters;
00986 }
00987 
00992 int CrsmSlam::getMaxHillClimbingIterations(void){
00993         return slamParams.max_hill_climbing_iterations;
00994 }
00995 
01000 double CrsmSlam::getDxLaserRobotCenter(void){
01001         return slamParams.dx_laser_robotCenter;
01002 }
01003 
01008 double CrsmSlam::getOccupancyGridMapFreq(void){
01009         return slamParams.occupancy_grid_map_freq;
01010 }
01011 
01016 double CrsmSlam::getRobotPoseTfFreq(void){
01017         return slamParams.robot_pose_tf_freq;
01018 }
01019 
01024 double CrsmSlam::getTrajectoryFreq(void){
01025         return slamParams.trajectory_freq;
01026 }
01027 
01032 int CrsmSlam::getDesiredNumberOfPickedRays(void){
01033         return slamParams.desired_number_of_picked_rays;
01034 }
01035 
01040 double CrsmSlam::getRobotWidth(void){
01041         return slamParams.robot_width;
01042 }
01043 
01048 double CrsmSlam::getRobotLength(void){
01049         return slamParams.robot_length;
01050 }
01051 
01056 std::string CrsmSlam::getOccupancyGridPublishTopic(void){
01057         return slamParams.occupancy_grid_publish_topic;
01058 }
01059 
01064 std::string CrsmSlam::getRobotTrajectoryPublishTopic(void){
01065         return slamParams.robot_trajectory_publish_topic;
01066 }
01067 
01072 std::string CrsmSlam::getTrajectoryPublisherFrameId(void){
01073         return slamParams.trajectory_publisher_frame_id;
01074 }
01075 
01080 std::string CrsmSlam::getLaserSubscriberTopic(void){
01081         return slamParams.laser_subscriber_topic;
01082 }
01083 
01088 std::string CrsmSlam::getWorldFrame(void){
01089         return slamParams.world_frame;
01090 }
01091 
01096 std::string CrsmSlam::getBaseFootprintFrame(void){
01097         return slamParams.base_footprint_frame;
01098 }
01099 
01104 std::string CrsmSlam::getBaseFrame(void){
01105         return slamParams.base_frame;
01106 }
01107 
01112 std::string CrsmSlam::getMapFrame(void){
01113         return slamParams.map_frame;
01114 }
01115 
01120 std::string CrsmSlam::getLaserFrame(void){
01121         return slamParams.laser_frame;
01122 }
01123 
01124 }