Merged master->ros2

src/CoreWrapper.cpp

@@ -245,6 +245,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	infoPub_ = this->create_publisher<rtabmap_ros::msg::Info>("info", 1);
 	mapDataPub_ = this->create_publisher<rtabmap_ros::msg::MapData>("mapData", 1);
 	mapGraphPub_ = this->create_publisher<rtabmap_ros::msg::MapGraph>("mapGraph", 1);
+	odomCachePub_ = this->create_publisher<rtabmap_ros::msg::MapGraph>("mapOdomCache", 1);
 	landmarksPub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("landmarks", 1);
 	labelsPub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("labels", 1);
 	mapPathPub_ = this->create_publisher<nav_msgs::msg::Path>("mapPath", 1);
@@ -792,6 +793,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1));
 #ifdef WITH_APRILTAG_MSGS
 	tagDetectionsSub_ = this->create_subscription<apriltag_ros::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
+#endif
+#ifdef WITH_FIDUCIAL_MSGS
+	fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1));
 #endif
 	imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
 	republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
@@ -964,7 +968,10 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
 		Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg.pose.pose);
 		if(!odom.isNull())
 		{
-			Transform odomTF = rtabmap_ros::getTransform(odomMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_);
+			Transform odomTF;
+			if(!stamp.seconds() == 0.0) {
+				odomTF = rtabmap_ros::getTransform(odomMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_);
+			}
 			if(odomTF.isNull())
 			{
 				static bool shown = false;
@@ -2442,6 +2449,27 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD
 }
 #endif
 
+#ifdef WITH_FIDUCIAL_MSGS
+void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::FiducialTransformArray::SharedPtr fiducialDetections)
+{
+	if(!paused_)
+	{
+		for(unsigned int i=0; i<fiducialDetections.transforms.size(); ++i)
+		{
+			geometry_msgs::PoseWithCovarianceStamped p;
+			p.pose.pose.orientation = fiducialDetections.transforms[i].transform.rotation;
+			p.pose.pose.position.x = fiducialDetections.transforms[i].transform.translation.x;
+			p.pose.pose.position.y = fiducialDetections.transforms[i].transform.translation.y;
+			p.pose.pose.position.z = fiducialDetections.transforms[i].transform.translation.z;
+			p.header = fiducialDetections.header;
+			uInsert(tags_,
+					std::make_pair(fiducialDetections.transforms[i].fiducial_id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
 void CoreWrapper::imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
 {
 	if(!paused_)
@@ -4128,6 +4156,40 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 		mapGraphPub_->publish(std::move(msg));
 	}
 
+	if(odomCachePub_->get_subscription_count())
+	{
+		rtabmap_ros::msg::MapGraph::UniquePtr msg(new rtabmap_ros::msg::MapGraph);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		// For visualization of the constraints (MapGraph rviz plugin), we should include target nodes from the map
+		std::map<int, Transform> poses = stats.odomCachePoses();
+		// transform in map frame
+		for(std::map<int, Transform>::iterator iter=poses.begin();
+			iter!=poses.end();
+			++iter)
+		{
+			iter->second = stats.mapCorrection() * iter->second;
+		}
+		for(std::multimap<int, rtabmap::Link>::const_iterator iter=stats.odomCacheConstraints().begin();
+			iter!=stats.odomCacheConstraints().end();
+			++iter)
+		{
+			std::map<int, Transform>::const_iterator pter = stats.poses().find(iter->second.to());
+			if(pter != stats.poses().end())
+			{
+				poses.insert(*pter);
+			}
+		}
+		rtabmap_ros::mapGraphToROS(
+			poses,
+			stats.odomCacheConstraints(),
+			stats.mapCorrection(),
+			*msg);
+
+		odomCachePub_->publish(std::move(msg));
+	}
+
 	if(localGridObstacle_->get_subscription_count() && !stats.getLastSignatureData().sensorData().gridObstacleCellsRaw().empty())
 	{
 		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridObstacleCellsRaw()));

src/MsgConversion.cpp

@@ -513,6 +513,13 @@ void infoFromROS(const rtabmap_ros::msg::Info & info, rtabmap::Statistics & stat
 	stat.setLocalPath(info.local_path);
 	stat.setCurrentGoalId(info.current_goal_id);
 
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> constraints;
+	rtabmap::Transform t;
+	mapGraphFromROS(info.odom_cache, poses, constraints, t);
+	stat.setOdomCachePoses(poses);
+	stat.setOdomCacheConstraints(constraints);
+
 	// Statistics data
 	for(unsigned int i=0; i<info.stats_keys.size() && i<info.stats_values.size(); i++)
 	{
@@ -548,6 +555,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::msg::Info & info)
 		info.labels_values = uValues(stats.labels());
 		info.local_path = stats.localPath();
 		info.current_goal_id = stats.currentGoalId();
+		mapGraphToROS(stats.odomCachePoses(), stats.odomCacheConstraints(), stats.mapCorrection(), info.odom_cache);
 
 		// Statistics data
 		info.stats_keys = uKeys(stats.data());