fixed MapsManager's topics not remapped when using rtabmap nodelet

include/rtabmap_ros/CommonDataSubscriber.h

@@ -69,7 +69,7 @@ public:
 	int getQueueSize() const {return queueSize_;}
 
 protected:
-	void setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh);
+	void setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name);
 	virtual void commonDepthCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -154,6 +154,7 @@ private:
 	bool subscribedToScan2d_;
 	bool subscribedToScan3d_;
 	bool subscribedToOdomInfo_;
+	std::string name_;
 
 	//for depth callback
 	image_transport::SubscriberFilter imageSub_;

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -100,7 +100,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s", \
-				ros::this_node::getName().c_str(), \
+				name_.c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str());
@@ -119,7 +119,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s", \
-				ros::this_node::getName().c_str(), \
+				name_.c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str(), \
@@ -139,7 +139,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s", \
-				ros::this_node::getName().c_str(), \
+				name_.c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str(), \
@@ -160,7 +160,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
-				ros::this_node::getName().c_str(), \
+				name_.c_str(), \
 				approxSync?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str(), \
@@ -182,7 +182,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
-				ros::this_node::getName().c_str(), \
+				name_.c_str(), \
 				APPROX?"approx":"exact", \
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str(), \

include/rtabmap_ros/MapsManager.h

@@ -45,11 +45,12 @@ class OccupancyGrid;
 
 class MapsManager {
 public:
-	MapsManager(bool usePublicNamespace);
+	MapsManager();
 	virtual ~MapsManager();
+	void init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace);
 	void clear();
 	bool hasSubscribers() const;
-	void backwardCompatibilityParameters(rtabmap::ParametersMap & parameters) const;
+	void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const;
 	void setParameters(const rtabmap::ParametersMap & parameters);
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(

src/CommonDataSubscriber.cpp

@@ -123,13 +123,14 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 {
 }
 
-void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh)
+void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name)
 {
 	bool subscribeScan2d = false;
 	bool subscribeScan3d = false;
 	bool subscribeOdomInfo = false;
 	bool subscribeUserData = false;
 	int rgbdCameras = 1;
+	name_ = name;
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
@@ -201,9 +202,9 @@ void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle
 		rgbdCameras = 1;
 	}
 
-	ROS_INFO("%s: queue_size    = %d", ros::this_node::getName().c_str(), queueSize_);
-	ROS_INFO("%s: rgbd_cameras = %d", ros::this_node::getName().c_str(), rgbdCameras);
-	ROS_INFO("%s: approx_sync   = %s", ros::this_node::getName().c_str(), approxSync_?"true":"false");
+	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
+	ROS_INFO("%s: rgbd_cameras = %d", name.c_str(), rgbdCameras);
+	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");
 
 	bool subscribeOdom = odomFrameId.empty();
 	if(subscribedToDepth_)
@@ -372,7 +373,7 @@ void CommonDataSubscriber::warningLoop()
 					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
 					"header are set. If topics are coming from different computers, make sure "
 					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
-					ros::this_node::getName().c_str(),
+					name_.c_str(),
 					approxSync_?
 							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
 							"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",

src/CoreWrapper.cpp

@@ -101,7 +101,6 @@ CoreWrapper::CoreWrapper() :
 		scanCloudMaxPoints_(0),
 		scanCloudNormalK_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
-		mapsManager_(true),
 		transformThread_(0),
 		tfThreadRunning_(false),
 		stereoToDepth_(false),
@@ -119,6 +118,8 @@ void CoreWrapper::onInit()
 	ros::NodeHandle & nh = getNodeHandle();
 	ros::NodeHandle & pnh = getPrivateNodeHandle();
 
+	mapsManager_.init(nh, pnh, getName(), true);
+
 	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
 	double tfTolerance = 0.1; // 100 ms
@@ -331,7 +332,7 @@ void CoreWrapper::onInit()
 	}
 
 	// Backward compatibility (MapsManager)
-	mapsManager_.backwardCompatibilityParameters(parameters_);
+	mapsManager_.backwardCompatibilityParameters(pnh, parameters_);
 
 	bool subscribeScan2d = false;
 	bool subscribeScan3d = false;
@@ -448,7 +449,7 @@ void CoreWrapper::onInit()
 				Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
 	}
 
-	setupCallbacks(nh, pnh); // do it at the end
+	setupCallbacks(nh, pnh, getName()); // do it at the end
 	if(!this->isDataSubscribed())
 	{
 		bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
@@ -466,7 +467,7 @@ void CoreWrapper::onInit()
 		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
 		defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
 
-		NODELET_INFO("\n%s subscribed to:\n   %s", ros::this_node::getName().c_str(), defaultSub_.getTopic().c_str());
+		NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
 	}
 }
 

src/GuiWrapper.cpp

@@ -171,7 +171,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
 	goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this);
 
-	setupCallbacks(nh, pnh); // do it at the end
+	setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end
 	if(!this->isDataSubscribed())
 	{
 		defaultSub_ = nh.subscribe("odom", queueSize_, &GuiWrapper::defaultCallback, this);

src/MapAssemblerNode.cpp

@@ -49,12 +49,13 @@ class MapAssembler
 {
 
 public:
-	MapAssembler() :
-		mapsManager_(false)
+	MapAssembler()
 	{
 		ros::NodeHandle pnh("~");
-
 		ros::NodeHandle nh;
+
+		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
+
 		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
 
 		// private service

src/MapsManager.cpp

@@ -57,7 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace rtabmap;
 
-MapsManager::MapsManager(bool usePublicNamespace) :
+MapsManager::MapsManager() :
 		cloudOutputVoxelized_(true),
 		cloudSubtractFiltering_(false),
 		cloudSubtractFilteringMinNeighbors_(2),
@@ -77,10 +77,10 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		octomapTreeDepth_(16),
 		octomapOccupancyThr_(0.5)
 {
+}
 
-	ros::NodeHandle nh;
-	ros::NodeHandle pnh("~");
-
+void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace)
+{
 	// common grid map stuff
 	pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
 	if(gridCellSize_ <= 0)
@@ -113,7 +113,6 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_);
 	pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_);
 
-	std::string name = ros::this_node::getName();
 	ROS_INFO("%s(maps): grid_cell_size             = %f", name.c_str(), gridCellSize_);
 	ROS_INFO("%s(maps): grid_incremental           = %s", name.c_str(), gridIncremental_?"true":"false");
 	ROS_INFO("%s(maps): grid_size                  = %f", name.c_str(), gridSize_);
@@ -155,23 +154,31 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("latch", latch, latch);
 
 	// mapping topics
-	ros::NodeHandle nht(usePublicNamespace?"":"~");
-	gridMapPub_ = nht.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
-	cloudMapPub_ = nht.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
-	cloudObstaclesPub_ = nht.advertise<sensor_msgs::PointCloud2>("cloud_obstacles", 1, latch);
-	cloudGroundPub_ = nht.advertise<sensor_msgs::PointCloud2>("cloud_ground", 1, latch);
+	ros::NodeHandle * nht;
+	if(usePublicNamespace)
+	{
+		nht = &nh;
+	}
+	else
+	{
+		nht = &pnh;
+	}
+	gridMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
+	cloudMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
+	cloudObstaclesPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_obstacles", 1, latch);
+	cloudGroundPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_ground", 1, latch);
 
 	// deprecated
-	projMapPub_ = nht.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
-	scanMapPub_ = nht.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
+	projMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
+	scanMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
 
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
-	octoMapPubBin_ = nht.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
-	octoMapPubFull_ = nht.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
-	octoMapCloud_ = nht.advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latch);
-	octoMapEmptySpace_ = nht.advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
-	octoMapProj_ = nht.advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latch);
+	octoMapPubBin_ = nht->advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
+	octoMapPubFull_ = nht->advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
+	octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latch);
+	octoMapEmptySpace_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
+	octoMapProj_ = nht->advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latch);
 #endif
 #endif
 }
@@ -234,10 +241,8 @@ void parameterMoved(
 	}
 }
 
-void MapsManager::backwardCompatibilityParameters(ParametersMap & parameters) const
+void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, ParametersMap & parameters) const
 {
-	ros::NodeHandle pnh("~");
-
 	// removed
 	if(pnh.hasParam("cloud_frustum_culling"))
 	{

src/OdometryROS.cpp

@@ -322,7 +322,7 @@ void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool appr
 			ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
 					"published (\"$ rostopic hz my_topic\") and the timestamps in their "
 					"header are set. %s%s",
-					ros::this_node::getName().c_str(),
+					getName().c_str(),
 					approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
 						"topics should have all the exact timestamp for the callback to be called.",
 					subscribedTopicsMsg.c_str());

src/nodelets/rgbd_odometry.cpp

@@ -142,7 +142,7 @@ private:
 					exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
 				}
 				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+						getName().c_str(),
 						approxSync?"approx":"exact",
 						rgbd_image1_sub_.getTopic().c_str(),
 						rgbd_image2_sub_.getTopic().c_str());
@@ -153,7 +153,7 @@ private:
 
 				subscribedTopicsMsg =
 						uFormat("\n%s subscribed to:\n   %s",
-						ros::this_node::getName().c_str(),
+						getName().c_str(),
 						rgbdSub_.getTopic().c_str());
 			}
 		}
@@ -184,7 +184,7 @@ private:
 			}
 
 			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
-					ros::this_node::getName().c_str(),
+					getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),

src/nodelets/rgbdicp_odometry.cpp

@@ -141,7 +141,7 @@ private:
 			}
 
 			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
-					ros::this_node::getName().c_str(),
+					getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
@@ -163,7 +163,7 @@ private:
 			}
 
 			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
-					ros::this_node::getName().c_str(),
+					getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),

src/nodelets/stereo_odometry.cpp

@@ -114,7 +114,7 @@ private:
 
 
 		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
-				ros::this_node::getName().c_str(),
+				getName().c_str(),
 				approxSync?"approx":"exact",
 				imageRectLeft_.getTopic().c_str(),
 				imageRectRight_.getTopic().c_str(),