00001
00020 #define USE_VISUALIZATION_DEBUG ///< Enable / Disable visualization
00021
00022 #ifdef USE_VISUALIZATION_DEBUG
00023 #include <ndt_visualisation/ndt_viz.h>
00024 NDTViz ndt_viz;
00025 #endif
00026
00027 #include <ros/ros.h>
00028 #include <rosbag/bag.h>
00029 #include <rosbag/view.h>
00030 #include <tf/transform_listener.h>
00031 #include <boost/foreach.hpp>
00032 #include <sensor_msgs/LaserScan.h>
00033 #include <message_filters/subscriber.h>
00034 #include <message_filters/sync_policies/approximate_time.h>
00035 #include <nav_msgs/Odometry.h>
00036 #include <nav_msgs/OccupancyGrid.h>
00037 #include <tf/transform_broadcaster.h>
00038
00039 #include <ndt_map/ndt_map.h>
00040 #include "ndt_mcl/tfMessageReader.h"
00041 #include "ndt_mcl/ndt_mcl.h"
00042
00043
00047 Eigen::Affine3d getAsAffine(float x, float y, float yaw ){
00048 Eigen::Matrix3d m;
00049 m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
00050 * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
00051 * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
00052 Eigen::Translation3d v(x,y,0);
00053 Eigen::Affine3d T = v*m;
00054
00055 return T;
00056 }
00057 double getDoubleTime()
00058 {
00059 struct timeval time;
00060 gettimeofday(&time,NULL);
00061 return time.tv_sec + time.tv_usec * 1e-6;
00062 }
00063
00073 NDTMCL *ndtmcl;
00074
00076 float offx = 0;
00077 float offy = 0;
00078 float offa = 0;
00079
00080 static bool has_sensor_offset_set = false;
00081 static bool isFirstLoad=true;
00082 Eigen::Affine3d Told,Todo;
00083 FILE *flog = fopen("ndtmcl_result.txt","wt");
00084
00089 double time_end;
00090 std::string tf_odo_topic = "odom_base_link";
00091 std::string tf_state_topic = "base_link";
00092 std::string tf_laser_link = "base_laser_link";
00093
00097 void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
00098 {
00099 static int counter = 0;
00100
00101 static tf::TransformListener tf_listener;
00102
00103 double time_now = getDoubleTime();
00104 double looptime = time_end - time_now;
00105 fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
00106
00107 if(has_sensor_offset_set == false) return;
00108
00109 double gx,gy,gyaw,x,y,yaw;
00110
00112 gyaw = tf::getYaw(state_pose.getRotation());
00113 gx = state_pose.getOrigin().x();
00114 gy = state_pose.getOrigin().y();
00115
00117 yaw = tf::getYaw(odo_pose.getRotation());
00118 x = odo_pose.getOrigin().x();
00119 y = odo_pose.getOrigin().y();
00120
00121 int N =(scan.angle_max - scan.angle_min)/scan.angle_increment;
00122
00126
00127 Eigen::Affine3d T = getAsAffine(x,y,yaw);
00128 Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
00129
00133 if(isFirstLoad){
00134 fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
00136 ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
00137 Told = T;
00138 Todo = Tgt;
00139 }
00140
00142 Eigen::Affine3d Tmotion = Told.inverse() * T;
00143 Todo = Todo*Tmotion;
00144 Told = T;
00145
00147 float dy =offy;
00148 float dx = offx;
00149 float alpha = atan2(dy,dx);
00150 float L = sqrt(dx*dx+dy*dy);
00151
00153 float lpx = L * cos(alpha);
00154 float lpy = L * sin(alpha);
00155 float lpa = offa;
00156
00157
00158
00160 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00161 for (int j=0;j<N;j++){
00162 double r = scan.ranges[j];
00163 if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
00164 double a = scan.angle_min + j*scan.angle_increment;
00165 pcl::PointXYZ pt;
00166 pt.x = r*cos(a+lpa)+lpx;
00167 pt.y = r*sin(a+lpa)+lpy;
00168 pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
00169 cloud->push_back(pt);
00170 }
00171 }
00175 ndtmcl->updateAndPredict(Tmotion, *cloud);
00176
00177 Eigen::Vector3d dm = ndtmcl->getMean();
00178 Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances();
00179
00180 time_end = getDoubleTime();
00181 fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",time_end-time_now,dm[0],dm[1],dm[2]);
00182 isFirstLoad = false;
00184
00185
00186 #ifdef USE_VISUALIZATION_DEBUG
00187 if(counter%500==0){
00188 ndt_viz.clear();
00189 ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);
00190 ndt_viz.displayParticles();
00191 ndt_viz.displayTrajectory();
00192 }
00193 ndt_viz.clearParticles();
00194 for(int i=0;i<ndtmcl->pf.NumOfParticles;i++){
00195 ndt_viz.addParticle(ndtmcl->pf.Particles[i].x, ndtmcl->pf.Particles[i].y, 0.5, 1.0, 1.0, 1.0);
00196 }
00197
00198 ndt_viz.addTrajectoryPoint(dm[0],dm[1],0.5,1.0,0,0);
00199 ndt_viz.addTrajectoryPoint(Tgt.translation()(0), Tgt.translation()(1),0.5,1.0,1.0,1.0);
00200 ndt_viz.addTrajectoryPoint(Todo.translation()(0), Todo.translation()(1),0.5,0.0,1.0,0.0);
00201
00202 ndt_viz.win3D->setCameraPointingToPoint(dm[0],dm[1],3.0);
00203 ndt_viz.repaint();
00204
00205 #endif
00206
00207 counter++;
00208 }
00209
00210 int main(int argc, char **argv){
00211 ros::init(argc, argv, "sauna_mcl");
00212 double resolution=0.2;
00213 ros::NodeHandle n;
00214 ros::NodeHandle nh;
00215 ros::NodeHandle paramHandle ("~");
00216 time_end = getDoubleTime();
00222 bool loadMap = false;
00223 std::string mapName("basement.ndmap");
00224
00225 bool makeMapVeryStatic = false;
00226
00227 bool saveMap = true;
00228 std::string output_map_name = std::string("ndt_mapper_output.ndmap");
00229
00235
00236 std::string input_laser_topic;
00237 paramHandle.param<std::string>("input_laser_topic", input_laser_topic, std::string("/base_scan"));
00238
00239 paramHandle.param<std::string>("tf_base_link", tf_state_topic, std::string("/state_base_link"));
00240 paramHandle.param<std::string>("tf_laser_link", tf_laser_link, std::string("/hokuyo1_link"));
00241
00242 bool use_sensor_pose;
00243 paramHandle.param<bool>("use_sensor_pose", use_sensor_pose, false);
00244 double sensor_pose_x, sensor_pose_y, sensor_pose_th;
00245 paramHandle.param<double>("sensor_pose_x", sensor_pose_x, 0.);
00246 paramHandle.param<double>("sensor_pose_y", sensor_pose_y, 0.);
00247 paramHandle.param<double>("sensor_pose_th", sensor_pose_th, 0.);
00248
00249 paramHandle.param<bool>("load_map_from_file", loadMap, false);
00250 paramHandle.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
00251
00252 paramHandle.param<bool>("save_output_map", saveMap, true);
00253 paramHandle.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
00254
00255 paramHandle.param<double>("map_resolution", resolution , 0.2);
00256 bool forceSIR=false;
00257 paramHandle.param<bool>("forceSIR", forceSIR, false);
00258
00259 std::string bagfilename="bagfile_unset.bag";
00260 paramHandle.param<std::string>("bagfile_name", bagfilename, std::string("bagfile_unset.bag"));
00261
00267 fprintf(stderr,"USING RESOLUTION %lf\n",resolution);
00268 lslgeneric::NDTMap ndmap(new lslgeneric::LazyGrid(resolution));
00269
00270 ndmap.setMapSize(80.0, 80.0, 1.0);
00271
00272 if(loadMap){
00273 fprintf(stderr,"Loading Map from '%s'\n",mapName.c_str());
00274 ndmap.loadFromJFF(mapName.c_str());
00275 }
00276
00277 ndtmcl = new NDTMCL(resolution,ndmap,-0.5);
00278 if(forceSIR) ndtmcl->forceSIR=true;
00279
00280 fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
00281
00287
00288 tfMessageReader<sensor_msgs::LaserScan> reader(bagfilename,
00289 input_laser_topic,
00290 std::string("/world"),
00291 tf_odo_topic);
00292
00294 offa = sensor_pose_th;
00295 offx = sensor_pose_x;
00296 offy = sensor_pose_y;
00297 has_sensor_offset_set = true;
00298
00299 fprintf(stderr,"Sensor Pose = (%lf %lf %lf)\n",offx, offy, offa);
00300
00301 #ifdef USE_VISUALIZATION_DEBUG
00302 ndt_viz.win3D->start_main_loop_own_thread();
00303 #endif
00304
00305 while(!reader.bagEnd() && ros::ok()){
00306 sensor_msgs::LaserScan s;
00307 tf::Transform odo_pose;
00308 bool hasOdo = false;
00309 bool hasState = false;
00310
00311 if(reader.getNextMessage(s, odo_pose)){
00312 hasOdo = true;
00313 }
00314
00315 tf::Transform state_pose;
00316
00317 if(reader.getTf(tf_state_topic, s.header.stamp, state_pose)){
00318 hasState = true;
00319 }
00320
00322 if(hasState && hasOdo){
00323 callback(s,odo_pose,state_pose);
00324 }
00325 #ifdef USE_VISUALIZATION_DEBUG
00326
00327 #endif
00328 }
00329
00330 fprintf(stderr,"-- THE END --\n");
00331
00332 return 0;
00333 }