rtabmap_util tests and doc (#1450)

* Initial tests

* more tests

* More in-depth deskew() testing

* slightly less verbose clamping corruption warning

* added tf buffer related tests

* added remaining tests

* Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp

* cleanup doc

* fixing ci

* rtabmap_util tests and doc

* Added db_player tests

* Added MapsManager tests

* Added map_assembler tests

* Documenting node first draft

* relative links

* Fixed british->usa english style. Reviewed all md files.

* added link to install ros1

* updated badges

* added Iron

* added ubuntu

* added codecov

* updated coverage ci

* fixing rosdep

* updated ci cov job

* ci bump

* fixing cov ci

* small doc cleanup

rtabmap_util/include/rtabmap_util/MapsManager.h

@@ -57,22 +57,131 @@ class GridMap;
 
 namespace rtabmap_util {
 
+/**
+ * @brief Turns a pose graph into the map topics, and publishes them.
+ *
+ * Given a set of node poses and the sensor data behind them, MapsManager assembles the
+ * ground and obstacle point clouds, the 2D occupancy grid, the octomap and the elevation
+ * map, and publishes whichever of them somebody is subscribed to.
+ *
+ * It is shared by rtabmap_slam's `rtabmap` node and rtabmap_util's `map_assembler`, which
+ * is why those two produce identical maps from identical parameters.
+ *
+ * @par Lifecycle
+ * Callers follow a fixed order:
+ * 1. init() to declare the ROS parameters and advertise the topics,
+ * 2. backwardCompatibilityParameters() to pick up parameters that have since moved into
+ *    the RTAB-Map library, then setParameters() to apply the whole set,
+ * 3. updateMapCaches() whenever the graph changes, then publishMaps().
+ *
+ * @par Laziness
+ * Nothing is assembled or published without a subscriber, and with `map_cleanup` set the
+ * caches are released once the last one goes away. A node can therefore call
+ * updateMapCaches() and publishMaps() unconditionally on every graph update and pay
+ * nothing while nobody is listening.
+ *
+ */
 class MapsManager {
 public:
 	MapsManager();
 	virtual ~MapsManager();
+
+	/**
+	 * @brief Declares the ROS parameters and advertises the map topics on @p node.
+	 *
+	 * Must be called before anything else, and exactly once per node: the parameters are
+	 * declared here, and declaring them twice throws.
+	 *
+	 * @param node               node to advertise on and read parameters from
+	 * @param name               prefix used in the log lines, normally the node's name
+	 * @param usePublicNamespace unused, kept for source compatibility
+	 */
 	void init(rclcpp::Node & node, const std::string & name, bool usePublicNamespace);
+
+	/// Drops every cached local grid, assembled cloud and global map.
 	void clear();
+
+	/// @return True if any map topic has at least one subscriber.
 	bool hasSubscribers() const;
+
+	/// @return True if the map topics are latched, i.e. delivered to late subscribers.
 	bool isLatching() const {return latching_;}
+
+	/**
+	 * @brief Whether the map changed on the last updateMapCaches().
+	 *
+	 * @note Reports true when nothing is subscribed to the grid topics. The answer comes
+	 *       from OccupancyGrid::update(), which only runs when a grid is wanted, so with
+	 *       nobody listening the safe assumption is that the graph moved.
+	 */
 	bool isMapUpdated() const;
+
+	/**
+	 * @brief Copies parameters that moved from rtabmap_ros into the RTAB-Map library.
+	 *
+	 * Reads the old ROS parameter names off @p node and, for each one that is set, writes
+	 * its value into @p parameters under the RTAB-Map name that replaced it, with a
+	 * warning. Call it before setParameters().
+	 *
+	 * @param[in]     node       node to read the legacy parameters from
+	 * @param[in,out] parameters parameter set to fill in
+	 */
 	void backwardCompatibilityParameters(rclcpp::Node & node, rtabmap::ParametersMap & parameters) const;
+
+	/**
+	 * @brief Applies the RTAB-Map parameters, rebuilding the map objects.
+	 *
+	 * The occupancy grid, octomap and elevation map are recreated, so anything already
+	 * assembled is lost; the cached local grids are kept.
+	 */
 	void setParameters(const rtabmap::ParametersMap & parameters);
+
+	/**
+	 * @brief Installs an already assembled 2D map, e.g. one loaded from a database.
+	 *
+	 * @param map      the grid, `CV_8SC1` with -1 unknown, 0 free, 100 occupied
+	 * @param xMin     world x of the map's origin, in meters
+	 * @param yMin     world y of the map's origin, in meters
+	 * @param cellSize resolution, in meters
+	 * @param poses    poses of the nodes @p map was assembled from
+	 * @param memory   optional memory to load the missing local grids from, so the map can
+	 *                 keep growing from where it left off
+	 *
+	 * @warning @p poses must not be empty. The grid is kept only together with the nodes
+	 *          it came from, so that the manager knows which are already in it; a call
+	 *          with no poses is ignored with a warning.
+	 */
 	void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
 
+	/**
+	 * @brief Applies the `map_filter_radius`/`map_filter_angle` thinning to @p poses.
+	 * @return The poses that survive, or all of them when filtering is disabled.
+	 */
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
 
+	/**
+	 * @brief Brings the local grid cache and the global maps up to date with the graph.
+	 *
+	 * For every pose not already mapped, the local occupancy grid is taken from the
+	 * signature or the memory, or regenerated from the sensor data when the node carries
+	 * none, and added to the cache. The global maps are then reassembled.
+	 *
+	 * @param poses         node poses; landmarks (negative ids) are ignored, and id 0 is
+	 *                      the not-yet-committed node, kept only if `map_always_update`
+	 * @param memory        memory to load node data from, may be null if @p signatures
+	 *                      carries everything
+	 * @param updateGrid    force the occupancy grid to be updated
+	 * @param updateOctomap force the octomap to be updated
+	 * @param signatures    node data, keyed by id, for nodes not in @p memory
+	 * @return The poses actually mapped, after filtering.
+	 *
+	 * @note With @p updateGrid and @p updateOctomap both false, what gets updated is
+	 *       decided by which topics have subscribers. That is also the only way the
+	 *       elevation map is ever built, as it has no flag of its own.
+	 * @note At least one of @p memory and @p signatures must be non-empty, and @p poses
+	 *       must not be empty; otherwise an error is logged and nothing is returned.
+	 */
 	std::map<int, rtabmap::Transform> updateMapCaches(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rtabmap::Memory * memory,
@@ -80,25 +189,49 @@ public:
 			bool updateOctomap,
 			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
 
+	/**
+	 * @brief Publishes every map topic that has a subscriber.
+	 *
+	 * @param poses      the same poses updateMapCaches() returned
+	 * @param stamp      stamp for all published messages
+	 * @param mapFrameId frame id for all published messages
+	 */
 	void publishMaps(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rclcpp::Time & stamp,
 			const std::string & mapFrameId);
 
+	/**
+	 * @brief The 2D occupancy grid as a ternary map.
+	 * @param[out] xMin         world x of the map's origin, in meters
+	 * @param[out] yMin         world y of the map's origin, in meters
+	 * @param[out] gridCellSize resolution, in meters
+	 * @return `CV_8SC1`, -1 unknown, 0 free, 100 occupied. Empty if nothing is assembled.
+	 */
 	cv::Mat getGridMap(
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
 
+	/**
+	 * @brief The 2D occupancy grid as probabilities.
+	 * @param[out] xMin         world x of the map's origin, in meters
+	 * @param[out] yMin         world y of the map's origin, in meters
+	 * @param[out] gridCellSize resolution, in meters
+	 * @return `CV_8SC1`, -1 unknown, otherwise 0-100. Empty if nothing is assembled.
+	 */
 	cv::Mat getGridProbMap(
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
 
 #ifdef RTABMAP_OCTOMAP
+	/// @return The octomap, owned by this object. Never null.
 	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
 #endif
+	/// @return The global occupancy grid, owned by this object. Never null.
 	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
+	/// @return The local grid segmenter, owned by this object. Never null.
 	const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
 
 private:

rtabmap_util/include/rtabmap_util/map_assembler.hpp

@@ -62,6 +62,9 @@ private:
 
 	void timerCallback();
 
+	/// Subscribes to "mapData"; the node is live from here on.
+	void subscribeToMapData();
+
 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
 	void octomapBinaryCallback(
@@ -100,6 +103,7 @@ private:
 #endif
 #endif
 	bool localGridsRegenerated_;
+	double initializeFromRtabmapTimeout_;
 };
 
 }

rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/sync_policies/approximate_time.hpp>
 #include <message_filters/subscriber.hpp>
 #include <message_filters/sync_policies/exact_time.hpp>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 namespace rtabmap_util
 {
@@ -66,8 +67,7 @@ private:
 						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2);
 	void combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs);
 
-	std::thread * warningThread_;
-	bool callbackCalled_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync4Policy;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync4Policy;

rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/MsgConversion.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 namespace rtabmap_util
 {
@@ -73,8 +74,7 @@ private:
 	void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
 
 private:
-	std::thread * warningThread_;
-	bool callbackCalled_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
 
 	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;

rtabmap_util/include/rtabmap_util/rgbd_split.hpp

@@ -48,6 +48,9 @@ public:
 	void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const;
 
 private:
+	/// True when the outputs are named left/right rather than rgb/depth.
+	bool stereo_;
+
 	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
 
 	image_transport::Publisher rgbPub_;