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

Added fiducial_msgs support (aruco_detect), just connect to /fiducial_transforms input of rtabmap node.

Browse Source
This commit is contained in:
matlabbe
2021-12-02 11:40:13 -05:00
parent 8a75963739
commit ec9b77d055
4 changed files with 49 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+24
View File
@@ -824,6 +824,9 @@ void CoreWrapper::onInit()
gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this);
#ifdef WITH_APRILTAG_ROS
tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
#endif
#ifdef WITH_FIDUCIAL_MSGS
fiducialTransfromsSub_ = nh.subscribe("fiducial_transforms", 1, &CoreWrapper::fiducialDetectionsAsyncCallback, this);
#endif
imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
@@ -2468,6 +2471,27 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
}
#endif
#ifdef WITH_FIDUCIAL_MSGS
void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::FiducialTransformArray & fiducialDetections)
{
if(!paused_)
{
for(unsigned int i=0; i<fiducialDetections.transforms.size(); ++i)
{
geometry_msgs::PoseWithCovarianceStamped p;
p.pose.pose.orientation = fiducialDetections.transforms[i].transform.rotation;
p.pose.pose.position.x = fiducialDetections.transforms[i].transform.translation.x;
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_,
std::make_pair(fiducialDetections.transforms[i].fiducial_id,
std::make_pair(p, 0.0f)));
}
}
}
#endif
void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
{
if(!paused_)