diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp index 0d54a5d7e9cfb1169edf988047d2030b50e56ebf..104ddcb2208749040a4e83c9d756958ac7191ea2 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp @@ -34,7 +34,11 @@ protected: std::string baseFrame; std::string mapFrame; std::string odomFrame; - std::string pubParticles; + std::string pub_historyName; + std::string pub_mapName; + std::string pub_particlesName; + std::string sub_scanName; + std::string sub_snapName; ros::Subscriber scanSub; ros::Subscriber snapSub; diff --git a/crosbot_fastslam/src/module.cpp b/crosbot_fastslam/src/module.cpp index e38c18d8d0f69a58fd161b44c353338a04e325cc..b7872cf13677e86f5aa8481faae8ccd59b5e4421 100644 --- a/crosbot_fastslam/src/module.cpp +++ b/crosbot_fastslam/src/module.cpp @@ -68,14 +68,18 @@ void FastSLAMModule::initialize(ros::NodeHandle& nh) { paramNH.param("base_frame", baseFrame, DEFAULT_BASEFRAME); paramNH.param("map_frame", mapFrame, DEFAULT_MAPFRAME); paramNH.param("odom_frame", odomFrame, DEFAULT_ODOMFRAME); - paramNH.param("pub_particles", pubParticles, "/crosbot_fastslam/particles"); + paramNH.param("pub_history", pub_historyName, "/crosbot_fastslam/history"); + paramNH.param("pub_map", pub_mapName, "/crosbot_fastslam/map"); + paramNH.param("pub_particles", pub_particlesName, "/crosbot_fastslam/particles"); + paramNH.param("sub_scan", sub_scanName, "/scan"); + paramNH.param("sub_snap", sub_snapName, "/crosbot_fastslam/snaps"); ROS_INFO("%s map: %s, odom: %s, base: %s", LOG_START, mapFrame.c_str(), odomFrame.c_str(), baseFrame.c_str()); - scanSub = nh.subscribe("scan", 1, &FastSLAMModule::callbackScan, this); - snapSub = nh.subscribe("snap", 1000, &FastSLAMModule::callbackSnap, this); - gridPub = nh.advertise("map", 1, true); - historyPub = nh.advertise("history", 1); - particlesPub = nh.advertise(pubParticles, 1); + scanSub = nh.subscribe(sub_scanName, 1, &FastSLAMModule::callbackScan, this); + snapSub = nh.subscribe(sub_snapName, 1000, &FastSLAMModule::callbackSnap, this); + gridPub = nh.advertise(pub_mapName, 1, true); + historyPub = nh.advertise(pub_historyName, 1); + particlesPub = nh.advertise(pub_particlesName, 1); // read configuration/parameters ConfigElementPtr config = new ROSConfigElement(paramNH);