mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
Fixed #1186
This commit is contained in:
matlabbe
+109
-112
@@ -932,124 +932,121 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
|
||||
}
|
||||
}
|
||||
|
||||
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
|
||||
if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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);
|
||||
}
|
||||
odomSensorDataPub_.publish(msg);
|
||||
}
|
||||
if(odomSensorDataCompressedPub_.getNumSubscribers())
|
||||
if(odomSensorDataFeaturesPub_.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);
|
||||
// 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(visParams_)
|
||||
{
|
||||
if(icpParams_)
|
||||
{
|
||||
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull());
|
||||
if(syncDiagnostic_.get() && !pose.isNull())
|
||||
{
|
||||
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
|
||||
syncDiagnostic_->tick(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = header.stamp.toSec();
|
||||
}
|
||||
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_)
|
||||
{
|
||||
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull());
|
||||
if(syncDiagnostic_.get())
|
||||
{
|
||||
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
|
||||
syncDiagnostic_->tick(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = header.stamp.toSec();
|
||||
}
|
||||
|
||||
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
|
||||
Reference in New Issue
Block a user