Added demo_catvehicle_mapping.launch (#592). MapCloud display: fixed clouds requested when changing sensor modality.

src/MapAssemblerNode.cpp

@@ -74,6 +74,8 @@ public:
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
 		if(!configPath.empty())
 		{
 			if(UFile::exists(configPath.c_str()))
@@ -374,6 +376,8 @@ int main(int argc, char** argv)
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
 			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
 			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 			{
 				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";

src/rviz/MapCloudDisplay.cpp

@@ -137,6 +137,7 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_from_scan_ = new rviz::BoolProperty( "Cloud from scan", false,
 										 "Create the cloud from laser scans instead of the RGB-D/Stereo images.",
 										 this, SLOT( updateCloudParameters() ), this );
+	fromScan_ = cloud_from_scan_->getBool();
 
 	cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4,
 										 "Decimation of the input RGB and depth images before creating the cloud.",
@@ -281,6 +282,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 	// Add new clouds...
 	bool fromDepth = !cloud_from_scan_->getBool();
+	std::set<int> nodeDataReceived;
 	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
 	{
 		int id = map.nodes[i].id;
@@ -378,6 +380,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 				}
 			}
 		}
+		nodeDataReceived.insert(id);
 	}
 
 	// Update graph
@@ -392,6 +395,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_ = poses;
 		current_map_updated_ = true;
+		nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
 	}
 }
 
@@ -531,7 +535,14 @@ void MapCloudDisplay::updateBillboardSize()
 
 void MapCloudDisplay::updateCloudParameters()
 {
-	// do nothing... only take effect on next generated clouds
+	// do nothing for most parameters... only take effect on next generated clouds
+
+	// if we change the kind of map, clear
+	if(fromScan_ != cloud_from_scan_->getBool())
+	{
+		reset();
+	}
+	fromScan_ = cloud_from_scan_->getBool();
 }
 
 void MapCloudDisplay::downloadMap(bool graphOnly)
@@ -758,7 +769,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}
 				}
-				else if(it->first>0 && current_map_updated_)
+				else if(it->first>0 && current_map_updated_&& nodeDataReceived_.find(it->first) == nodeDataReceived_.end())
 				{
 					missingNodes.push_back(it->first);
 				}
@@ -816,6 +827,7 @@ void MapCloudDisplay::reset()
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_.clear();
 		current_map_updated_ = false;
+		nodeDataReceived_.clear();
 	}
 	MFDClass::reset();
 }

src/rviz/MapCloudDisplay.h

@@ -175,6 +175,9 @@ private:
 	std::map<int, CloudInfoPtr> new_cloud_infos_;
 	boost::mutex new_clouds_mutex_;
 
+	std::set<int> nodeDataReceived_;
+	bool fromScan_;
+
 	std::map<int, rtabmap::Transform> current_map_;
 	boost::mutex current_map_mutex_;
 	bool current_map_updated_;