From 07951eee0882cac4774fbb1daf557add41a95f40 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 20 Dec 2021 17:03:13 -0500 Subject: [PATCH 1/4] merged master -> ros2 --- CMakeLists.txt | 14 ++++ include/rtabmap_ros/CoreWrapper.h | 13 ++++ launch/demo/demo_husky.launch | 2 +- .../demo/demo_isaac_carter_navigation.launch | 76 +++++++++++++++++++ launch/ros2/rtabmap.launch.py | 2 + launch/rtabmap.launch | 2 + msg/Info.msg | 3 + src/CoreWrapper.cpp | 64 +++++++++++++++- src/MsgConversion.cpp | 8 ++ 9 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 launch/demo/demo_isaac_carter_navigation.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 69564064..51de3a7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,7 @@ find_package(octomap_msgs) #find_package(apriltag_msgs) #find_package(find_object_2d) find_package(move_base_msgs) +#find_package(fiducial_msgs) ## System dependencies are found with CMake's conventions find_package(RTABMap 0.20.15 REQUIRED) @@ -321,6 +322,19 @@ SET(Libraries ADD_DEFINITIONS("-DWITH_MOVE_BASE_MSGS") ENDIF(move_base_msgs_FOUND) +# If fiducial_msgs is found, add definition +IF(fiducial_msgs_FOUND) +MESSAGE(STATUS "WITH fiducial_msgs") +include_directories( + ${fiducial_msgs_INCLUDE_DIRS} +) +SET(Libraries + ${fiducial_msgs_LIBRARIES} + ${Libraries} +) +ADD_DEFINITIONS("-DWITH_FIDUCIAL_MSGS") +ENDIF(fiducial_msgs_FOUND) + ############################ ## Declare a cpp library ############################ diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index d948017f..ec738f08 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -88,6 +88,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +//#define WITH_FIDUCIAL_MSGS +#ifdef WITH_FIDUCIAL_MSGS +#include +#endif + namespace rtabmap { class StereoDense; } @@ -164,6 +169,9 @@ private: void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg); #ifdef WITH_APRILTAG_MSGS void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections); +#endif +#ifdef WITH_FIDUCIAL_MSGS + void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections); #endif void imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg); void republishNodeDataCallback(const std_msgs::msg::Int32MultiArray::ConstSharedPtr msg); @@ -294,6 +302,7 @@ private: rclcpp::Publisher::SharedPtr infoPub_; rclcpp::Publisher::SharedPtr mapDataPub_; rclcpp::Publisher::SharedPtr mapGraphPub_; + rclcpp::Publisher::SharedPtr odomCachePub_; rclcpp::Publisher::SharedPtr landmarksPub_; rclcpp::Publisher::SharedPtr labelsPub_; rclcpp::Publisher::SharedPtr mapPathPub_; @@ -375,9 +384,13 @@ private: rtabmap::GPS gps_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; +#endif +#ifdef WITH_FIDUCIAL_MSGS + rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; #endif std::map > tags_; // id, rclcpp::Subscription::SharedPtr imuSub_; + std::map imus_; std::string imuFrameId_; rclcpp::Subscription::SharedPtr republishNodeDataSub_; diff --git a/launch/demo/demo_husky.launch b/launch/demo/demo_husky.launch index 012603fe..ec0e01b9 100644 --- a/launch/demo/demo_husky.launch +++ b/launch/demo/demo_husky.launch @@ -1,4 +1,4 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ros2/rtabmap.launch.py b/launch/ros2/rtabmap.launch.py index 9447ba38..c0141bf6 100644 --- a/launch/ros2/rtabmap.launch.py +++ b/launch/ros2/rtabmap.launch.py @@ -296,6 +296,7 @@ def launch_setup(context, *args, **kwargs): ("user_data_async", LaunchConfiguration('user_data_async_topic')), ("gps/fix", LaunchConfiguration('gps_topic')), ("tag_detections", LaunchConfiguration('tag_topic')), + ("fiducial_transforms", LaunchConfiguration('fiducial_topic')), ("odom", LaunchConfiguration('odom_topic')), ("imu", LaunchConfiguration('imu_topic'))], arguments=[LaunchConfiguration("args")], @@ -465,6 +466,7 @@ def generate_launch_description(): DeclareLaunchArgument('tag_topic', default_value='/tag_detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map.'), DeclareLaunchArgument('tag_linear_variance', default_value='0.0001', description=''), DeclareLaunchArgument('tag_angular_variance', default_value='9999.0', description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'), + DeclareLaunchArgument('fiducial_topic', default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'), OpaqueFunction(function=launch_setup) ]) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 69db25ba..f8bb245e 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -142,6 +142,7 @@ + @@ -382,6 +383,7 @@ + diff --git a/msg/Info.msg b/msg/Info.msg index fbfaf6e5..0bc40fc7 100644 --- a/msg/Info.msg +++ b/msg/Info.msg @@ -45,3 +45,6 @@ float32[] stats_values # std::vector local_path int32[] local_path int32 current_goal_id + +# std::vector odomCache +MapGraph odom_cache diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 48b12da3..f91e3ba2 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -245,6 +245,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : infoPub_ = this->create_publisher("info", 1); mapDataPub_ = this->create_publisher("mapData", 1); mapGraphPub_ = this->create_publisher("mapGraph", 1); + odomCachePub_ = this->create_publisher("mapOdomCache", 1); landmarksPub_ = this->create_publisher("landmarks", 1); labelsPub_ = this->create_publisher("labels", 1); mapPathPub_ = this->create_publisher("mapPath", 1); @@ -792,6 +793,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : gpsFixAsyncSub_ = this->create_subscription("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("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1)); +#endif +#ifdef WITH_FIDUCIAL_MSGS + fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1)); #endif imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1)); republishNodeDataSub_ = this->create_subscription("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; ipublish(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 poses = stats.odomCachePoses(); + // transform in map frame + for(std::map::iterator iter=poses.begin(); + iter!=poses.end(); + ++iter) + { + iter->second = stats.mapCorrection() * iter->second; + } + for(std::multimap::const_iterator iter=stats.odomCacheConstraints().begin(); + iter!=stats.odomCacheConstraints().end(); + ++iter) + { + std::map::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())); diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 6a58af1f..f1c682d5 100644 --- a/src/MsgConversion.cpp +++ b/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 poses; + std::multimap 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 Date: Fri, 24 Dec 2021 17:22:08 -0500 Subject: [PATCH 2/4] NodeData: fixed wrong word indexes (causing visualization issues of corresponding features in rtabmapviz) --- msg/NodeData.msg | 8 ++-- src/MsgConversion.cpp | 106 ++++++++++++++++++++++++------------------ 2 files changed, 67 insertions(+), 47 deletions(-) diff --git a/msg/NodeData.msg b/msg/NodeData.msg index 9a1123fe..e6833078 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -54,9 +54,11 @@ uint8[] grid_empty_cells float32 grid_cell_size Point3f grid_view_point -# std::multimap -# std::multimap -int32[] wordIds +# std::multimap +# std::vector +# std::vector +int32[] wordIdKeys +int32[] wordIdValues KeyPoint[] wordKpts Point3f[] wordPts # compressed descriptors diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 62d32de7..68b944e9 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -1067,31 +1067,45 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) std::vector words3D; cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.wordDescriptors); - if(!msg.wordKpts.empty() && msg.wordKpts.size() != msg.wordIds.size()) + if(msg.wordIdKeys.size() != msg.wordIdValues.size()) { - ROS_ERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.wordIds.size(), (int)msg.wordKpts.size()); + ROS_ERROR("Word ID keys and values should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordIdValues.size()); } - if(!msg.wordPts.empty() && msg.wordPts.size() != msg.wordIds.size()) + if(!msg.wordKpts.empty() && msg.wordKpts.size() != msg.wordIdKeys.size()) { - ROS_ERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.wordIds.size(), (int)msg.wordPts.size()); + ROS_ERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordKpts.size()); } - if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.wordIds.size()) + if(!msg.wordPts.empty() && msg.wordPts.size() != msg.wordIdKeys.size()) { - ROS_ERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.wordIds.size(), wordsDescriptors.rows); + ROS_ERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), (int)msg.wordPts.size()); + } + if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.wordIdKeys.size()) + { + ROS_ERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.wordIdKeys.size(), wordsDescriptors.rows); wordsDescriptors = cv::Mat(); } - for(unsigned int i=0; i::const_iterator iter=signature.getWords().begin(); + iter!=signature.getWords().end(); + ++iter) { - for(size_t i=0; ifirst; + msg.wordIdValues.at(i) = iter->second; + if(signature.getWordsKpts().size() == signature.getWords().size()) { - if(!msg.wordKpts.empty()) - keypointToROS(signature.getWordsKpts().at(i), msg.wordKpts.at(i)); - if(!msg.wordPts.empty()) - point3fToROS(signature.getWords3().at(i), msg.wordPts[i]); + if(msg.wordKpts.empty()) + { + msg.wordKpts.resize(signature.getWords().size()); + } + keypointToROS(signature.getWordsKpts().at(i), msg.wordKpts.at(i)); } + if(signature.getWords3().size() == signature.getWords().size()) + { + if(msg.wordPts.empty()) + { + msg.wordPts.resize(signature.getWords().size()); + } + point3fToROS(signature.getWords3().at(i), msg.wordPts.at(i)); + } + ++i; } if(!signature.getWordsDescriptors().empty()) From df7ee6ee8d06e7a6d64ce249a29c646c50540071 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 26 Dec 2021 14:54:28 -0500 Subject: [PATCH 3/4] Merged master->ros2 --- msg/NodeData.msg | 8 ++-- src/MsgConversion.cpp | 106 ++++++++++++++++++++++++------------------ 2 files changed, 67 insertions(+), 47 deletions(-) diff --git a/msg/NodeData.msg b/msg/NodeData.msg index 51b3694c..a19d0125 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -54,9 +54,11 @@ uint8[] grid_empty_cells float32 grid_cell_size Point3f grid_view_point -# std::multimap -# std::multimap -int32[] word_ids +# std::multimap +# std::vector +# std::vector +int32[] word_id_keys +int32[] word_id_values KeyPoint[] word_kpts Point3f[] word_pts # compressed descriptors diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index f1c682d5..3d9ae7b2 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -1074,31 +1074,45 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg) cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors); - if(!msg.word_kpts.empty() && msg.word_kpts.size() != msg.word_ids.size()) + if(msg.word_id_keys.size() != msg.word_id_values.size()) { - UERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_kpts.size()); + UERROR("Word ID keys and values should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_id_values.size()); } - if(!msg.word_pts.empty() && msg.word_pts.size() != msg.word_ids.size()) + if(!msg.word_kpts.empty() && msg.word_kpts.size() != msg.word_id_keys.size()) { - UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_pts.size()); + UERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_kpts.size()); } - if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_ids.size()) + if(!msg.word_pts.empty() && msg.word_pts.size() != msg.word_id_keys.size()) { - UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_ids.size(), wordsDescriptors.rows); + UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_pts.size()); + } + if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_id_keys.size()) + { + UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), wordsDescriptors.rows); wordsDescriptors = cv::Mat(); } - for(size_t i=0; i::const_iterator iter=signature.getWords().begin(); + iter!=signature.getWords().end(); + ++iter) { - for(size_t i=0; ifirst; + msg.word_id_values.at(i) = iter->second; + if(signature.getWordsKpts().size() == signature.getWords().size()) { - if(!msg.word_kpts.empty()) - keypointToROS(signature.getWordsKpts().at(i), msg.word_kpts.at(i)); - if(!msg.word_pts.empty()) - point3fToROS(signature.getWords3().at(i), msg.word_pts[i]); + if(msg.word_kpts.empty()) + { + msg.word_kpts.resize(signature.getWords().size()); + } + keypointToROS(signature.getWordsKpts().at(i), msg.word_kpts.at(i)); } + if(signature.getWords3().size() == signature.getWords().size()) + { + if(msg.word_pts.empty()) + { + msg.word_pts.resize(signature.getWords().size()); + } + point3fToROS(signature.getWords3().at(i), msg.word_pts.at(i)); + } + ++i; } if(!signature.getWordsDescriptors().empty()) From 1c7d3ccca2d950fe700eaa6c9e04492ae56b6a88 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 26 Dec 2021 15:00:16 -0500 Subject: [PATCH 4/4] Updated version to 0.20.16 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 43c85026..a9342caf 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.20.15 + 0.20.16 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe