0.18.3: added apriltags2 support (input topic: tag_detections)

include/rtabmap_ros/CoreWrapper.h

@@ -62,6 +62,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <octomap_msgs/GetOctomap.h>
 #endif
 
+#ifdef WITH_APRILTAGS2_ROS
+#include <apriltags2_ros/AprilTagDetectionArray.h>
+#endif
+
 #include <actionlib/client/simple_action_client.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <move_base_msgs/MoveBaseActionGoal.h>
@@ -129,6 +133,9 @@ private:
 	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
 	void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
 	void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg);
+#ifdef WITH_APRILTAGS2_ROS
+	void tagDetectionsAsyncCallback(const apriltags2_ros::AprilTagDetectionArray & tagDetections);
+#endif
 
 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
 
@@ -208,6 +215,8 @@ private:
 	std::string databasePath_;
 	double odomDefaultAngVariance_;
 	double odomDefaultLinVariance_;
+	double landmarkDefaultAngVariance_;
+	double landmarkDefaultLinVariance_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
 	bool useActionForGoal_;
@@ -292,6 +301,8 @@ private:
 	geometry_msgs::PoseWithCovarianceStamped globalPose_;
 	ros::Subscriber gpsFixAsyncSub_;
 	rtabmap::GPS gps_;
+	ros::Subscriber tagDetectionsSub_;
+	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
 
 	bool stereoToDepth_;
 	bool odomSensorSync_;

include/rtabmap_ros/MapsManager.h

@@ -90,8 +90,8 @@ private:
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;
-	bool negativePosesIgnored_;
-	bool negativeScanEmptyRayTracing_;
+	bool alwaysUpdateMap_;
+	bool scanEmptyRayTracing_;
 
 	ros::Publisher cloudMapPub_;
 	ros::Publisher cloudGroundPub_;

include/rtabmap_ros/MsgConversion.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf/transform_listener.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/Image.h>
@@ -150,6 +151,16 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
 void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
 
+rtabmap::Landmarks landmarksFromROS(
+		const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		tf::TransformListener & listener,
+		double waitForTransform,
+		double defaultLinVariance,
+		double defaultAngVariance);
+
 inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;}
 
 // common stuff

include/rtabmap_ros/OdometryROS.h

@@ -128,7 +128,7 @@ private:
 	bool visParams_;
 	bool icpParams_;
 	rtabmap::Transform guess_;
-	double guessStamp_;
+	rtabmap::Transform guessPreviousPose_;
 	double previousStamp_;
 	double expectedUpdateRate_;
 };