Merged master->ros2. Updated turtlebot3 RGB-D examples (added instructions to add depth camera instead of copying sdf from another project)

src/GuiWrapper.cpp

@@ -222,6 +222,12 @@ void GuiWrapper::goalReachedCallback(
 
 void GuiWrapper::processRequestedMap(const rtabmap_ros::msg::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/MapAssemblerNode.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
 #include "rtabmap_ros/MapsManager.h"
+#include "rtabmap_ros/GetMap.h"
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -44,6 +45,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <nav_msgs/OccupancyGrid.h>
 #include <std_srvs/Empty.h>
 
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+#include <octomap_msgs/GetOctomap.h>
+#include <octomap_msgs/conversions.h>
+#include <rtabmap/core/OctoMap.h>
+#endif
+#endif
+
 using namespace rtabmap;
 
 class MapAssembler
@@ -63,7 +72,10 @@ public:
 		//parameters
 		rtabmap::ParametersMap parameters;
 		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()))
@@ -112,12 +124,6 @@ public:
 				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
 				iter->second = uNumber2Str(vInt);
 			}
-
-			if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
-			{
-				ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
-				iter->second = uNumber2Str(8);
-			}
 		}
 
 		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
@@ -162,15 +168,75 @@ public:
 			}
 		}
 
+		// set private parameters
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			pnh.setParam(iter->first, iter->second);
+		}
+
 		ROS_INFO("%s: regenerate_local_grids          = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
 		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
 		mapsManager_.backwardCompatibilityParameters(pnh, parameters);
 		mapsManager_.setParameters(parameters);
 
+		std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
+		std::string rtabmapNs;
+		for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
+		{
+			if(!rtabmapNs.empty())
+			{
+				rtabmapNs += "/";
+			}
+			rtabmapNs += *iter;
+		}
+		ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
+		if(rtabmapNs.empty())
+		{
+			rtabmapNs = "get_map_data";
+		}
+		else
+		{
+			rtabmapNs += "/get_map_data";
+		}
+
+		rtabmap_ros::GetMap getMapSrv;
+		getMapSrv.request.global = false;
+		getMapSrv.request.optimized = true;
+		getMapSrv.request.graphOnly = false;
+		if(ros::service::waitForService(rtabmapNs, 5000))
+		{
+			if(!ros::service::call(rtabmapNs, getMapSrv))
+			{
+				ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
+			}
+			else
+			{
+				ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
+				processMapData(getMapSrv.response.data);
+				ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
+						" will be assembled on next subscriber connection.", rtabmapNs.c_str());
+			}
+		}
+		else
+		{
+			ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
+					"may not be a problem if rtabmap is started afterwards. If rtabmap "
+					"is started after in localization mode, call /rtabmap/publish_maps "
+					"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
+		}
+
+
 		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
 
-		// private service
+		// private services
 		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+		octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
+		octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
+#endif
+#endif
 	}
 
 	~MapAssembler()
@@ -178,25 +244,29 @@ public:
 	}
 
 	void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
+	{
+		processMapData(*msg);
+	}
+	void processMapData(const rtabmap_ros::MapData & msg)
 	{
 		UTimer timer;
 
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
 		Transform mapOdom;
-		rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
-		for(unsigned int i=0; i<msg->nodes.size(); ++i)
+		rtabmap_ros::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
+		for(unsigned int i=0; i<msg.nodes.size(); ++i)
 		{
-			if(msg->nodes[i].image.size() ||
-			   msg->nodes[i].depth.size() ||
-			   msg->nodes[i].laserScan.size())
+			if(msg.nodes[i].image.size() ||
+			   msg.nodes[i].depth.size() ||
+			   msg.nodes[i].laserScan.size())
 			{
-				Signature data = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
+				Signature data = rtabmap_ros::nodeDataFromROS(msg.nodes[i]);
 				if(localGridsRegenerated_)
 				{
 					data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
 				}
-				uInsert(nodes_, std::make_pair(msg->nodes[i].id, data));
+				uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
 			}
 		}
 
@@ -211,16 +281,23 @@ public:
 		}
 
 		// Update maps
-		poses = mapsManager_.updateMapCaches(
-				poses,
-				0,
-				false,
-				false,
-				nodes_);
+		if(!nodes_.empty())
+		{
+			poses = mapsManager_.updateMapCaches(
+					poses,
+					0,
+					false,
+					false,
+					nodes_);
+		}
+		double updateTime = timer.ticks();
 
-		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
+		mapFrameId_ = msg.header.frame_id;
+		optimizedPoses_ = poses;
 
-		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
+		mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
+
+		ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
 	}
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -230,20 +307,64 @@ public:
 		return true;
 	}
 
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	bool octomapBinaryCallback(
+			octomap_msgs::GetOctomap::Request  &req,
+			octomap_msgs::GetOctomap::Response &res)
+	{
+		ROS_INFO("Sending binary map data on service request");
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+
+	bool octomapFullCallback(
+			octomap_msgs::GetOctomap::Request  &req,
+			octomap_msgs::GetOctomap::Response &res)
+	{
+		ROS_INFO("Sending full map data on service request");
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+#endif
+#endif
+
 private:
 	MapsManager mapsManager_;
 	std::map<int, Signature> nodes_;
+	std::map<int, Transform> optimizedPoses_;
+	std::string mapFrameId_;
 
 	ros::Subscriber mapDataTopic_;
 
 	ros::ServiceServer resetService_;
-
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	ros::ServiceServer octomapBinarySrv_;
+	ros::ServiceServer octomapFullSrv_;
+#endif
+#endif
 	bool localGridsRegenerated_;
 };
 
 
 int main(int argc, char** argv)
 {
+	ULogger::setLevel(ULogger::kError);
+	ULogger::setType(ULogger::kTypeConsole);
+
 	ros::init(argc, argv, "map_assembler");
 
 	// process "--params" argument
@@ -253,7 +374,10 @@ int main(int argc, char** argv)
 		{
 			rtabmap::ParametersMap parameters;
 			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/PreferencesDialogROS.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QMessageBox>
 #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 &)
 {
 	this->setInputRate(0);
@@ -77,6 +83,13 @@ QString PreferencesDialogROS::getParamMessage()
 	return tr("Reading parameters from the ROS server...");
 }
 
+bool PreferencesDialogROS::hasAllParameters()
+{
+	auto node = std::make_shared<rclcpp::Node>("rtabmapviz");
+	auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
+	return client->service_is_ready();
+}
+
 bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 {
 	QString path = getIniFilePath();
@@ -102,6 +115,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 	}
 
 	std::vector<std::string> rosParameters;
+
 	for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
 	{
 		if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0)

src/rviz/MapCloudDisplay.cpp

@@ -137,6 +137,7 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_from_scan_ = new rviz_common::properties::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_common::properties::IntProperty( "Cloud decimation", 4,
 										 "Decimation of the input RGB and depth images before creating the cloud.",
@@ -269,6 +270,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::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;
@@ -366,6 +368,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
 				}
 			}
 		}
+		nodeDataReceived.insert(id);
 	}
 
 	// Update graph
@@ -380,6 +383,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
 		std::unique_lock<std::mutex> lock(current_map_mutex_);
 		current_map_ = poses;
 		current_map_updated_ = true;
+		nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
 	}
 }
 
@@ -486,7 +490,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*/)
@@ -706,7 +717,8 @@ void MapCloudDisplay::update( float, float )
 					cloudInfoIt->second->pose_ = it->second;
 					Ogre::Vector3 framePosition;
 					Ogre::Quaternion frameOrientation;
-					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, framePosition, frameOrientation))
+					std::string error;
+					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, framePosition, frameOrientation))
 					{
 						// Multiply frame with pose
 						Ogre::Matrix4 frameTransform;
@@ -726,15 +738,16 @@ void MapCloudDisplay::update( float, float )
 						cloudInfoIt->second->scene_node_->setVisible(true);
 						++totalNodesShown;
 					}
-					else
+					else if(context_->getFrameManager()->transformHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
 					{
-						RVIZ_COMMON_LOG_ERROR(uFormat("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")", 
+						RVIZ_COMMON_LOG_ERROR(uFormat("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()));
 					}
 				}
-				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);
 				}
@@ -792,6 +805,7 @@ void MapCloudDisplay::reset()
 		std::unique_lock<std::mutex> lock(current_map_mutex_);
 		current_map_.clear();
 		current_map_updated_ = false;
+		nodeDataReceived_.clear();
 	}
 }
 

src/rviz/MapCloudDisplay.h

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