i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17:45 +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/src/nodelets/rgb_sync.cpp
+6 -3
View File
@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RgbSync : public nodelet::Nodelet, public SyncDiagnostic
class RgbSync : public nodelet::Nodelet
{
public:
RgbSync() :
@@ -123,7 +123,8 @@ private:
cameraInfoSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(rgb_nh.resolveName("image_rect"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(rgb_nh.resolveName("image_rect"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -137,7 +138,7 @@ private:
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->tick(image->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -205,6 +206,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RgbSync, nodelet::Nodelet);
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+6 -3
View File
@@ -58,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDSync : public nodelet::Nodelet, public SyncDiagnostic
class RGBDSync : public nodelet::Nodelet
{
public:
RGBDSync() :
@@ -142,7 +142,8 @@ private:
cameraInfoSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(rgb_nh.resolveName("image"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(rgb_nh.resolveName("image"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -157,7 +158,7 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
tick(image->header.stamp);
syncDiagnostic_->tick(image->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -304,6 +305,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RGBDSync, nodelet::Nodelet);
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+13 -9
View File
@@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDXSync : public nodelet::Nodelet, public SyncDiagnostic
class RGBDXSync : public nodelet::Nodelet
{
public:
RGBDXSync() :
@@ -167,7 +167,8 @@ private:
NODELET_INFO(subscribedTopicsMsg.c_str());
// Setup diagnostic
initDiagnostic("",
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init("",
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -189,13 +190,16 @@ private:
ros::Publisher rgbdImagesPub_;
std::vector<message_filters::Subscriber<rtabmap_msgs::RGBDImage>*> rgbdSubs_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
void RGBDXSync::rgbd2Callback(
const rtabmap_msgs::RGBDImageConstPtr& image0,
const rtabmap_msgs::RGBDImageConstPtr& image1)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
@@ -209,7 +213,7 @@ void RGBDXSync::rgbd3Callback(
const rtabmap_msgs::RGBDImageConstPtr& image1,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
@@ -225,7 +229,7 @@ void RGBDXSync::rgbd4Callback(
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
@@ -243,7 +247,7 @@ void RGBDXSync::rgbd5Callback(
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
@@ -263,7 +267,7 @@ void RGBDXSync::rgbd6Callback(
const rtabmap_msgs::RGBDImageConstPtr& image4,
const rtabmap_msgs::RGBDImageConstPtr& image5)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
@@ -285,7 +289,7 @@ void RGBDXSync::rgbd7Callback(
const rtabmap_msgs::RGBDImageConstPtr& image5,
const rtabmap_msgs::RGBDImageConstPtr& image6)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
@@ -309,7 +313,7 @@ void RGBDXSync::rgbd8Callback(
const rtabmap_msgs::RGBDImageConstPtr& image6,
const rtabmap_msgs::RGBDImageConstPtr& image7)
{
tick(image0->header.stamp);
syncDiagnostic_->tick(image0->header.stamp);
rtabmap_msgs::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(8);
rtabmap_sync/src/nodelets/stereo_sync.cpp
+6 -3
View File
@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class StereoSync : public nodelet::Nodelet, public SyncDiagnostic
class StereoSync : public nodelet::Nodelet
{
public:
StereoSync() :
@@ -131,7 +131,8 @@ private:
cameraInfoRightSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
initDiagnostic(left_nh.resolveName("image_rect"),
syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, getName()));
syncDiagnostic_->init(left_nh.resolveName("image_rect"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
@@ -148,7 +149,7 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
tick(imageLeft->header.stamp);
syncDiagnostic_->tick(imageLeft->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
@@ -240,6 +241,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_sync::StereoSync, nodelet::Nodelet);