00001
00020 #define USE_VISUALIZATION_DEBUG ///< Enable / Disable visualization
00021
00022 #include <mrpt/utils/CTicTac.h>
00023
00024 #ifdef USE_VISUALIZATION_DEBUG
00025 #include <mrpt/gui.h>
00026 #include <mrpt/base.h>
00027 #include <mrpt/opengl.h>
00028 #include <GL/gl.h>
00029 #include "CMyEllipsoid.h"
00030 #endif
00031
00032 #include <ros/ros.h>
00033 #include <rosbag/bag.h>
00034 #include <rosbag/view.h>
00035 #include <tf/transform_listener.h>
00036 #include <boost/foreach.hpp>
00037 #include <sensor_msgs/LaserScan.h>
00038 #include <message_filters/subscriber.h>
00039 #include <message_filters/sync_policies/approximate_time.h>
00040 #include <nav_msgs/Odometry.h>
00041 #include <nav_msgs/OccupancyGrid.h>
00042 #include <ndt_map.h>
00043 #include <tf/transform_broadcaster.h>
00044 #include "tfMessageReader.h"
00045 #include "ndt_mcl.hpp"
00046
00047 #ifdef USE_VISUALIZATION_DEBUG
00048 #include "mcl_visualization.hpp"
00049 #endif
00050
00054 Eigen::Affine3d getAsAffine(float x, float y, float yaw ){
00055 Eigen::Matrix3d m;
00056 m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
00057 * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00058 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
00059 Eigen::Translation3d v(x,y,0);
00060 Eigen::Affine3d T = v*m;
00061
00062 return T;
00063 }
00064
00074 NDTMCL<pcl::PointXYZ> *ndtmcl;
00075
00077 float offx = 0;
00078 float offy = 0;
00079 float offa = 0;
00080
00081 static bool has_sensor_offset_set = false;
00082 static bool isFirstLoad=true;
00083 Eigen::Affine3d Told,Todo;
00084 FILE *flog = fopen("ndtmcl_result.txt","wt");
00085
00090 mrpt::utils::CTicTac TT;
00091
00092 std::string tf_odo_topic = "odom_base_link";
00093 std::string tf_state_topic = "base_link";
00094 std::string tf_laser_link = "base_laser_link";
00095
00099 void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
00100 {
00101 static int counter = 0;
00102 counter++;
00103
00104 static tf::TransformListener tf_listener;
00105
00106 double looptime = TT.Tac();
00107 TT.Tic();
00108 fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
00109
00110 if(has_sensor_offset_set == false) return;
00111
00112 double gx,gy,gyaw,x,y,yaw;
00113
00115 gyaw = tf::getYaw(state_pose.getRotation());
00116 gx = state_pose.getOrigin().x();
00117 gy = state_pose.getOrigin().y();
00118
00120 yaw = tf::getYaw(odo_pose.getRotation());
00121 x = odo_pose.getOrigin().x();
00122 y = odo_pose.getOrigin().y();
00123
00124 mrpt::utils::CTicTac tictac;
00125 tictac.Tic();
00126
00127 int N =(scan.angle_max - scan.angle_min)/scan.angle_increment;
00128
00132
00133 Eigen::Affine3d T = getAsAffine(x,y,yaw);
00134 Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
00135
00139 if(isFirstLoad){
00140 fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
00142 ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
00143 Told = T;
00144 Todo = Tgt;
00145 }
00146
00148 Eigen::Affine3d Tmotion = Told.inverse() * T;
00149 Todo = Todo*Tmotion;
00150 Told = T;
00151
00153 float dy =offy;
00154 float dx = offx;
00155 float alpha = atan2(dy,dx);
00156 float L = sqrt(dx*dx+dy*dy);
00157
00159 float lpx = L * cos(alpha);
00160 float lpy = L * sin(alpha);
00161 float lpa = offa;
00162
00163
00164
00166 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00167 for (int j=0;j<N;j++){
00168 double r = scan.ranges[j];
00169 if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
00170 double a = scan.angle_min + j*scan.angle_increment;
00171 pcl::PointXYZ pt;
00172 pt.x = r*cos(a+lpa)+lpx;
00173 pt.y = r*sin(a+lpa)+lpy;
00174 pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
00175 cloud->push_back(pt);
00176 }
00177 }
00181 ndtmcl->updateAndPredict(Tmotion, *cloud);
00182
00183 Eigen::Vector3d dm = ndtmcl->getMean();
00184 Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances();
00185
00186 double Time = tictac.Tac();
00187 fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",Time*1000,dm[0],dm[1],dm[2]);
00188 isFirstLoad = false;
00190
00191
00192 #ifdef USE_VISUALIZATION_DEBUG
00193
00194 Eigen::Vector3d origin(dm[0] + L * cos(dm[2]+alpha),dm[1] + L * sin(dm[2]+alpha),0.1);
00195
00196 Eigen::Affine3d ppos = getAsAffine(dm[0],dm[1],dm[2]);
00197
00198
00199 lslgeneric::transformPointCloudInPlace(ppos, *cloud);
00200 mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
00201 win3D.setCameraPointingToPoint(gx,gy,1);
00202 if(counter%2000==0) gl_points->clear();
00203 scene->clear();
00204 scene->insert(plane);
00205
00206 addMap2Scene(ndtmcl->map, origin, scene);
00207 addPoseCovariance(dm[0],dm[1],cov,scene);
00208 addScanToScene(scene, origin, cloud);
00209 addParticlesToWorld(ndtmcl->pf,Tgt.translation(),dm, Todo.translation());
00210 scene->insert(gl_points);
00211 scene->insert(gl_particles);
00212 win3D.unlockAccess3DScene();
00213 win3D.repaint();
00214
00215 if (win3D.keyHit())
00216 {
00217 mrpt::gui::mrptKeyModifier kmods;
00218 int key = win3D.getPushedKey(&kmods);
00219 }
00220 #endif
00221
00222
00223
00224 }
00225
00226 int main(int argc, char **argv){
00227 ros::init(argc, argv, "sauna_mcl");
00228 double resolution=0.2;
00229 #ifdef USE_VISUALIZATION_DEBUG
00230 initializeScene();
00231 #endif
00232 ros::NodeHandle n;
00233 ros::NodeHandle nh;
00234 ros::NodeHandle paramHandle ("~");
00235 TT.Tic();
00241 bool loadMap = false;
00242 std::string mapName("basement.ndmap");
00243
00244 bool makeMapVeryStatic = false;
00245
00246 bool saveMap = true;
00247 std::string output_map_name = std::string("ndt_mapper_output.ndmap");
00248
00254
00255 std::string input_laser_topic;
00256 paramHandle.param<std::string>("input_laser_topic", input_laser_topic, std::string("/base_scan"));
00257
00258 paramHandle.param<std::string>("tf_base_link", tf_state_topic, std::string("/state_base_link"));
00259 paramHandle.param<std::string>("tf_laser_link", tf_laser_link, std::string("/hokuyo1_link"));
00260
00261 bool use_sensor_pose;
00262 paramHandle.param<bool>("use_sensor_pose", use_sensor_pose, false);
00263 double sensor_pose_x, sensor_pose_y, sensor_pose_th;
00264 paramHandle.param<double>("sensor_pose_x", sensor_pose_x, 0.);
00265 paramHandle.param<double>("sensor_pose_y", sensor_pose_y, 0.);
00266 paramHandle.param<double>("sensor_pose_th", sensor_pose_th, 0.);
00267
00268 paramHandle.param<bool>("load_map_from_file", loadMap, false);
00269 paramHandle.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
00270
00271 paramHandle.param<bool>("save_output_map", saveMap, true);
00272 paramHandle.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
00273
00274 paramHandle.param<double>("map_resolution", resolution , 0.2);
00275 bool forceSIR=false;
00276 paramHandle.param<bool>("forceSIR", forceSIR, false);
00277
00278 std::string bagfilename="bagfile_unset.bag";
00279 paramHandle.param<std::string>("bagfile_name", bagfilename, std::string("bagfile_unset.bag"));
00280
00286 fprintf(stderr,"USING RESOLUTION %lf\n",resolution);
00287 lslgeneric::NDTMap<pcl::PointXYZ> ndmap(new lslgeneric::LazyGrid<pcl::PointXYZ>(resolution));
00288
00289 ndmap.setMapSize(80.0, 80.0, 1.0);
00290
00291 if(loadMap){
00292 fprintf(stderr,"Loading Map from '%s'\n",mapName.c_str());
00293 ndmap.loadFromJFF(mapName.c_str());
00294 }
00295
00296 ndtmcl = new NDTMCL<pcl::PointXYZ>(resolution,ndmap,-0.5);
00297 if(forceSIR) ndtmcl->forceSIR=true;
00298
00299 fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
00300
00306
00307 tfMessageReader<sensor_msgs::LaserScan> reader(bagfilename,
00308 input_laser_topic,
00309 std::string("/world"),
00310 tf_odo_topic);
00311
00313 offa = sensor_pose_th;
00314 offx = sensor_pose_x;
00315 offy = sensor_pose_y;
00316 has_sensor_offset_set = true;
00317
00318 fprintf(stderr,"Sensor Pose = (%lf %lf %lf)\n",offx, offy, offa);
00319
00321 while(!reader.bagEnd()){
00322 sensor_msgs::LaserScan s;
00323 tf::Transform odo_pose;
00324 bool hasOdo = false;
00325 bool hasState = false;
00326
00327 if(reader.getNextMessage(s, odo_pose)){
00328 hasOdo = true;
00329 }
00330
00331 tf::Transform state_pose;
00332
00333 if(reader.getTf(tf_state_topic, s.header.stamp, state_pose)){
00334 hasState = true;
00335 }
00336
00338 if(hasState && hasOdo){
00339 callback(s,odo_pose,state_pose);
00340 }
00341 }
00342
00343 fprintf(stderr,"-- THE END --\n");
00344
00345 return 0;
00346 }