mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
merged master -> ros2
This commit is contained in:
matlabbe
+109
-112
@@ -929,124 +929,121 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
|
||||
}
|
||||
}
|
||||
|
||||
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
|
||||
if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
if(odomSensorDataPub_->get_subscription_count()>0)
|
||||
{
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
if(odomSensorDataPub_->get_subscription_count()>0)
|
||||
{
|
||||
odomSensorDataPub_->publish(msg);
|
||||
}
|
||||
if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
// remove data
|
||||
msg.left = sensor_msgs::msg::Image();
|
||||
msg.right = sensor_msgs::msg::Image();
|
||||
msg.laser_scan = sensor_msgs::msg::PointCloud2();
|
||||
msg.grid_ground.clear();
|
||||
msg.grid_obstacles.clear();
|
||||
msg.grid_empty_cells.clear();
|
||||
odomSensorDataFeaturesPub_->publish(msg);
|
||||
}
|
||||
odomSensorDataPub_->publish(msg);
|
||||
}
|
||||
if(odomSensorDataCompressedPub_->get_subscription_count()>0)
|
||||
if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
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::msg::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::msg::Image();
|
||||
msg.right = sensor_msgs::msg::Image();
|
||||
msg.laser_scan = sensor_msgs::msg::PointCloud2();
|
||||
msg.grid_ground.clear();
|
||||
msg.grid_obstacles.clear();
|
||||
msg.grid_empty_cells.clear();
|
||||
odomSensorDataFeaturesPub_->publish(msg);
|
||||
}
|
||||
|
||||
if(visParams_)
|
||||
{
|
||||
if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull());
|
||||
if(syncDiagnostic_.get() && !pose.isNull())
|
||||
{
|
||||
double curentRate = 1.0/(this->now()-timeStart).seconds();
|
||||
syncDiagnostic_->tick(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
|
||||
}
|
||||
if(odomSensorDataCompressedPub_->get_subscription_count()>0)
|
||||
{
|
||||
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::msg::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_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "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)), (now()-timeStart).seconds());
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull());
|
||||
if(syncDiagnostic_.get())
|
||||
{
|
||||
double curentRate = 1.0/(this->now()-timeStart).seconds();
|
||||
syncDiagnostic_->tick(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
|
||||
}
|
||||
|
||||
void OdometryROS::resetOdom(
|
||||
|
||||
Reference in New Issue
Block a user