diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index df0eeb6f..f53b7aaa 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -38,6 +38,10 @@ find_package(rtabmap_sync REQUIRED) #optional find_package(apriltag_msgs) +find_package(aruco_msgs) +find_package(aruco_markers_msgs) +find_package(aruco_opencv_msgs) +find_package(ros2_aruco_interfaces) find_package(nav2_msgs) IF(WIN32) @@ -88,6 +92,46 @@ SET(Libraries ) ENDIF(apriltag_msgs_FOUND) +# If aruco_msgs is found, add definition +IF(aruco_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MSGS") +SET(Libraries + ${Libraries} + aruco_msgs +) +ENDIF(aruco_msgs_FOUND) + +# If aruco_opencv_msgs is found, add definition +IF(aruco_opencv_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_opencv_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS") +SET(Libraries + ${Libraries} + aruco_opencv_msgs +) +ENDIF(aruco_opencv_msgs_FOUND) + +# If aruco_markers_msgs is found, add definition +IF(aruco_markers_msgs_FOUND) +MESSAGE(STATUS "WITH aruco_markers_msgs") +ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS") +SET(Libraries + ${Libraries} + aruco_markers_msgs +) +ENDIF(aruco_markers_msgs_FOUND) + +# If ros2_aruco_interfaces is found, add definition +IF(ros2_aruco_interfaces_FOUND) +MESSAGE(STATUS "WITH ros2_aruco_interfaces") +ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES") +SET(Libraries + ${Libraries} + ros2_aruco_interfaces +) +ENDIF(ros2_aruco_interfaces_FOUND) + # If nav2_msgs is found, add definition IF(nav2_msgs_FOUND) MESSAGE(STATUS "WITH nav2_msgs") diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index a0eb6e73..157a9b26 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -88,6 +88,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef WITH_ARUCO_MSGS +#include +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +#include +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +#include +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +#include +#endif + #ifdef WITH_NAV2_MSGS #include #include @@ -178,7 +194,20 @@ private: 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); + void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); + void apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MSGS + void arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + void arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + void arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + void arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg); #endif #ifdef WITH_FIDUCIAL_MSGS void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections); @@ -420,6 +449,19 @@ private: rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; + rclcpp::Subscription::SharedPtr apriltagSub_; +#endif +#ifdef WITH_ARUCO_MSGS + rclcpp::Subscription::SharedPtr arucoSub_; +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + rclcpp::Subscription::SharedPtr arucoOpencvSub_; +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + rclcpp::Subscription::SharedPtr arucoMarkersSub_; +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + rclcpp::Subscription::SharedPtr arucoInterfacesSub_; #endif #ifdef WITH_FIDUCIAL_MSGS rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 38d38b64..08543237 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -27,6 +27,9 @@ tf2_ros visualization_msgs apriltag_msgs + aruco_msgs + aruco_opencv_msgs + rtabmap_msgs rtabmap_util diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index bb86ad56..c6a20333 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -886,6 +886,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + apriltagSub_ = this->create_subscription("apriltag/detections", 5, std::bind(&CoreWrapper::apriltagAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MSGS + arucoSub_ = this->create_subscription("aruco/detections", 5, std::bind(&CoreWrapper::arucoAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + arucoOpencvSub_ = this->create_subscription("aruco_opencv/detections", 5, std::bind(&CoreWrapper::arucoOpencvAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + arucoMarkersSub_ = this->create_subscription("aruco_markers/detections", 5, std::bind(&CoreWrapper::arucoMarkersAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + arucoInterfacesSub_ = this->create_subscription("aruco_interfaces/detections", 5, std::bind(&CoreWrapper::arucoInterfacesAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); @@ -2651,18 +2664,30 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm } #ifdef WITH_APRILTAG_MSGS -void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections) +void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) +{ + if(!paused_) + { + static bool warningShow = false; + if(!warningShow) { + RCLCPP_WARN(this->get_logger(), "\"tag_detections\" input topic name for apriltag_msgs is deprecated, remap \"apriltag\" input topic name instead. This message is only printed once."); + warningShow = true; + } + apriltagAsyncCallback(msg); + } +} +void CoreWrapper::apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg) { if(!paused_) { UScopeMutex lock(landmarksMutex_); - for(unsigned int i=0; idetections.size(); ++i) + for(unsigned int i=0; idetections.size(); ++i) { - std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id); + std::string tagFrameId = msg->detections[i].family+":"+uNumber2Str(msg->detections[i].id); Transform camToTag = rtabmap_conversions::getTransform( - tagDetections->header.frame_id, // e.g., camera_optical_frame + msg->header.frame_id, // e.g., camera_optical_frame tagFrameId, // e.g., tag36h11:42 - tagDetections->header.stamp, + msg->header.stamp, *tfBuffer_, waitForTransform_); if(camToTag.isNull()) @@ -2670,16 +2695,97 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.", frameId_.c_str(), tagFrameId.c_str(), - tagDetections->detections[i].id); + msg->detections[i].id); continue; } geometry_msgs::msg::PoseWithCovarianceStamped p; rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose); - p.header = tagDetections->header; + p.header = msg->header; uInsert(landmarks_, - std::make_pair(tagDetections->detections[i].id, + std::make_pair(msg->detections[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MSGS +void CoreWrapper::arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose = msg->markers[i].pose; + p.header = msg->markers[i].header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +void CoreWrapper::arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].marker_id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +void CoreWrapper::arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + for(unsigned int i=0; imarkers.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->markers[i].pose.pose; + p.header = msg->markers[i].pose.header; + + uInsert(landmarks_, + std::make_pair((int)msg->markers[i].id, + std::make_pair(p, 0.0f))); + } + } +} +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +void CoreWrapper::arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg) +{ + if(!paused_) + { + UScopeMutex lock(landmarksMutex_); + UASSERT(msg->marker_ids.size() == msg->poses.size()); + for(unsigned int i=0; imarker_ids.size(); ++i) + { + geometry_msgs::msg::PoseWithCovarianceStamped p; + p.pose.pose = msg->poses[i]; + p.header = msg->header; + + uInsert(landmarks_, + std::make_pair((int)msg->marker_ids[i], std::make_pair(p, 0.0f))); } }