/**
* This is where the localization is done... not really callback in the offline version
*/
void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
{
static int counter = 0;
counter++;
static tf::TransformListener tf_listener;
double looptime = TT.Tac();
TT.Tic();
fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
if(has_sensor_offset_set == false) return;
double gx,gy,gyaw,x,y,yaw;
///Get the ground truth
gyaw = tf::getYaw(state_pose.getRotation());
gx = state_pose.getOrigin().x();
gy = state_pose.getOrigin().y();
///Get the odometry
yaw = tf::getYaw(odo_pose.getRotation());
x = odo_pose.getOrigin().x();
y = odo_pose.getOrigin().y();
mrpt::utils::CTicTac tictac;
tictac.Tic();
int N =(scan.angle_max - scan.angle_min)/scan.angle_increment; ///< number of scan lines
/////
/// Pose conversions
////
Eigen::Affine3d T = getAsAffine(x,y,yaw);
Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
/**
* We are now using the information from the ground truth to initialize the filter
*/
if(isFirstLoad){
fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
///Initialize the filter with 1m^2 variance in position and 20deg in heading
ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
Told = T;
Todo = Tgt;
}
///Compute the differential motion between two time steps
Eigen::Affine3d Tmotion = Told.inverse() * T;
Todo = Todo*Tmotion;
Told = T;
///Get the laser pose with respect to the base
float dy =offy;
float dx = offx;
float alpha = atan2(dy,dx);
float L = sqrt(dx*dx+dy*dy);
///Laser pose in base frame
float lpx = L * cos(alpha);
float lpy = L * sin(alpha);
float lpa = offa;
///Laser scan to PointCloud transformed with respect to the base
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (int j=0;j<N;j++){
double r = scan.ranges[j];
if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
double a = scan.angle_min + j*scan.angle_increment;
pcl::PointXYZ pt;
pt.x = r*cos(a+lpa)+lpx;
pt.y = r*sin(a+lpa)+lpy;
pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
cloud->push_back(pt);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Now we have the differential motion and pointcloud -- Lets do MCL
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ndtmcl->updateAndPredict(Tmotion, *cloud); ///< does the prediction, update and resampling steps (see ndt_mcl.hpp)
Eigen::Vector3d dm = ndtmcl->getMean(); ///<Maximum aposteriori estimate of the pose
Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///distribution variances
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double Time = tictac.Tac();
fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",Time*1000,dm[0],dm[1],dm[2]);
isFirstLoad = false;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If visualization desired
#ifdef USE_VISUALIZATION_DEBUG
///The sensor pose in the global frame for visualization
Eigen::Vector3d origin(dm[0] + L * cos(dm[2]+alpha),dm[1] + L * sin(dm[2]+alpha),0.1);
Eigen::Affine3d ppos = getAsAffine(dm[0],dm[1],dm[2]);
//lslgeneric::transformPointCloudInPlace(Tgt, *cloud);
lslgeneric::transformPointCloudInPlace(ppos, *cloud);
mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();
win3D.setCameraPointingToPoint(gx,gy,1);
if(counter%2000==0) gl_points->clear();
scene->clear();
scene->insert(plane);
addMap2Scene(ndtmcl->map, origin, scene);
addPoseCovariance(dm[0],dm[1],cov,scene);
addScanToScene(scene, origin, cloud);
addParticlesToWorld(ndtmcl->pf,Tgt.translation(),dm, Todo.translation());
scene->insert(gl_points);
scene->insert(gl_particles);
win3D.unlockAccess3DScene();
win3D.repaint();
if (win3D.keyHit())
{
mrpt::gui::mrptKeyModifier kmods;
int key = win3D.getPushedKey(&kmods);
}
#endif
}
/** This virtual function will be called upon receive of any event after starting listening at any CObservable object. */
virtual void OnEvent(const mrptEvent &e)
{
if (e.isOfType<mrpt::gui::mrptEventWindowChar>())
{
const mrpt::gui::mrptEventWindowChar * ev = e.getAs<mrpt::gui::mrptEventWindowChar>();
bool rebuild_3d_obj = false;
switch (ev->char_code)
{
case 'h':
case 'H':
if (!showing_help) {
tim_show_start.Tic();
showing_help = true;
}
else {
tim_show_end.Tic();
showing_help = false;
hiding_help = true;
}
break;
case 'q':
case 'Q':
request_to_quit=true;
break;
case 'i':
case 'I':
params["show_ID_labels"] = params["show_ID_labels"]!=0 ? 0:1;
rebuild_3d_obj = true;
break;
case 'e':
case 'E':
params["show_edges"] = params["show_edges"]!=0 ? 0:1;
rebuild_3d_obj = true;
break;
case 'c':
case 'C':
params["show_node_corners"] = params["show_node_corners"]!=0 ? 0:1;
rebuild_3d_obj = true;
break;
case 's':
case 'S':
{
static int cnt = 0;
const std::string sFil = mrpt::format("dump_graph_%05i.3Dscene",++cnt);
std::cout << "Dumping scene to file: " << sFil << std::endl;
mrpt::opengl::COpenGLScene scene;
scene.insert(m_new_3dobj);
mrpt::utils::CFileGZOutputStream f(sFil);
f << scene;
}
break;
case 'v':
case 'V':
params["show_edge_rel_poses"] = params["show_edge_rel_poses"]!=0 ? 0:1;
rebuild_3d_obj = true;
break;
case '+':
params["nodes_point_size"]+=1.0;
rebuild_3d_obj = true;
break;
case '-':
params["nodes_point_size"]=std::max(0.0, params["nodes_point_size"] - 1.0 );
rebuild_3d_obj = true;
break;
case 'o':
case 'O':
params["nodes_corner_scale"]*=1.2;
rebuild_3d_obj = true;
break;
case 'p':
case 'P':
params["nodes_corner_scale"]*=1.0/1.2;
rebuild_3d_obj = true;
break;
case 'k':
case 'K':
params["nodes_edges_corner_scale"]*=1.2;
rebuild_3d_obj = true;
break;
case 'l':
case 'L':
params["nodes_edges_corner_scale"]*=1.0/1.2;
rebuild_3d_obj = true;
break;
};
if (rebuild_3d_obj)
{
m_new_3dobj = mrpt::opengl::graph_tools::graph_visualize(m_graph,params);
request_to_refresh_3D_view = true;
}
}
else
if (e.isOfType<mrptEventGLPostRender>())
{
//const mrptEventGLPostRender* ev = e.getAs<mrptEventGLPostRender>();
// Show small message in the corner:
mrpt::opengl::gl_utils::renderMessageBox(
0.8f, 0.94f, // x,y (in screen "ratios")
0.19f, 0.05f, // width, height (in screen "ratios")
"Press 'h' for help",
0.015f // text size
);
// Also showing help?
if (showing_help || hiding_help)
{
static const double TRANSP_ANIMATION_TIME_SEC = 0.5;
const double show_tim = tim_show_start.Tac();
const double hide_tim = tim_show_end.Tac();
const double tranparency = hiding_help ?
1.0-std::min(1.0,hide_tim/TRANSP_ANIMATION_TIME_SEC)
:
std::min(1.0,show_tim/TRANSP_ANIMATION_TIME_SEC);
mrpt::opengl::gl_utils::renderMessageBox(
0.05f, 0.05f, // x,y (in screen "ratios")
0.50f, 0.50f, // width, height (in screen "ratios")
MSG_HELP_WINDOW,
0.02f, // text size
mrpt::utils::TColor(190,190,190, 200*tranparency), // background
mrpt::utils::TColor(0,0,0, 200*tranparency), // border
mrpt::utils::TColor(200,0,0, 150*tranparency), // text
5.0f, // border width
"serif", // text font
mrpt::opengl::NICE // text style
);
if (hide_tim>TRANSP_ANIMATION_TIME_SEC && hiding_help)
hiding_help = false;
}
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "sauna_mcl");
double resolution=0.2;
#ifdef USE_VISUALIZATION_DEBUG
initializeScene();
#endif
ros::NodeHandle n;
ros::NodeHandle nh;
ros::NodeHandle paramHandle ("~");
TT.Tic();
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
/// Parameters for the mapper
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
bool loadMap = false; ///< flag to indicate that we want to load a map
std::string mapName("basement.ndmap"); ///<name and the path to the map
bool makeMapVeryStatic = false; ///< indicates if we should make the map over confident (stationary that is)
bool saveMap = true; ///< indicates if we want to save the map in a regular intervals
std::string output_map_name = std::string("ndt_mapper_output.ndmap");
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
/// Set the values from a config file
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
std::string input_laser_topic;
paramHandle.param<std::string>("input_laser_topic", input_laser_topic, std::string("/base_scan"));
paramHandle.param<std::string>("tf_base_link", tf_state_topic, std::string("/state_base_link"));
paramHandle.param<std::string>("tf_laser_link", tf_laser_link, std::string("/hokuyo1_link"));
bool use_sensor_pose;
paramHandle.param<bool>("use_sensor_pose", use_sensor_pose, false);
double sensor_pose_x, sensor_pose_y, sensor_pose_th;
paramHandle.param<double>("sensor_pose_x", sensor_pose_x, 0.);
paramHandle.param<double>("sensor_pose_y", sensor_pose_y, 0.);
paramHandle.param<double>("sensor_pose_th", sensor_pose_th, 0.);
paramHandle.param<bool>("load_map_from_file", loadMap, false);
paramHandle.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));
paramHandle.param<bool>("save_output_map", saveMap, true);
paramHandle.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));
paramHandle.param<double>("map_resolution", resolution , 0.2);
bool forceSIR=false;
paramHandle.param<bool>("forceSIR", forceSIR, false);
std::string bagfilename="bagfile_unset.bag";
paramHandle.param<std::string>("bagfile_name", bagfilename, std::string("bagfile_unset.bag"));
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
/// Prepare the map
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
fprintf(stderr,"USING RESOLUTION %lf\n",resolution);
lslgeneric::NDTMap<pcl::PointXYZ> ndmap(new lslgeneric::LazyGrid<pcl::PointXYZ>(resolution));
ndmap.setMapSize(80.0, 80.0, 1.0);
if(loadMap){
fprintf(stderr,"Loading Map from '%s'\n",mapName.c_str());
ndmap.loadFromJFF(mapName.c_str());
}
ndtmcl = new NDTMCL<pcl::PointXYZ>(resolution,ndmap,-0.5);
if(forceSIR) ndtmcl->forceSIR=true;
fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
/// Open the bag file for reading
//////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////
tfMessageReader<sensor_msgs::LaserScan> reader(bagfilename,
input_laser_topic,
std::string("/world"),
tf_odo_topic);
///Set sensor offsets
offa = sensor_pose_th;
offx = sensor_pose_x;
offy = sensor_pose_y;
has_sensor_offset_set = true;
fprintf(stderr,"Sensor Pose = (%lf %lf %lf)\n",offx, offy, offa);
///Loop while we have data in the bag
while(!reader.bagEnd()){
sensor_msgs::LaserScan s;
tf::Transform odo_pose;
bool hasOdo = false;
bool hasState = false;
if(reader.getNextMessage(s, odo_pose)){
hasOdo = true;
}
tf::Transform state_pose;
if(reader.getTf(tf_state_topic, s.header.stamp, state_pose)){
hasState = true;
}
///If we have the data then lets run the localization
if(hasState && hasOdo){
callback(s,odo_pose,state_pose);
}
}
fprintf(stderr,"-- THE END --\n");
return 0;
}