i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27: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
CMakeLists.txt
+14
View File
@@ -28,6 +28,7 @@ find_package(octomap_msgs)
find_package(apriltag_ros)
find_package(rviz)
find_package(find_object_2d)
find_package(fiducial_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -293,6 +294,19 @@ SET(Libraries
ADD_DEFINITIONS("-DWITH_APRILTAG_ROS")
ENDIF(apriltag_ros_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
############################
include/rtabmap_ros/CoreWrapper.h
+9
View File
@@ -78,6 +78,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <apriltag_ros/AprilTagDetectionArray.h>
#endif
//#define WITH_FIDUCIAL_MSGS
#ifdef WITH_FIDUCIAL_MSGS
#include <fiducial_msgs/FiducialTransformArray.h>
#endif
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
@@ -164,6 +169,9 @@ private:
void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg);
#ifdef WITH_APRILTAG_ROS
void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
#endif
#ifdef WITH_FIDUCIAL_MSGS
void fiducialDetectionsAsyncCallback(const fiducial_msgs::FiducialTransformArray & fiducialDetections);
#endif
void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg);
@@ -368,6 +376,7 @@ private:
ros::Subscriber gpsFixAsyncSub_;
rtabmap::GPS gps_;
ros::Subscriber tagDetectionsSub_;
ros::Subscriber fiducialTransfromsSub_;
std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_;
launch/rtabmap.launch
+2
View File
@@ -142,6 +142,7 @@
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
<arg name="tag_linear_variance" default="0.0001" />
<arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
<arg name="fiducial_topic" default="/fiducial_transforms" /> <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
@@ -382,6 +383,7 @@
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap from="gps/fix" to="$(arg gps_topic)"/>
<remap from="tag_detections" to="$(arg tag_topic)"/>
<remap from="fiducial_transforms" to="$(arg fiducial_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
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_)