diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt index 17c8cffd..bbe3dfeb 100644 --- a/rtabmap_msgs/CMakeLists.txt +++ b/rtabmap_msgs/CMakeLists.txt @@ -25,6 +25,8 @@ set(msg_files "msg/MapGraph.msg" "msg/NodeData.msg" "msg/Link.msg" + "msg/LandmarkDetection.msg" + "msg/LandmarkDetections.msg" "msg/OdomInfo.msg" "msg/Point2f.msg" "msg/Point3f.msg" diff --git a/rtabmap_msgs/msg/LandmarkDetection.msg b/rtabmap_msgs/msg/LandmarkDetection.msg new file mode 100644 index 00000000..8f676c07 --- /dev/null +++ b/rtabmap_msgs/msg/LandmarkDetection.msg @@ -0,0 +1,21 @@ + +# header.stamp: the timestamp of the detection (e.g. image timestamp) +# header.frame_id: the base frame of pose (e.g., camera optical frame) +std_msgs/Header header + +# Landmark's frame id +string landmark_frame_id + +# Landmark's unique ID: should be >0 +int32 id + +# Size in meters of the landmark/tag (optional, set 0 to not use it). +float32 size + +# Pose of the landmark in header.frame_id frame. +# If covariance is unknown, keep it as null matrix. +# rtabmap_slam/rtabmap's landmark_angular_variance and +# landmark_linear_variance parameters can be used +# for convenience if covariance is null. +geometry_msgs/PoseWithCovariance pose + diff --git a/rtabmap_msgs/msg/LandmarkDetections.msg b/rtabmap_msgs/msg/LandmarkDetections.msg new file mode 100644 index 00000000..332d3afb --- /dev/null +++ b/rtabmap_msgs/msg/LandmarkDetections.msg @@ -0,0 +1,7 @@ + +# header.stamp: the timestamp of the detection (e.g. image timestamp) +# header.frame_id: the base frame of pose (e.g., camera optical frame) +std_msgs/Header header + +LandmarkDetection[] landmarks + diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 83b50917..500f0fb2 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -67,6 +67,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_msgs/msg/odom_info.hpp" #include "rtabmap_msgs/msg/info.hpp" +#include "rtabmap_msgs/msg/landmark_detection.hpp" +#include "rtabmap_msgs/msg/landmark_detections.hpp" #include "rtabmap_msgs/srv/get_nodes_in_radius.hpp" #include "rtabmap_msgs/srv/load_database.hpp" #include "rtabmap_msgs/srv/detect_more_loop_closures.hpp" @@ -159,6 +161,8 @@ private: void userDataAsyncCallback(const rtabmap_msgs::msg::UserData::SharedPtr dataMsg); void globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr globalPoseMsg); void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg); + void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection); + void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections); #ifdef WITH_APRILTAG_MSGS void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections); #endif @@ -379,13 +383,15 @@ private: geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; rtabmap::GPS gps_; + rclcpp::Subscription::SharedPtr landmarkDetectionSub_; + rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; #endif #ifdef WITH_FIDUCIAL_MSGS rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; #endif - std::map > tags_; // id, + std::map > landmarks_; // id, rclcpp::Subscription::SharedPtr imuSub_; std::map imus_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 88ebfb2d..bc144a55 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -808,6 +808,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1)); globalPoseAsyncSub_ = this->create_subscription("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1)); gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1)); + landmarkDetectionSub_ = this->create_subscription("landmark_detection", 5, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1)); + landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 5, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1)); #ifdef WITH_APRILTAG_MSGS tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1)); #endif @@ -1886,7 +1888,7 @@ void CoreWrapper::process( //tag detections Landmarks landmarks = rtabmap_conversions::landmarksFromROS( - tags_, + landmarks_, frameId_, odomFrameId, lastPoseStamp_, @@ -1894,7 +1896,7 @@ void CoreWrapper::process( waitForTransform_, landmarkDefaultLinVariance_, landmarkDefaultAngVariance_); - tags_.clear(); + landmarks_.clear(); if(!landmarks.empty()) { data.setLandmarks(landmarks); @@ -2286,6 +2288,35 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP } } +void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection) +{ + if(!paused_) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.header = landmarkDetection->header; + p.pose = landmarkDetection->pose; + uInsert(landmarks_, + std::make_pair(landmarkDetection->id, + std::make_pair(p, landmarkDetection->size))); + } +} + +void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections) +{ + if(!paused_) + { + for(unsigned int i=0; ilandmarks.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.header = landmarkDetections->landmarks[i].header; + p.pose = landmarkDetections->landmarks[i].pose; + uInsert(landmarks_, + std::make_pair(landmarkDetections->landmarks[i].id, + std::make_pair(p, landmarkDetections->landmarks[i].size))); + } + } +} + #ifdef WITH_APRILTAG_MSGS void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections) { @@ -2313,7 +2344,7 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose); p.header = tagDetections->header; - uInsert(tags_, + uInsert(landmarks_, std::make_pair(tagDetections->detections[i].id, std::make_pair(p, 0.0f))); } @@ -2334,7 +2365,7 @@ void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::Fidu 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_, + uInsert(landmarks_, std::make_pair(fiducialDetections.transforms[i].fiducial_id, std::make_pair(p, 0.0f))); } @@ -2675,7 +2706,7 @@ void CoreWrapper::resetRtabmapCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); - tags_.clear(); + landmarks_.clear(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); @@ -2769,7 +2800,7 @@ void CoreWrapper::loadDatabaseCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); - tags_.clear(); + landmarks_.clear(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); @@ -2896,7 +2927,7 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.unlock(); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); - tags_.clear(); + landmarks_.clear(); RCLCPP_INFO(this->get_logger(), "Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back");