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