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

created ros2 branch

Browse Source
This commit is contained in:
matlabbe
2020-01-30 17:18:48 -05:00
parent d80a66a724
commit e6ea46f5f0
86 changed files with 9079 additions and 9079 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+375 -409
View File
@@ -25,21 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_ros/OdometryROS.h>
#include <rtabmap_ros/icp_odometry.hpp>
#include <pluginlib/class_list_macros.h>
#include <pluginlib/class_loader.hpp>
#include <laser_geometry/laser_geometry.hpp>
#include <nodelet/nodelet.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/PluginInterface.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_surface.h>
@@ -55,237 +47,374 @@ using namespace rtabmap;
namespace rtabmap_ros
{
class ICPOdometry : public rtabmap_ros::OdometryROS
ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
OdometryROS("icp_odometry", options),
scanCloudMaxPoints_(0),
scanDownsamplingStep_(1),
scanRangeMin_(0),
scanRangeMax_(0),
scanVoxelSize_(0.0),
scanNormalK_(0),
scanNormalRadius_(0.0)
//plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface")
{
public:
ICPOdometry() :
OdometryROS(false, false, true),
scanCloudMaxPoints_(0),
scanDownsamplingStep_(1),
scanRangeMin_(0),
scanRangeMax_(0),
scanVoxelSize_(0.0),
scanNormalK_(0),
scanNormalRadius_(0.0),
plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface")
OdometryROS::init(false, false, true);
}
ICPOdometry::~ICPOdometry()
{
//plugins_.clear();
}
void ICPOdometry::onOdomInit()
{
scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
scanDownsamplingStep_ = this->declare_parameter("scan_downsampling_step", scanDownsamplingStep_);
scanRangeMin_ = this->declare_parameter("scan_range_min", scanRangeMin_);
scanRangeMax_ = this->declare_parameter("scan_range_max", scanRangeMax_);
scanVoxelSize_ = this->declare_parameter("scan_voxel_size", scanVoxelSize_);
scanNormalK_ = this->declare_parameter("scan_normal_k", scanNormalK_);
scanNormalRadius_ = this->declare_parameter("scan_normal_radius", scanNormalRadius_);
/*if (pnh.hasParam("plugins"))
{
XmlRpc::XmlRpcValue pluginsList;
pnh.getParam("plugins", pluginsList);
for (int32_t i = 0; i < pluginsList.size(); ++i)
{
std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
std::string type = static_cast<std::string>(pluginsList[i]["type"]);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
try {
boost::shared_ptr<rtabmap_ros::PluginInterface> plugin = plugin_loader_.createInstance(type);
plugins_.push_back(plugin);
plugin->initialize(pluginName, pnh);
if(!plugin->isEnabled())
{
RCLCPP_WARN(this->get_logger(), "Plugin: %s is not enabled, filtering will not occur. \"enabled_\" member "
"should be managed in subclasses. This can be ignored if the "
"plugin should really be initialized as disabled.",
plugin->getName().c_str());
}
}
catch(pluginlib::PluginlibException & ex) {
RCLCPP_ERROR(this->get_logger(), "Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
}
}
}*/
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_min = %f m", scanRangeMin_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_max = %f m", scanRangeMax_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k = %d", scanNormalK_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS(), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::SensorDataQoS(), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::SensorDataQoS());
}
void ICPOdometry::updateParameters(ParametersMap & parameters)
{
//make sure we are using Reg/Strategy=0
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
if(iter != parameters.end() && iter->second.compare("1") != 0)
{
RCLCPP_WARN(this->get_logger(), "ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
}
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
iter = parameters.find(Parameters::kIcpDownsamplingStep());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!this->has_parameter("scan_downsampling_step"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanDownsamplingStep_ = value;
iter->second = "1";
}
else
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!this->has_parameter("scan_range_min"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMin_ = value;
iter->second = "0";
}
else
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!this->has_parameter("scan_range_max"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMax_ = value;
iter->second = "0";
}
else
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpVoxelSize());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!this->has_parameter("scan_voxel_size"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanVoxelSize_ = value;
iter->second = "0";
}
else
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value != 0)
{
if(!this->has_parameter("scan_normal_k"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalK_ = value;
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!this->has_parameter("scan_normal_radius"))
{
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalRadius_ = value;
}
}
}
}
void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)
{
if(this->isPaused())
{
return;
}
virtual ~ICPOdometry()
// make sure the frame of the laser is updated too
Transform localScanTransform = getTransform(this->frameId(),
scanMsg->header.frame_id,
rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration(scanMsg->ranges.size()*scanMsg->time_increment*10e9),
tfBuffer(), waitForTransform());
if(localScanTransform.isNull())
{
plugins_.clear();
RCLCPP_ERROR(this->get_logger(), "TF of received laser scan topic at time %fs is not set, aborting odometry update.", timestampFromROS(scanMsg->header.stamp));
return;
}
private:
//transform in frameId_ frame
sensor_msgs::msg::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfBuffer());
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
virtual void onOdomInit()
cv::Mat scan;
int maxLaserScans = (int)scanMsg->ranges.size();
if(pclScan->size())
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
if (pnh.hasParam("plugins"))
if(scanDownsamplingStep_ > 1)
{
XmlRpc::XmlRpcValue pluginsList;
pnh.getParam("plugins", pluginsList);
for (int32_t i = 0; i < pluginsList.size(); ++i)
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scanVoxelSize_ > 0.0f)
{
std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
std::string type = static_cast<std::string>(pluginsList[i]["type"]);
NODELET_INFO("IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
try {
boost::shared_ptr<rtabmap_ros::PluginInterface> plugin = plugin_loader_.createInstance(type);
plugins_.push_back(plugin);
plugin->initialize(pluginName, pnh);
if(!plugin->isEnabled())
{
NODELET_WARN("Plugin: %s is not enabled, filtering will not occur. \"enabled_\" member "
"should be managed in subclasses. This can be ignored if the "
"plugin should really be initialized as disabled.",
plugin->getName().c_str());
}
}
catch(pluginlib::PluginlibException & ex) {
ROS_ERROR("Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
}
normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*pclScanNormal, *msg);
msg->header = scanMsg->header;
filtered_scan_pub_->publish(std::move(msg));
}
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
}
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_range_min = %f m", scanRangeMin_);
NODELET_INFO("IcpOdometry: scan_range_max = %f m", scanRangeMax_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
}
virtual void updateParameters(ParametersMap & parameters)
{
//make sure we are using Reg/Strategy=0
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
if(iter != parameters.end() && iter->second.compare("1") != 0)
{
ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
}
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
ros::NodeHandle & pnh = getPrivateNodeHandle();
iter = parameters.find(Parameters::kIcpDownsamplingStep());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
if(filtered_scan_pub_->get_subscription_count())
{
if(!pnh.hasParam("scan_downsampling_step"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanDownsamplingStep_ = value;
iter->second = "1";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!pnh.hasParam("scan_range_min"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMin_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!pnh.hasParam("scan_range_max"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanRangeMax_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpVoxelSize());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_voxel_size"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanVoxelSize_ = value;
iter->second = "0";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value != 0)
{
if(!pnh.hasParam("scan_normal_k"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalK_ = value;
}
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!pnh.hasParam("scan_normal_radius"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalRadius_ = value;
}
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*pclScan, *msg);
msg->header = scanMsg->header;
filtered_scan_pub_->publish(std::move(msg));
}
}
}
void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
rtabmap::SensorData data(
LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
0,
rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
this->processData(data, scanMsg->header.stamp);
}
void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
{
if(this->isPaused())
{
if(this->isPaused())
return;
}
sensor_msgs::msg::PointCloud2 cloudMsg;
/*if (!plugins_.empty())
{
if (plugins_[0]->isEnabled())
{
return;
cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
}
else
{
cloudMsg = *pointCloudMsg;
}
// make sure the frame of the laser is updated too
Transform localScanTransform = getTransform(this->frameId(),
scanMsg->header.frame_id,
scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
if(localScanTransform.isNull())
if (plugins_.size() > 1)
{
ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
return;
for (int i = 1; i < plugins_.size(); i++) {
if (plugins_[i]->isEnabled()) {
cloudMsg = plugins_[i]->filterPointCloud(cloudMsg);
}
}
}
}
else*/
{
cloudMsg = *pointCloudMsg;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
cv::Mat scan;
bool containNormals = false;
if(scanVoxelSize_ == 0.0f)
{
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
{
if(cloudMsg.fields[i].name.compare("normal_x") == 0)
{
containNormals = true;
break;
}
}
}
Transform localScanTransform = getTransform(this->frameId(), cloudMsg.header.frame_id, cloudMsg.header.stamp, tfBuffer(), waitForTransform());
if(localScanTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "TF of received scan cloud at time %fs is not set, aborting rtabmap update.", timestampFromROS(cloudMsg.header.stamp));
return;
}
if(scanCloudMaxPoints_ == 0 && cloudMsg.height > 1)
{
scanCloudMaxPoints_ = cloudMsg.height * cloudMsg.width;
RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
}
int maxLaserScans = scanCloudMaxPoints_;
if(containNormals)
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*pclScan, *msg);
msg->header = cloudMsg.header;
filtered_scan_pub_->publish(std::move(msg));
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
pcl::fromROSMsg(cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
cv::Mat scan;
int maxLaserScans = (int)scanMsg->ranges.size();
if(pclScan->size())
{
if(scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
@@ -296,224 +425,61 @@ private:
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scanVoxelSize_ > 0.0f)
{
normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
if(filtered_scan_pub_->get_subscription_count())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*pclScanNormal, *msg);
msg->header = cloudMsg.header;
filtered_scan_pub_->publish(std::move(msg));
}
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
if(filtered_scan_pub_->get_subscription_count())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = scanMsg->header;
filtered_scan_pub_.publish(msg);
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*pclScan, *msg);
msg->header = cloudMsg.header;
filtered_scan_pub_->publish(std::move(msg));
}
}
}
rtabmap::SensorData data(
LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
0,
rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
this->processData(data, scanMsg->header.stamp);
}
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
LaserScan laserScan = LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform);
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
{
if(this->isPaused())
{
return;
}
sensor_msgs::PointCloud2 cloudMsg;
if (!plugins_.empty())
{
if (plugins_[0]->isEnabled())
{
cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
}
else
{
cloudMsg = *pointCloudMsg;
}
if (plugins_.size() > 1)
{
for (int i = 1; i < plugins_.size(); i++) {
if (plugins_[i]->isEnabled()) {
cloudMsg = plugins_[i]->filterPointCloud(cloudMsg);
}
}
}
}
else
{
cloudMsg = *pointCloudMsg;
}
cv::Mat scan;
bool containNormals = false;
if(scanVoxelSize_ == 0.0f)
{
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
{
if(cloudMsg.fields[i].name.compare("normal_x") == 0)
{
containNormals = true;
break;
}
}
}
Transform localScanTransform = getTransform(this->frameId(), cloudMsg.header.frame_id, cloudMsg.header.stamp);
if(localScanTransform.isNull())
{
ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg.header.stamp.toSec());
return;
}
if(scanCloudMaxPoints_ == 0 && cloudMsg.height > 1)
{
scanCloudMaxPoints_ = cloudMsg.height * cloudMsg.width;
NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
"cloud is not dense, for convenience it will be set to %d (%dx%d)",
scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
}
int maxLaserScans = scanCloudMaxPoints_;
if(containNormals)
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
if(pclScan->size())
{
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScanNormal, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(filtered_scan_pub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*pclScan, msg);
msg.header = cloudMsg.header;
filtered_scan_pub_.publish(msg);
}
}
}
}
LaserScan laserScan = LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform);
if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
{
laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
}
rtabmap::SensorData data(
laserScan,
cv::Mat(),
cv::Mat(),
CameraModel(),
0,
rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
this->processData(data, cloudMsg.header.stamp);
laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
}
protected:
virtual void flushCallbacks()
{
// flush callbacks
}
rtabmap::SensorData data(
laserScan,
cv::Mat(),
cv::Mat(),
CameraModel(),
0,
rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
private:
ros::Subscriber scan_sub_;
ros::Subscriber cloud_sub_;
ros::Publisher filtered_scan_pub_;
int scanCloudMaxPoints_;
int scanDownsamplingStep_;
double scanRangeMin_;
double scanRangeMax_;
double scanVoxelSize_;
int scanNormalK_;
double scanNormalRadius_;
std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
this->processData(data, cloudMsg.header.stamp);
}
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
void ICPOdometry::flushCallbacks()
{
// flush callbacks
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::ICPOdometry)
src/nodelets/obstacles_detection.cpp
+207 -401
View File
@@ -25,433 +25,239 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/obstacles_detection.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <rtabmap_ros/MsgConversion.h>
#include "rtabmap/core/OccupancyGrid.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap_ros
{
class ObstaclesDetection : public nodelet::Nodelet
ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
Node("obstacles_detection", options),
frameId_("base_link"),
waitForTransform_(0.2),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
warned_(false)
{
public:
ObstaclesDetection() :
frameId_("base_link"),
waitForTransform_(false),
mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
warned_(false)
{}
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
virtual ~ObstaclesDetection()
{}
int queueSize = 10;
queueSize = this->declare_parameter("queue_size", queueSize);
frameId_ = this->declare_parameter("frame_id", frameId_);
mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
private:
void parameterMoved(
ros::NodeHandle & nh,
const std::string & rosName,
const std::string & parameterName,
rtabmap::ParametersMap & parameters)
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
{
if(nh.hasParam(rosName))
std::string vStr = declare_parameter(iter->first, iter->second);
if(vStr.compare(iter->second) != 0)
{
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName);
if(iter != gridParameters.end())
{
NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from "
"rtabmap_ros to rtabmap library. Use "
"parameter \"%s\" instead. The value is still "
"copied to new parameter name.",
rosName.c_str(),
parameterName.c_str());
std::string type = rtabmap::Parameters::getType(parameterName);
if(type.compare("float") || type.compare("double"))
{
double v = uStr2Double(iter->second);
nh.getParam(rosName, v);
parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
}
else if(type.compare("int") || type.compare("unsigned int"))
{
int v = uStr2Int(iter->second);
nh.getParam(rosName, v);
parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
}
else
{
NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
}
}
else
{
NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
}
RCLCPP_INFO(this->get_logger(), "obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
}
virtual void onInit()
UASSERT(uContains(gridParameters, rtabmap::Parameters::kGridMapFrameProjection()));
mapFrameProjection_ = uStr2Bool(gridParameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
if(mapFrameProjection_ && mapFrameId_.empty())
{
ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!?
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
if(pnh.hasParam("optimize_for_close_objects"))
{
NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist "
"anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use "
"the old interface.");
}
rtabmap::ParametersMap parameters;
// Backward compatibility
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
iter!=rtabmap::Parameters::getRemovedParameters().end();
++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
std::string paramValue;
if(pnh.getParam(iter->first, vStr))
{
paramValue = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
paramValue = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vDouble))
{
paramValue = uNumber2Str(vDouble);
}
else if(pnh.getParam(iter->first, vInt))
{
paramValue = uNumber2Str(vInt);
}
if(!paramValue.empty())
{
if(iter->second.first)
{
// can be migrated
uInsert(parameters, rtabmap::ParametersPair(iter->second.second, paramValue));
NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), paramValue.c_str());
}
else
{
if(iter->second.second.empty())
{
NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
}
}
rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vDouble))
{
NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
else if(pnh.getParam(iter->first, vInt))
{
NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
}
}
uInsert(parameters, gridParameters);
parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters);
parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters);
parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters);
parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters);
parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters);
parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters);
parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
if(mapFrameProjection_ && mapFrameId_.empty())
{
NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
}
grid_.parseParameters(parameters);
cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
RCLCPP_ERROR(this->get_logger(), "obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
}
grid_.parseParameters(gridParameters);
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS(), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
ros::WallTime time = ros::WallTime::now();
if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
{
// no one wants the results
return;
}
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
{
NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
NODELET_ERROR("%s",ex.what());
return;
}
rtabmap::Transform pose = rtabmap::Transform::getIdentity();
if(!mapFrameId_.empty())
{
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1)))
{
NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
pose = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
NODELET_ERROR("%s",ex.what());
return;
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *inputCloud);
if(inputCloud->isOrganized())
{
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
}
else if(!inputCloud->is_dense && inputCloud->height == 1)
{
if(!warned_)
{
NODELET_WARN("Detected possible wrong format of point cloud \"%s\", it is "
"indicated that it is not dense, but there is only one row. "
"Assuming it is dense... This message will only appear once.", cloudSub_.getTopic().c_str());
warned_ = true;
}
inputCloud->is_dense = true;
}
//Common variables for all strategies
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
if(inputCloud->size())
{
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
pcl::IndicesPtr flatObstacles(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
pose,
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
ground,
obstacles,
&flatObstacles);
if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
{
if(groundPub_.getNumSubscribers() &&
ground.get() && ground->size())
{
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
}
if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
obstacles.get() && obstacles->size())
{
// remove flat obstacles from obstacles
std::set<int> flatObstaclesSet;
if(projObstaclesPub_.getNumSubscribers())
{
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
}
obstaclesCloud->resize(obstacles->size());
obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
int oi=0;
for(unsigned int i=0; i<obstacles->size(); ++i)
{
obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
if(flatObstaclesSet.size() == 0 ||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
{
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
++oi;
}
}
obstaclesCloudWithoutFlatSurfaces->resize(oi);
}
if(!localTransform.isIdentity() || !pose.isIdentity())
{
//transform back in topic frame for 3d clouds and base frame for 2d clouds
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
{
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
}
t = (t*localTransform).inverse();
if(groundCloud->size())
{
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
}
if(obstaclesCloud->size())
{
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
}
}
}
}
else
{
ROS_WARN("obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
}
if(groundPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*groundCloud, rosCloud);
rosCloud.header = cloudMsg->header;
//publish the message
groundPub_.publish(rosCloud);
}
if(obstaclesPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*obstaclesCloud, rosCloud);
rosCloud.header = cloudMsg->header;
//publish the message
obstaclesPub_.publish(rosCloud);
}
if(projObstaclesPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
projObstaclesPub_.publish(rosCloud);
}
NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
}
private:
std::string frameId_;
std::string mapFrameId_;
bool waitForTransform_;
rtabmap::OccupancyGrid grid_;
bool mapFrameProjection_;
bool warned_;
tf::TransformListener tfListener_;
ros::Publisher groundPub_;
ros::Publisher obstaclesPub_;
ros::Publisher projObstaclesPub_;
ros::Subscriber cloudSub_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", 1);
obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", 1);
projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", 1);
}
void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
{
rclcpp::Time time = now();
if (groundPub_->get_subscription_count() == 0 && obstaclesPub_->get_subscription_count() == 0 && projObstaclesPub_->get_subscription_count() == 0)
{
// no one wants the results
return;
}
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
localTransform = getTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
if(localTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
}
rtabmap::Transform pose = rtabmap::Transform::getIdentity();
if(!mapFrameId_.empty())
{
pose = getTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
if(pose.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", mapFrameId_.c_str(), frameId_.c_str());
return;
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *inputCloud);
if(inputCloud->isOrganized())
{
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
}
else if(!inputCloud->is_dense && inputCloud->height == 1)
{
if(!warned_)
{
RCLCPP_WARN(this->get_logger(), "Detected possible wrong format of point cloud \"%s\", it is "
"indicated that it is not dense, but there is only one row. "
"Assuming it is dense... This message will only appear once.", cloudSub_->get_topic_name());
warned_ = true;
}
inputCloud->is_dense = true;
}
//Common variables for all strategies
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
if(inputCloud->size())
{
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
pcl::IndicesPtr flatObstacles(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
pose,
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
ground,
obstacles,
&flatObstacles);
if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
{
if(groundPub_->get_subscription_count() &&
ground.get() && ground->size())
{
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
}
if((obstaclesPub_->get_subscription_count() || projObstaclesPub_->get_subscription_count()) &&
obstacles.get() && obstacles->size())
{
// remove flat obstacles from obstacles
std::set<int> flatObstaclesSet;
if(projObstaclesPub_->get_subscription_count())
{
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
}
obstaclesCloud->resize(obstacles->size());
obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
int oi=0;
for(unsigned int i=0; i<obstacles->size(); ++i)
{
obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
if(flatObstaclesSet.size() == 0 ||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
{
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
++oi;
}
}
obstaclesCloudWithoutFlatSurfaces->resize(oi);
}
if(!localTransform.isIdentity() || !pose.isIdentity())
{
//transform back in topic frame for 3d clouds and base frame for 2d clouds
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
{
obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
}
t = (t*localTransform).inverse();
if(groundCloud->size())
{
groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
}
if(obstaclesCloud->size())
{
obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
}
}
}
}
else
{
RCLCPP_WARN(this->get_logger(), "obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
}
if(groundPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*groundCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
groundPub_->publish(std::move(rosCloud));
}
if(obstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloud, *rosCloud);
rosCloud->header = cloudMsg->header;
//publish the message
obstaclesPub_->publish(std::move(rosCloud));
}
if(projObstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
rosCloud->header.stamp = cloudMsg->header.stamp;
rosCloud->header.frame_id = frameId_;
//publish the message
projObstaclesPub_->publish(std::move(rosCloud));
}
RCLCPP_DEBUG(this->get_logger(), "Obstacles segmentation time = %f s", (now() - time).seconds());
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::ObstaclesDetection)
src/nodelets/point_cloud_xyz.cpp
+213 -293
View File
@@ -25,31 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/point_cloud_xyz.hpp>
#include <rtabmap_ros/MsgConversion.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/pinhole_camera_model.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
@@ -63,10 +46,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_ros
{
class PointCloudXYZ : public nodelet::Nodelet
{
public:
PointCloudXYZ() :
PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
Node("point_cloud_xyz", options),
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
@@ -80,298 +61,237 @@ public:
approxSyncDisparity_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0)
{}
{
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
decimation_ = this->declare_parameter("decimation", decimation_);
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
normalK_ = this->declare_parameter("normal_k", normalK_);
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr);
virtual ~PointCloudXYZ()
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(approxSyncDisparity_)
delete approxSyncDisparity_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(exactSyncDisparity_)
delete exactSyncDisparity_;
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("normal_k", normalK_, normalK_);
pnh.param("normal_radius", normalRadius_, normalRadius_);
pnh.param("filter_nans", filterNaNs_, filterNaNs_);
pnh.param("roi_ratios", roiStr, roiStr);
// Deprecated
if(pnh.hasParam("cut_left"))
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored.");
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
if(pnh.hasParam("cut_right"))
else
{
ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored.");
}
if(pnh.hasParam("special_filter_close_object"))
{
ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing "
"should be done before or after this nodelet. See old implementation here: "
"https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201.");
}
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
}
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
disparitySub_.subscribe(nh, "disparity/image", 1);
disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
}
void callback(
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
{
NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
return;
}
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
m,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depth->header);
NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
}
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
void callbackDisparity(
const stereo_msgs::DisparityImageConstPtr& disparityMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
else
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
return;
}
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
cv::Mat disparity;
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
else
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparity(
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, disparityMsg->header);
NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
}
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
}
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
image_transport::TransportHints hints(this);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoSub_.subscribe(this, "depth/camera_info", rmw_qos_profile_sensor_data);
sensor_msgs::PointCloud2 rosCloud;
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, rosCloud);
}
rosCloud.header.stamp = header.stamp;
rosCloud.header.frame_id = header.frame_id;
//publish the message
cloudPub_.publish(rosCloud);
}
private:
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
int normalK_;
double normalRadius_;
bool filterNaNs_;
std::vector<float> roiRatios_;
ros::Publisher cloudPub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet);
disparitySub_.subscribe(this, "disparity/image", rmw_qos_profile_sensor_data);
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rmw_qos_profile_sensor_data);
}
PointCloudXYZ::~PointCloudXYZ()
{
delete approxSyncDepth_;
delete approxSyncDisparity_;
delete exactSyncDepth_;
delete exactSyncDisparity_;
}
void PointCloudXYZ::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
m,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depth->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZ::callbackDisparity(
const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
return;
}
cv::Mat disparity;
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
else
{
disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparity(
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, disparityMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
}
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;
//publish the message
cloudPub_->publish(std::move(rosCloud));
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::PointCloudXYZ)
src/nodelets/point_cloud_xyzrgb.cpp
+350 -455
View File
@@ -25,33 +25,15 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/point_cloud_xyzrgb.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_ros/MsgConversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
@@ -65,266 +47,163 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_ros
{
class PointCloudXYZRGB : public nodelet::Nodelet
PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
Node("point_cloud_xyzrgb", options),
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
approxSyncStereo_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0),
exactSyncStereo_(0)
{
public:
PointCloudXYZRGB() :
maxDepth_(0.0),
minDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
normalK_(0),
normalRadius_(0.0),
filterNaNs_(false),
approxSyncDepth_(0),
approxSyncDisparity_(0),
approxSyncStereo_(0),
exactSyncDepth_(0),
exactSyncDisparity_(0),
exactSyncStereo_(0)
{}
bool approxSync = true;
std::string roiStr;
int queueSize = 10;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
decimation_ = this->declare_parameter("decimation", decimation_);
noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
normalK_ = this->declare_parameter("normal_k", normalK_);
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr);
virtual ~PointCloudXYZRGB()
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(approxSyncDisparity_)
delete approxSyncDisparity_;
if(approxSyncStereo_)
delete approxSyncStereo_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(exactSyncDisparity_)
delete exactSyncDisparity_;
if(exactSyncStereo_)
delete exactSyncStereo_;
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("normal_k", normalK_, normalK_);
pnh.param("normal_radius", normalRadius_, normalRadius_);
pnh.param("filter_nans", filterNaNs_, filterNaNs_);
pnh.param("roi_ratios", roiStr, roiStr);
//parse roi (region of interest)
roiRatios_.resize(4, 0);
if(!roiStr.empty())
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
std::list<std::string> strValues = uSplit(roiStr, ' ');
if(strValues.size() != 4)
{
ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
}
}
// StereoBM parameters
stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vDouble))
{
NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
else if(pnh.getParam(iter->first, vInt))
{
NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
}
}
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &PointCloudXYZRGB::rgbdImageCallback, this);
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageDisparitySub_.subscribe(nh, "disparity", 1);
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
}
void depthCallback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
imagePtr = cv_bridge::toCvShare(image);
tmpValues[i] = uStr2Float(*jter);
++i;
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
roiRatios_ = tmpValues;
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
}
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
m,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imagePtr->header);
NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
}
}
void disparityCallback(
const sensor_msgs::ImageConstPtr& image,
const stereo_msgs::DisparityImageConstPtr& imageDisparity,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
// StereoBM parameters
stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
{
std::string vStr = declare_parameter(iter->first, iter->second);
if(vStr.compare(iter->second)!=0)
{
RCLCPP_INFO(this->get_logger(), "point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
}
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoSub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
imageDisparitySub_.subscribe(this, "disparity", rmw_qos_profile_sensor_data);
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rmw_qos_profile_sensor_data);
imageRight_.subscribe(this, "right/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoLeft_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
cameraInfoRight_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
}
PointCloudXYZRGB::~PointCloudXYZRGB()
{
delete approxSyncDepth_;
delete approxSyncDisparity_;
delete approxSyncStereo_;
delete exactSyncDepth_;
delete exactSyncDisparity_;
delete exactSyncStereo_;
}
void PointCloudXYZRGB::depthCallback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::Image::ConstSharedPtr imageDepth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
@@ -340,230 +219,246 @@ private:
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
return;
}
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
cv::Mat disparity;
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
UASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
UASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
m,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imagePtr->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::disparityCallback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const stereo_msgs::msg::DisparityImage::ConstSharedPtr imageDisparity,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
imagePtr = cv_bridge::toCvShare(image);
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
return;
}
cv::Mat disparity;
if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
}
else
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imageDisparity->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from disparity time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::stereoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoRight)
{
if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
return;
}
if(cloudPub_->get_subscription_count())
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
}
else
{
disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
if(cloudPub_.getNumSubscribers())
if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
{
ros::WallTime time = ros::WallTime::now();
cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(disparity, roi),
stereoModel,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imageDisparity->header);
NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec());
RCLCPP_WARN(this->get_logger(), "\"roi_ratios\" set but ignored for stereo images.");
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
processAndPublish(pclCloud, indices, imageLeft->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from stereo time = %f s", (now() - time).seconds());
}
}
void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
const sensor_msgs::ImageConstPtr& imageRight,
const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
const sensor_msgs::CameraInfoConstPtr& camInfoRight)
void PointCloudXYZRGB::rgbdImageCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image)
{
if(cloudPub_->get_subscription_count())
{
if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
rclcpp::Time time = now();
rtabmap::SensorData data = rtabmap_ros::rgbdImageFromROS(image);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(data.isValid())
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
return;
}
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
}
else
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
{
ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
data,
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
processAndPublish(pclCloud, indices, imageLeft->header);
NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
processAndPublish(pclCloud, indices, image->header);
}
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from rgbd_image time = %f s", (now() - time).seconds());
}
}
void PointCloudXYZRGB::processAndPublish(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud,
pcl::IndicesPtr & indices,
const std_msgs::msg::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
}
void rgbdImageCallback(const rtabmap_ros::RGBDImageConstPtr & image)
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(cloudPub_.getNumSubscribers())
if(pclCloud->is_dense)
{
ros::WallTime time = ros::WallTime::now();
rtabmap::SensorData data = rtabmap_ros::rgbdImageFromROS(image);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(data.isValid())
{
pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
data,
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
processAndPublish(pclCloud, indices, image->header);
}
NODELET_DEBUG("point_cloud_xyzrgb from rgbd_image time = %f s", (ros::WallTime::now() - time).toSec());
}
}
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
{
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
}
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::PointCloud2 rosCloud;
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, rosCloud);
indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, rosCloud);
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
}
rosCloud.header.stamp = header.stamp;
rosCloud.header.frame_id = header.frame_id;
//publish the message
cloudPub_.publish(rosCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
private:
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
if(filterNaNs_)
{
pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
}
pcl::toROSMsg(*pclCloudNormal, *rosCloud);
}
else
{
if(filterNaNs_ && !pclCloud->is_dense)
{
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::toROSMsg(*pclCloud, *rosCloud);
}
rosCloud->header.stamp = header.stamp;
rosCloud->header.frame_id = header.frame_id;
double maxDepth_;
double minDepth_;
double voxelSize_;
int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
int normalK_;
double normalRadius_;
bool filterNaNs_;
std::vector<float> roiRatios_;
rtabmap::ParametersMap stereoBMParameters_;
ros::Publisher cloudPub_;
ros::Subscriber rgbdImageSub_;
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<stereo_msgs::DisparityImage> imageDisparitySub_;
image_transport::SubscriberFilter imageLeft_;
image_transport::SubscriberFilter imageRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
//publish the message
cloudPub_->publish(std::move(rosCloud));
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::PointCloudXYZRGB)
src/nodelets/pointcloud_to_depthimage.cpp
+164 -201
View File
@@ -25,40 +25,24 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/publisher.h>
#include <ros/subscriber.h>
#include <rtabmap_ros/pointcloud_to_depthimage.hpp>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
namespace rtabmap_ros
{
class PointCloudToDepthImage : public nodelet::Nodelet
{
public:
PointCloudToDepthImage() :
listener_(0),
PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & options) :
Node("pointcloud_to_depthimage", options),
waitForTransform_(0.1),
fillHolesSize_ (0),
fillHolesError_(0.1),
@@ -66,205 +50,184 @@ public:
decimation_(1),
approxSync_(0),
exactSync_(0)
{}
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
virtual ~PointCloudToDepthImage()
int queueSize = 10;
bool approx = true;
queueSize = this->declare_parameter("queue_size", queueSize);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
fillHolesSize_ = this->declare_parameter("fill_holes_size", fillHolesSize_);
fillHolesError_ = this->declare_parameter("fill_holes_error", fillHolesError_);
fillIterations_ = this->declare_parameter("fill_iterations", fillIterations_);
decimation_ = this->declare_parameter("decimation", decimation_);
approx = this->declare_parameter("approx", approx);
if(fixedFrameId_.empty() && approx)
{
delete listener_;
if(approxSync_)
{
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
RCLCPP_FATAL(this->get_logger(), "fixed_frame_id should be set when using approximate "
"time synchronization (approx=true)! If the robot "
"is moving, it could be \"odom\". If not moving, it "
"could be \"base_link\".");
}
private:
virtual void onInit()
RCLCPP_INFO(this->get_logger(), "Params:");
RCLCPP_INFO(this->get_logger(), " approx=%s", approx?"true":"false");
RCLCPP_INFO(this->get_logger(), " queue_size=%d", queueSize);
RCLCPP_INFO(this->get_logger(), " fixed_frame_id=%s", fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), " wait_for_transform=%fs", waitForTransform_);
RCLCPP_INFO(this->get_logger(), " fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
RCLCPP_INFO(this->get_logger(), " fill_holes_error=%f", fillHolesError_);
RCLCPP_INFO(this->get_logger(), " fill_iterations=%d", fillIterations_);
RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_);
auto node = rclcpp::Node::make_shared(this->get_name());
image_transport::ImageTransport it(node);
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1);// 32 bits float in meters
if(approx)
{
listener_ = new tf::TransformListener();
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approx = true;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
pnh.param("fill_holes_error", fillHolesError_, fillHolesError_);
pnh.param("fill_iterations", fillIterations_, fillIterations_);
pnh.param("decimation", decimation_, decimation_);
pnh.param("approx", approx, approx);
if(fixedFrameId_.empty() && approx)
{
ROS_FATAL("fixed_frame_id should be set when using approximate "
"time synchronization (approx=true)! If the robot "
"is moving, it could be \"odom\". If not moving, it "
"could be \"base_link\".");
}
ROS_INFO("Params:");
ROS_INFO(" approx=%s", approx?"true":"false");
ROS_INFO(" queue_size=%d", queueSize);
ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str());
ROS_INFO(" wait_for_transform=%fs", waitForTransform_);
ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
ROS_INFO(" fill_holes_error=%f", fillHolesError_);
ROS_INFO(" fill_iterations=%d", fillIterations_);
ROS_INFO(" decimation=%d", decimation_);
image_transport::ImageTransport it(nh);
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters
if(approx)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2));
}
else
{
fixedFrameId_.clear();
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2));
}
pointCloudSub_.subscribe(nh, "cloud", 1);
cameraInfoSub_.subscribe(nh, "camera_info", 1);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
fixedFrameId_.clear();
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
void callback(
const sensor_msgs::PointCloud2ConstPtr & pointCloud2Msg,
const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
pointCloudSub_.subscribe(this, "cloud", rmw_qos_profile_sensor_data);
cameraInfoSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
}
PointCloudToDepthImage::~PointCloudToDepthImage()
{
delete approxSync_;
delete exactSync_;
}
void PointCloudToDepthImage::callback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointCloud2Msg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
{
if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
{
if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
double cloudStamp = timestampFromROS(pointCloud2Msg->header.stamp);
double infoStamp = timestampFromROS(cameraInfoMsg->header.stamp);
rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
if(!fixedFrameId_.empty())
{
double cloudStamp = pointCloud2Msg->header.stamp.toSec();
double infoStamp = cameraInfoMsg->header.stamp.toSec();
rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
if(!fixedFrameId_.empty())
{
// approx sync
cloudDisplacement = rtabmap_ros::getTransform(
pointCloud2Msg->header.frame_id,
fixedFrameId_,
pointCloud2Msg->header.stamp,
cameraInfoMsg->header.stamp,
*listener_,
waitForTransform_);
}
if(cloudDisplacement.isNull())
{
return;
}
rtabmap::Transform cloudToCamera = rtabmap_ros::getTransform(
// approx sync
cloudDisplacement = rtabmap_ros::getTransform(
pointCloud2Msg->header.frame_id,
cameraInfoMsg->header.frame_id,
fixedFrameId_,
pointCloud2Msg->header.stamp,
cameraInfoMsg->header.stamp,
*listener_,
*tfBuffer_,
waitForTransform_);
}
if(cloudToCamera.isNull())
if(cloudDisplacement.isNull())
{
return;
}
rtabmap::Transform cloudToCamera = rtabmap_ros::getTransform(
pointCloud2Msg->header.frame_id,
cameraInfoMsg->header.frame_id,
cameraInfoMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
if(cloudToCamera.isNull())
{
return;
}
rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
return;
}
rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
model = model.scaled(1.0f/float(decimation_));
}
else
{
ROS_ERROR("decimation (%d) not valid for image size %dx%d",
decimation_,
model.imageWidth(),
model.imageHeight());
}
}
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
cv_bridge::CvImage depthImage;
if(cloud->data.empty())
{
ROS_WARN("Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
model = model.scaled(1.0f/float(decimation_));
}
else
{
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{
for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
}
}
depthImage.header = cameraInfoMsg->header;
if(depthImage32Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg());
}
if(depthImage16Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg());
}
if( cloudStamp != pointCloud2Msg->header.stamp.toSec() ||
infoStamp != cameraInfoMsg->header.stamp.toSec())
{
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"cloud=%f->%f info=%f->%f",
cloudStamp, pointCloud2Msg->header.stamp.toSec(),
infoStamp, cameraInfoMsg->header.stamp.toSec());
RCLCPP_ERROR(this->get_logger(), "decimation (%d) not valid for image size %dx%d",
decimation_,
model.imageWidth(),
model.imageHeight());
}
}
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
cv_bridge::CvImage depthImage;
if(cloud->data.empty())
{
RCLCPP_WARN(this->get_logger(), "Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
}
else
{
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{
for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
}
}
depthImage.header = cameraInfoMsg->header;
if(depthImage32Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg());
}
if(depthImage16Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg());
}
if( cloudStamp != timestampFromROS(pointCloud2Msg->header.stamp) ||
infoStamp != timestampFromROS(cameraInfoMsg->header.stamp))
{
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"cloud=%f->%f info=%f->%f",
cloudStamp, timestampFromROS(pointCloud2Msg->header.stamp),
infoStamp, timestampFromROS(cameraInfoMsg->header.stamp));
}
}
private:
image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
std::string fixedFrameId_;
tf::TransformListener * listener_;
double waitForTransform_;
int fillHolesSize_;
double fillHolesError_;
int fillIterations_;
int decimation_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudToDepthImage, nodelet::Nodelet);
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::PointCloudToDepthImage)
src/nodelets/rgbd_odometry.cpp
+475 -558
View File
File diff suppressed because it is too large Load Diff
src/nodelets/rgbd_relay.cpp
+95 -140
View File
@@ -25,28 +25,16 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include "rtabmap_ros/rgbd_relay.hpp"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <boost/thread.hpp>
#include "rtabmap_ros/RGBDImage.h"
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap/core/Compression.h"
@@ -55,147 +43,114 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_ros
{
class RGBDRelay : public nodelet::Nodelet
RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
Node("rgbd_relay", options),
compress_(false),
uncompress_(false)
{
public:
RGBDRelay() :
compress_(false),
uncompress_(false)
{}
compress_ = this->declare_parameter("compress", compress_);
uncompress_ = this->declare_parameter("uncompress", uncompress_);
virtual ~RGBDRelay()
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image_relay", 1);
}
void RGBDRelay::callback(const rtabmap_ros::msg::RGBDImage::SharedPtr input) const
{
if(rgbdImagePub_->get_subscription_count())
{
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compress", compress_, compress_);
pnh.param("uncompress", uncompress_, uncompress_);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this);
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>(nh.resolveName("rgbd_image") + "_relay", 1);
}
void callback(const rtabmap_ros::RGBDImageConstPtr& input)
{
if(rgbdImagePub_.getNumSubscribers())
if(!compress_ && !uncompress_)
{
if(!compress_ && !uncompress_)
//just republish it
rgbdImagePub_->publish(*input);
return;
}
auto output = std::make_unique<rtabmap_ros::msg::RGBDImage>();
output->header = input->header;
output->rgb_camera_info = input->rgb_camera_info;
output->depth_camera_info = input->depth_camera_info;
rtabmap::StereoCameraModel stereoModel;// = stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
if(compress_)
{
if(!input->rgb_compressed.data.empty())
{
//just republish it
rgbdImagePub_.publish(input);
return;
// already compressed, just copy pointer
output->rgb_compressed = input->rgb_compressed;
}
else if(!input->rgb.data.empty())
{
cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG);
}
rtabmap_ros::RGBDImage output;
output.header = input->header;
output.rgbCameraInfo = input->rgbCameraInfo;
output.depthCameraInfo = input->depthCameraInfo;
rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(input->rgbCameraInfo, input->depthCameraInfo, rtabmap::Transform::getIdentity());
if(compress_)
if(!input->depth_compressed.data.empty())
{
if(!input->rgbCompressed.data.empty())
{
// already compressed, just copy pointer
output.rgbCompressed = input->rgbCompressed;
}
else if(!input->rgb.data.empty())
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
rgb->toCompressedImageMsg(output.rgbCompressed, cv_bridge::JPG);
#endif
}
if(!input->depthCompressed.data.empty())
{
// already compressed, just copy pointer
output.depthCompressed = input->depthCompressed;
}
else if(!input->depth.data.empty())
{
if(stereoModel.isValidForProjection())
{
// right stereo image
cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
imageRightPtr->toCompressedImageMsg(output.depthCompressed, cv_bridge::JPG);
}
else
{
// depth image
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
output.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
output.depthCompressed.format = "png";
}
}
// already compressed, just copy pointer
output->depth_compressed = input->depth_compressed;
}
if(uncompress_)
else if(!input->depth.data.empty())
{
if(!input->rgb.data.empty())
{
// already raw, just copy pointer
output.rgb = input->rgb;
}
if(!input->rgbCompressed.data.empty())
{
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
cv_bridge::toCvCopy(input->rgbCompressed)->toImageMsg(output.rgb);
#endif
}
if(!input->depth.data.empty())
{
// already raw, just copy pointer
output.depth = input->depth;
}
else if(input->depthCompressed.format.compare("jpg")==0)
if(stereoModel.isValidForProjection())
{
// right stereo image
#ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else
cv_bridge::toCvCopy(input->depthCompressed)->toImageMsg(output.depth);
#endif
cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG);
}
else
{
// dpeth image
cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
ptr->header = input->depthCompressed.header;
ptr->image = rtabmap::uncompressImage(input->depthCompressed.data);
ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
ptr->toImageMsg(output.depth);
// depth image
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
output->depth_compressed.format = "png";
}
}
rgbdImagePub_.publish(output);
}
if(uncompress_)
{
if(!input->rgb.data.empty())
{
// already raw, just copy pointer
output->rgb = input->rgb;
}
if(!input->rgb_compressed.data.empty())
{
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
}
if(!input->depth.data.empty())
{
// already raw, just copy pointer
output->depth = input->depth;
}
else if(input->depth_compressed.format.compare("jpg")==0)
{
// right stereo image
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
}
else
{
// dpeth image
auto cvImg = std::make_unique<cv_bridge::CvImage>();
cvImg->header = input->depth_compressed.header;
cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
cvImg->toImageMsg(output->depth);
}
}
rgbdImagePub_->publish(std::move(output));
}
private:
bool compress_;
bool uncompress_;
ros::Subscriber rgbdImageSub_;
ros::Publisher rgbdImagePub_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDRelay, nodelet::Nodelet);
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::RGBDRelay)
src/nodelets/rgbd_sync.cpp
+133 -189
View File
@@ -25,243 +25,187 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/rgbd_sync.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <boost/thread.hpp>
#include "rtabmap_ros/RGBDImage.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap_ros/MsgConversion.h"
namespace rtabmap_ros
{
class RGBDSync : public nodelet::Nodelet
RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
Node("rgbd_sync", options),
depthScale_(1.0),
compressedRate_(0),
callbackCalled_(false),
approxSyncDepth_(0),
exactSyncDepth_(0)
{
public:
RGBDSync() :
depthScale_(1.0),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSyncDepth_(0),
exactSyncDepth_(0)
{}
int queueSize = 10;
bool approxSync = true;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
depthScale_ = this->declare_parameter("depth_scale", depthScale_);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
virtual ~RGBDSync()
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoSub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("depth_scale", depthScale_, depthScale_);
pnh.param("compressed_rate", compressedRate_, compressedRate_);
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
getName().c_str(),
approxSync?"approx":"exact",
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
}
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(5.0);
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
RCLCPP_WARN(this->get_logger(),
"%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",
getName().c_str(),
this->get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
subscribedTopicsMsg_.c_str());
}
}
}
});
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
RGBDSync::~RGBDSync()
{
delete approxSyncDepth_;
delete exactSyncDepth_;
callbackCalled_ = true;
warningThread_->join();
delete warningThread_;
}
void RGBDSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
callbackCalled_ = true;
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
callbackCalled_ = true;
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
double rgbStamp = timestampFromROS(image->header.stamp);
double depthStamp = timestampFromROS(depth->header.stamp);
rtabmap_ros::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfo->header.frame_id;
msg->header.stamp = rgbStamp>depthStamp?image->header.stamp:depth->header.stamp;
msg->rgb_camera_info = *cameraInfo;
msg->depth_camera_info = *cameraInfo;
if(rgbdImageCompressedPub_->get_subscription_count())
{
double rgbStamp = image->header.stamp.toSec();
double depthStamp = depth->header.stamp.toSec();
double infoStamp = cameraInfo->header.stamp.toSec();
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
msg.rgbCameraInfo = *cameraInfo;
msg.depthCameraInfo = *cameraInfo;
if(rgbdImageCompressedPub_.getNumSubscribers())
bool publishCompressed = true;
if (compressedRate_ > 0.0)
{
bool publishCompressed = true;
if (compressedRate_ > 0.0)
if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
{
if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
{
NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
publishCompressed = false;
}
}
if(publishCompressed)
{
lastCompressedPublished_ = ros::Time::now();
rtabmap_ros::RGBDImage msgCompressed = msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
msgCompressed.depthCompressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
}
else
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
}
msgCompressed.depthCompressed.format = "png";
rgbdImageCompressedPub_.publish(msgCompressed);
RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
publishCompressed = false;
}
}
if(rgbdImagePub_.getNumSubscribers())
if(publishCompressed)
{
msg.rgb = *image;
lastCompressedPublished_ = now();
rtabmap_ros::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_ros::msg::RGBDImage);
*msgCompressed = *msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
msgCompressed->depth_compressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
{
cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
imageDepthPtr->image*=depthScale_;
msg.depth = *imageDepthPtr->toImageMsg();
msgCompressed->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
}
else
{
msg.depth = *depth;
msgCompressed->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
}
rgbdImagePub_.publish(msg);
}
msgCompressed->depth_compressed.format = "png";
if( rgbStamp != image->header.stamp.toSec() ||
depthStamp != depth->header.stamp.toSec())
{
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"rgb=%f->%f depth=%f->%f",
rgbStamp, image->header.stamp.toSec(),
depthStamp, depth->header.stamp.toSec());
rgbdImageCompressedPub_->publish(std::move(msgCompressed));
}
}
if(rgbdImagePub_->get_subscription_count())
{
msg->rgb = *image;
if(depthScale_ != 1.0)
{
cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
imageDepthPtr->image*=depthScale_;
msg->depth = *imageDepthPtr->toImageMsg();
}
else
{
msg->depth = *depth;
}
rgbdImagePub_->publish(std::move(msg));
}
if( rgbStamp != timestampFromROS(image->header.stamp) ||
depthStamp != timestampFromROS(depth->header.stamp))
{
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"rgb=%f->%f depth=%f->%f",
rgbStamp, timestampFromROS(image->header.stamp),
depthStamp, timestampFromROS(depth->header.stamp));
}
}
private:
double depthScale_;
double compressedRate_;
boost::thread * warningThread_;
bool callbackCalled_;
ros::Time lastCompressedPublished_;
ros::Publisher rgbdImagePub_;
ros::Publisher rgbdImageCompressedPub_;
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet);
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::RGBDSync)
src/nodelets/stereo_odometry.cpp
+269 -310
View File
@@ -25,19 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_ros/OdometryROS.h"
#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include <rtabmap_ros/stereo_odometry.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/image_encodings.hpp>
#include <image_geometry/stereo_camera_model.h>
@@ -56,319 +46,288 @@ using namespace rtabmap;
namespace rtabmap_ros
{
class StereoOdometry : public rtabmap_ros::OdometryROS
{
public:
StereoOdometry() :
rtabmap_ros::OdometryROS(true, true, false),
StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
rtabmap_ros::OdometryROS("stereo_odometry", options),
approxSync_(0),
exactSync_(0),
queueSize_(5)
{
OdometryROS::init(true, true, false);
}
StereoOdometry::~StereoOdometry()
{
delete approxSync_;
delete exactSync_;
}
void StereoOdometry::onOdomInit()
{
bool approxSync = false;
bool subscribeRGBD = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdSub_->get_topic_name());
}
virtual ~StereoOdometry()
else
{
if(approxSync_)
image_transport::TransportHints hints(this);
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoLeft_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
cameraInfoRight_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
if(approxSync)
{
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
}
private:
virtual void onOdomInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
bool approxSync = false;
bool subscribeRGBD = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
getName().c_str(),
rgbdSub_.getTopic().c_str());
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
{
//make sure we are using Reg/Strategy=0
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
if(iter != parameters.end() && iter->second.compare("0") != 0)
{
ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
}
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
}
void callback(
const sensor_msgs::ImageConstPtr& imageRectLeft,
const sensor_msgs::ImageConstPtr& imageRectRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.frame_id,
cameraInfoLeft->header.stamp);
if(stereoTransform.isNull())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
NODELET_WARN("Odom: input images empty?!?");
}
}
}
void callbackRGBD(
const rtabmap_ros::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform);
if(stereoModel.baseline() <= 0)
{
NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
NODELET_WARN("Odom: input images empty?!?");
}
}
}
protected:
virtual void flushCallbacks()
{
//flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
private:
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
ros::Subscriber rgbdSub_;
int queueSize_;
};
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);
void StereoOdometry::updateParameters(ParametersMap & parameters)
{
//make sure we are using Reg/Strategy=0
ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
if(iter != parameters.end() && iter->second.compare("0") != 0)
{
RCLCPP_WARN(this->get_logger(), "Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
}
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
}
void StereoOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
}
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.frame_id,
cameraInfoLeft->header.stamp,
tfBuffer(),
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
}
}
}
void StereoOdometry::callbackRGBD(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image)
{
callbackCalled();
if(!this->isPaused())
{
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
}
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
if(stereoModel.baseline() <= 0)
{
RCLCPP_FATAL(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
}
}
}
void StereoOdometry::flushCallbacks()
{
//flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::StereoOdometry)
src/nodelets/stereo_sync.cpp
+133 -186
View File
@@ -25,225 +25,172 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/stereo_sync.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <boost/thread.hpp>
#include "rtabmap_ros/RGBDImage.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap_ros/MsgConversion.h"
namespace rtabmap_ros
{
class StereoSync : public nodelet::Nodelet
{
public:
StereoSync() :
StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
Node("stereo_sync", options),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSync_(0),
exactSync_(0)
{}
{
int queueSize = 10;
bool approxSync = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
virtual ~StereoSync()
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
if(approxSync_)
delete approxSync_;
if(exactSync_)
delete exactSync_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
image_transport::TransportHints hints(this);
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoLeftSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
cameraInfoRightSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
int queueSize = 10;
bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compressed_rate", compressedRate_, compressedRate_);
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
imageLeftSub_.getTopic().c_str(),
imageRightSub_.getTopic().c_str(),
cameraInfoLeftSub_.getTopic().c_str(),
cameraInfoRightSub_.getTopic().c_str());
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
}
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport rgb_it(left_nh);
image_transport::ImageTransport depth_it(right_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh);
imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb);
imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth);
cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1);
cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
getName().c_str(),
approxSync?"approx":"exact",
imageLeftSub_.getTopic().c_str(),
imageRightSub_.getTopic().c_str(),
cameraInfoLeftSub_.getTopic().c_str(),
cameraInfoRightSub_.getTopic().c_str());
warningThread_ = new boost::thread(boost::bind(&StereoSync::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
}
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(5.0);
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
RCLCPP_WARN(this->get_logger(),
"%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",
getName().c_str(),
get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
subscribedTopicsMsg_.c_str());
}
}
}
void callback(
const sensor_msgs::ImageConstPtr& imageLeft,
const sensor_msgs::ImageConstPtr& imageRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled_ = true;
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
double leftStamp = imageLeft->header.stamp.toSec();
double rightStamp = imageRight->header.stamp.toSec();
double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfoLeft->header.frame_id;
msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
msg.rgbCameraInfo = *cameraInfoLeft;
msg.depthCameraInfo = *cameraInfoRight;
if(rgbdImageCompressedPub_.getNumSubscribers())
{
bool publishCompressed = true;
if (compressedRate_ > 0.0)
{
if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
{
NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
publishCompressed = false;
}
}
if(publishCompressed)
{
lastCompressedPublished_ = ros::Time::now();
rtabmap_ros::RGBDImage msgCompressed = msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG);
rgbdImageCompressedPub_.publish(msgCompressed);
}
}
if(rgbdImagePub_.getNumSubscribers())
{
msg.rgb = *imageLeft;
msg.depth = *imageRight;
rgbdImagePub_.publish(msg);
}
if( leftStamp != imageLeft->header.stamp.toSec() ||
rightStamp != imageRight->header.stamp.toSec())
{
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"left%f->%f right=%f->%f",
leftStamp, imageLeft->header.stamp.toSec(),
rightStamp, imageRight->header.stamp.toSec());
}
}
}
private:
double compressedRate_;
boost::thread * warningThread_;
bool callbackCalled_;
ros::Time lastCompressedPublished_;
ros::Publisher rgbdImagePub_;
ros::Publisher rgbdImageCompressedPub_;
image_transport::SubscriberFilter imageLeftSub_;
image_transport::SubscriberFilter imageRightSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeftSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoSync, nodelet::Nodelet);
});
}
StereoSync::~StereoSync()
{
delete approxSync_;
delete exactSync_;
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
void StereoSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled_ = true;
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double leftStamp = timestampFromROS(imageLeft->header.stamp);
double rightStamp = timestampFromROS(imageRight->header.stamp);
rtabmap_ros::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfoLeft->header.frame_id;
msg->header.stamp = leftStamp>rightStamp?imageLeft->header.stamp:imageRight->header.stamp;
msg->rgb_camera_info = *cameraInfoLeft;
msg->depth_camera_info = *cameraInfoRight;
if(rgbdImageCompressedPub_->get_subscription_count())
{
bool publishCompressed = true;
if (compressedRate_ > 0.0)
{
if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
{
RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
publishCompressed = false;
}
}
if(publishCompressed)
{
lastCompressedPublished_ = now();
rtabmap_ros::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_ros::msg::RGBDImage);
*msgCompressed = *msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
imageDepthPtr->toCompressedImageMsg(msgCompressed->depth_compressed, cv_bridge::JPG);
rgbdImageCompressedPub_->publish(std::move(msgCompressed));
}
}
if(rgbdImagePub_->get_subscription_count())
{
msg->rgb = *imageLeft;
msg->depth = *imageRight;
rgbdImagePub_->publish(std::move(msg));
}
if( leftStamp != timestampFromROS(imageLeft->header.stamp) ||
rightStamp != timestampFromROS(imageRight->header.stamp))
{
RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"left%f->%f right=%f->%f",
leftStamp, timestampFromROS(imageLeft->header.stamp),
rightStamp, timestampFromROS(imageRight->header.stamp));
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::StereoSync)