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

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_odom/include/rtabmap_odom/OdometryROS.h
+9 -1
View File
@@ -55,7 +55,7 @@ class Odometry;
namespace rtabmap_odom {
class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
class OdometryROS : public nodelet::Nodelet
{
public:
@@ -113,6 +113,7 @@ private:
bool waitForTransform_;
double waitForTransformDuration_;
bool publishNullWhenLost_;
bool publishCompressedSensorData_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
@@ -122,6 +123,9 @@ private:
ros::Publisher odomLocalScanMap_;
ros::Publisher odomLastFrame_;
ros::Publisher odomRgbdImagePub_;
ros::Publisher odomSensorDataPub_;
ros::Publisher odomSensorDataFeaturesPub_;
ros::Publisher odomSensorDataCompressedPub_;
ros::ServiceServer resetSrv_;
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
@@ -146,6 +150,8 @@ private:
double expectedUpdateRate_;
double maxUpdateRate_;
double minUpdateRate_;
std::string compressionImgFormat_;
bool compressionParallelized_;
int odomStrategy_;
bool waitIMUToinit_;
bool imuProcessed_;
@@ -162,8 +168,10 @@ private:
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
private:
bool lost_;
bool dataReceived_;
};
OdomStatusTask statusDiagnostic_;
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
};
}
rtabmap_odom/src/OdometryROS.cpp
+115 -8
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Compression.h>
#include "rtabmap_conversions/MsgConversion.h"
#include "rtabmap_msgs/OdomInfo.h"
#include "rtabmap/utilite/UConversion.h"
@@ -57,7 +58,6 @@ using namespace rtabmap;
namespace rtabmap_odom {
OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
rtabmap_sync::SyncDiagnostic(0.5),
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
@@ -71,6 +71,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
publishNullWhenLost_(true),
publishCompressedSensorData_(false),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -81,6 +82,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
minUpdateRate_(0.0),
compressionImgFormat_(".jpg"),
compressionParallelized_(true),
odomStrategy_(Parameters::defaultOdomStrategy()),
waitIMUToinit_(false),
imuProcessed_(false)
@@ -105,6 +108,9 @@ void OdometryROS::onInit()
odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
@@ -143,6 +149,9 @@ void OdometryROS::onInit()
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
int eventLevel = ULogger::kFatal;
@@ -168,6 +177,7 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: ground_truth_base_frame_id = %s", groundTruthBaseFrameId_.c_str());
NODELET_INFO("Odometry: config_path = %s", configPath.c_str());
NODELET_INFO("Odometry: publish_null_when_lost = %s", publishNullWhenLost_?"true":"false");
NODELET_INFO("Odometry: publish_compressed_sensor_data = %s", publishCompressedSensorData_?"true":"false");
NODELET_INFO("Odometry: guess_frame_id = %s", guessFrameId_.c_str());
NODELET_INFO("Odometry: guess_min_translation = %f", guessMinTranslation_);
NODELET_INFO("Odometry: guess_min_rotation = %f", guessMinRotation_);
@@ -176,6 +186,8 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_);
NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_);
NODELET_INFO("Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
NODELET_INFO("Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
NODELET_INFO("Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
@@ -373,9 +385,10 @@ void OdometryROS::onInit()
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(getNodeHandle(), getPrivateNodeHandle(), getName(), 0.5));
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
syncDiagnostic_->init(subscribedTopic,
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",
@@ -902,6 +915,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
postProcessData(data, header);
if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
{
if(!header.frame_id.empty())
@@ -917,10 +932,96 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
}
postProcessData(data, header);
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers())
{
rtabmap_msgs::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers());
msg.header.stamp = header.stamp; // use corresponding time stamp to image
if(odomSensorDataPub_.getNumSubscribers())
{
odomSensorDataPub_.publish(msg);
}
if(odomSensorDataFeaturesPub_.getNumSubscribers())
{
// remove data
msg.left = sensor_msgs::Image();
msg.right = sensor_msgs::Image();
msg.laser_scan = sensor_msgs::PointCloud2();
msg.grid_ground.clear();
msg.grid_obstacles.clear();
msg.grid_empty_cells.clear();
odomSensorDataFeaturesPub_.publish(msg);
}
}
if(odomSensorDataCompressedPub_.getNumSubscribers())
{
cv::Mat compressedImage;
cv::Mat compressedDepth;
cv::Mat compressedScan;
if(compressionParallelized_)
{
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
if(!data.imageRaw().empty())
{
ctImage.start();
}
if(!data.depthOrRightRaw().empty())
{
ctDepth.start();
}
if(!data.laserScanRaw().isEmpty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
}
else
{
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
compressedScan = compressData2(data.laserScanRaw().data());
}
if(!compressedImage.empty() && !data.stereoCameraModels().empty())
{
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
}
else if(!compressedImage.empty() && !data.cameraModels().empty())
{
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
}
if(!compressedScan.empty())
{
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
LaserScan(compressedScan,
data.laserScanRaw().maxPoints(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().format(),
data.laserScanRaw().localTransform()):
LaserScan(compressedScan,
data.laserScanRaw().format(),
data.laserScanRaw().rangeMin(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().angleMin(),
data.laserScanRaw().angleMax(),
data.laserScanRaw().angleIncrement(),
data.laserScanRaw().localTransform()), false);
}
rtabmap_msgs::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataCompressedPub_.publish(msg);
}
if(visParams_)
{
if(icpParams_)
@@ -938,10 +1039,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
if(syncDiagnostic_.get() && !pose.isNull())
{
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
tick(header.stamp,
syncDiagnostic_->tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
@@ -1034,17 +1135,23 @@ bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
lost_(false),
dataReceived_(false)
{}
void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
dataReceived_ = true;
lost_ = isLost;
}
void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
if(!dataReceived_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data received!");
}
else if(lost_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Lost!");
}