rtabmapviz: automatically read parameters from ros param server when receiving a map (fixing occupancy grid cell size not correctly set if rtabmapviz is started before rtabmap node, without having to open Preferences dialog and click apply to refresh the gui parameters)

src/GuiWrapper.cpp

@@ -235,6 +235,12 @@ void GuiWrapper::goalReachedCallback(
 
 void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 {
+	// Make sure parameters are loaded
+	if(((PreferencesDialogROS*)prefDialog_)->hasAllParameters())
+	{
+		QMetaObject::invokeMethod(((PreferencesDialogROS*)prefDialog_), "readRtabmapNodeParameters");
+	}
+
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
 	std::multimap<int, rtabmap::Link> constraints;

src/PreferencesDialogROS.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/exceptions.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThread.h>
 
 using namespace rtabmap;
 
@@ -67,6 +68,11 @@ QString PreferencesDialogROS::getTmpIniFilePath() const
 	return QDir::homePath()+"/.ros/"+QFileInfo(configFile_).fileName()+".tmp";
 }
 
+void PreferencesDialogROS::readRtabmapNodeParameters()
+{
+	readCoreSettings(getTmpIniFilePath());
+}
+
 void PreferencesDialogROS::readCameraSettings(const QString & filePath)
 {
 	this->setInputRate(0);
@@ -77,6 +83,32 @@ QString PreferencesDialogROS::getParamMessage()
 	return tr("Reading parameters from the ROS server...");
 }
 
+bool PreferencesDialogROS::hasAllParameters()
+{
+	ros::NodeHandle nh(rtabmapNodeName_);
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
+	{
+		if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && !nh.hasParam(i->first))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool PreferencesDialogROS::hasAllParameters(const ros::NodeHandle & nh, const rtabmap::ParametersMap & parameters)
+{
+	for(rtabmap::ParametersMap::const_iterator i=parameters.begin(); i!=parameters.end(); ++i)
+	{
+		if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) != 0 && !nh.hasParam(i->first))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
 bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 {
 	QString path = getIniFilePath();
@@ -103,23 +135,31 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		}
 	}
 
-	// Wait rtabmap parameters to appear (if gui noe has been launched at the same time than rtabmap)
+	// Wait rtabmap parameters to appear (if gui node has been launched at the same time than rtabmap)
 	if(!this->isVisible())
 	{
 		double stamp = UTimer::now();
 		std::string tmp;
 		bool warned = false;
-		while(!rnh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
+		while(!hasAllParameters(rnh, parameters) && UTimer::now()-stamp < 5.0)
 		{
 			if(!warned)
 			{
 				ROS_INFO("rtabmapviz: Cannot get rtabmap's parameters, waiting max 5 seconds in case the node has just been launched.");
 				warned = true;
 			}
+			uSleep(100);
 		}
-		if(warned && UTimer::now()-stamp < 5.0)
+		if(warned)
 		{
-			ROS_INFO("rtabmapviz: rtabmap's parameters seem now there! continuing...");
+			if(UTimer::now()-stamp < 5.0)
+			{
+				ROS_INFO("rtabmapviz: rtabmap's parameters seem now there! continuing...");
+			}
+			else
+			{
+				ROS_WARN("rtabmapviz: rtabmap's parameters seem not all there yet! continuing with those there if some...");
+			}
 		}
 	}
 

src/rviz/MapCloudDisplay.cpp

@@ -741,6 +741,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 					cloudInfoIt->second->pose_ = it->second;
 					Ogre::Vector3 framePosition;
 					Ogre::Quaternion frameOrientation;
+					std::string error;
 					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation))
 					{
 						// Multiply frame with pose
@@ -761,11 +762,12 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 						cloudInfoIt->second->scene_node_->setVisible(true);
 						++totalNodesShown;
 					}
-					else
+					else if(context_->getFrameManager()->frameHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
 					{
-						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
+						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\" (reason=%s), set fixed frame in global options to \"%s\")",
 								it->first,
 								cloudInfoIt->second->message_->header.frame_id.c_str(),
+								error.c_str(),
 								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}
 				}