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

Add rtabmap_msgs/SensorData (#1055)

Browse Source 
* Add rtabmap_msgs/SensorData

* After testing fixes

* Updated output topic name

* added explicit --logconsole for nodelets

* fixed intermediate nodes not generated

* Refactored SyncDiagnostic usage to handle nodelet name

* SyncDiagnostic: added TimeStampStatus. Odom: added new status when data not received yet

* rtabmap: Fixed parameters not updated when using nodelet
This commit is contained in:
matlabbe
2023-11-19 13:16:35 -08:00
committed by GitHub
parent 3801006832
commit 67de27b1ee
30 changed files with 1318 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+32 -2
View File
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/UserData.h>
#include <rtabmap_msgs/OdomInfo.h>
#include <rtabmap_msgs/ScanDescriptor.h>
#include <rtabmap_msgs/SensorData.h>
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
@@ -57,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
class CommonDataSubscriber : public SyncDiagnostic {
class CommonDataSubscriber {
public:
CommonDataSubscriber(bool gui);
virtual ~CommonDataSubscriber();
@@ -69,8 +70,9 @@ public:
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom();}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;}
bool isApproxSync() const {return approxSync_;}
@@ -107,6 +109,10 @@ protected:
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg) = 0;
virtual void commonSensorDataCallback(
const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg) = 0;
void commonSingleCameraCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -123,6 +129,8 @@ protected:
const std::vector<rtabmap_msgs::Point3f> & localPoints3d = std::vector<rtabmap_msgs::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
void tick(const ros::Time & stamp, double targetFrequency = 0);
private:
void setupDepthCallbacks(
ros::NodeHandle & nh,
@@ -232,6 +240,13 @@ private:
int queueSize,
bool approxSync);
#endif
void setupSensorDataCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
void setupScanCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
@@ -261,6 +276,7 @@ private:
bool subscribedToRGB_;
bool subscribedToOdom_;
bool subscribedToRGBD_;
bool subscribedToSensorData_;
bool subscribedToScan2d_;
bool subscribedToScan3d_;
bool subscribedToScanDescriptor_;
@@ -278,6 +294,10 @@ private:
ros::Subscriber rgbdXSubOnly_;
message_filters::Subscriber<rtabmap_msgs::RGBDImages> rgbdXSub_;
//for sensor data callback
ros::Subscriber sensorDataSubOnly_;
message_filters::Subscriber<rtabmap_msgs::SensorData> sensorDataSub_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
@@ -296,6 +316,8 @@ private:
ros::Subscriber scanDescSubOnly_;
ros::Subscriber odomSubOnly_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
// RGB + Depth
DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
@@ -448,6 +470,14 @@ private:
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::Odometry, rtabmap_msgs::UserData, rtabmap_msgs::RGBDImages, rtabmap_msgs::OdomInfo);
#endif
// SensorData
void sensorDataCallback(const rtabmap_msgs::SensorDataConstPtr&);
DATA_SYNCS2(sensorDataInfo, rtabmap_msgs::SensorData, rtabmap_msgs::OdomInfo);
// SensorData + Odom
DATA_SYNCS2(sensorDataOdom, nav_msgs::Odometry, rtabmap_msgs::SensorData);
DATA_SYNCS3(sensorDataOdomInfo, nav_msgs::Odometry, rtabmap_msgs::SensorData, rtabmap_msgs::OdomInfo);
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+11 -4
View File
@@ -12,8 +12,11 @@ namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(double tolerance = 0.1, int windowSize = 5) :
SyncDiagnostic(ros::NodeHandle h = ros::NodeHandle(), ros::NodeHandle ph = ros::NodeHandle("~"), std::string nodeName = ros::this_node::getName(), double tolerance = 0.1, int windowSize = 5) :
diagnosticUpdater_(h, ph, nodeName),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
timeStampStatus_(diagnostic_updater::TimeStampStatusParam()),
compositeTask_("Sync status"),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0),
windowSize_(windowSize)
@@ -21,8 +24,7 @@ class SyncDiagnostic {
UASSERT(windowSize_ >= 1);
}
protected:
void initDiagnostic(
void init(
const std::string & topic,
const std::string & topicsNotReceivedWarningMsg,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
@@ -35,7 +37,9 @@ protected:
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
compositeTask_.addTask(&frequencyStatus_);
compositeTask_.addTask(&timeStampStatus_);
diagnosticUpdater_.add(compositeTask_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
@@ -48,6 +52,7 @@ protected:
void tick(const ros::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
timeStampStatus_.tick(stamp);
double singlePeriod = stamp.toSec() - lastCallbackCalledStamp_;
window_.push_back(singlePeriod);
@@ -91,6 +96,8 @@ private:
std::string topicsNotReceivedWarningMsg_;
diagnostic_updater::Updater diagnosticUpdater_;
diagnostic_updater::FrequencyStatus frequencyStatus_;
diagnostic_updater::TimeStampStatus timeStampStatus_;
diagnostic_updater::CompositeDiagnosticTask compositeTask_;
ros::Timer diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;