i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-16 08:10:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added LandmarkDetection msg for convenience

Browse Source
This commit is contained in:
matlabbe
2023-05-11 19:58:59 -07:00
parent 96e8f0e223
commit ff43fcbde2
5 changed files with 75 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_msgs/CMakeLists.txt
+2
View File
@@ -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"
rtabmap_msgs/msg/LandmarkDetection.msg
+21
View File
@@ -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
rtabmap_msgs/msg/LandmarkDetections.msg
+7
View File
@@ -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
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+7 -1
View File
@@ -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<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
rtabmap::GPS gps_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
#ifdef WITH_APRILTAG_MSGS
rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr tagDetectionsSub_;
#endif
#ifdef WITH_FIDUCIAL_MSGS
rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;
#endif
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > landmarks_; // id, <pose, size>
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
std::map<double, rtabmap::Transform> imus_;
rtabmap_slam/src/CoreWrapper.cpp
+38 -7
View File
@@ -808,6 +808,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
userDataAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::UserData>("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<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1));
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));
landmarkDetectionSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 5, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1));
landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 5, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1));
#ifdef WITH_APRILTAG_MSGS
tagDetectionsSub_ = this->create_subscription<apriltag_msgs::msg::AprilTagDetectionArray>("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; i<landmarkDetections->landmarks.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");