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

Added "decimation" parameter to data_throttle and stereo_throttle nodelets. Added cloud processing parameters from point_cloud_xyz to point_cloud_xyzrgb nodelet. Added stereo inputs to point_cloud_xyzrgb.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-16 16:26:44 -05:00
parent 4ac2eb09b0
commit 4314246d1e
20 changed files with 491 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/data_throttle.cpp
+73 -14
View File
@@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h>
namespace rtabmap_ros
{
@@ -47,9 +51,10 @@ class DataThrottleNodelet : public nodelet::Nodelet
public:
//Constructor
DataThrottleNodelet():
max_update_rate_(0),
rate_(0),
approxSync_(0),
exactSync_(0)
exactSync_(0),
decimation_(1)
{
}
@@ -67,7 +72,7 @@ public:
private:
ros::Time last_update_;
double max_update_rate_;
double rate_;
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
@@ -84,9 +89,17 @@ private:
int queueSize = 10;
bool approxSync = true;
private_nh.param("max_rate", max_update_rate_, max_update_rate_);
if(private_nh.getParam("max_rate", rate_))
{
ROS_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
@@ -104,40 +117,84 @@ private:
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
imagePub_ = rgb_it.advertise("image_out", 10);
imageDepthPub_ = depth_it.advertise("image_out", 10);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
imagePub_ = rgb_it.advertise("image_out", 1);
imageDepthPub_ = depth_it.advertise("image_out", 1);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
};
void callback(const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& camInfo)
{
if (max_update_rate_ > 0.0)
if (rate_ > 0.0)
{
NODELET_DEBUG("update set to %f", max_update_rate_);
if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
NODELET_DEBUG("update set to %f", rate_);
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
{
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
return;
}
}
else
NODELET_DEBUG("update_rate unset continuing");
NODELET_DEBUG("rate unset continuing");
last_update_ = ros::Time::now();
if(imagePub_.getNumSubscribers())
{
imagePub_.publish(image);
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg());
}
else
{
imagePub_.publish(image);
}
}
if(imageDepthPub_.getNumSubscribers())
{
imageDepthPub_.publish(imageDepth);
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg());
}
else
{
imageDepthPub_.publish(imageDepth);
}
}
if(infoPub_.getNumSubscribers())
{
infoPub_.publish(camInfo);
if(decimation_ > 1)
{
sensor_msgs::CameraInfo info = *camInfo;
info.height /= decimation_;
info.width /= decimation_;
info.roi.height /= decimation_;
info.roi.width /= decimation_;
info.K[2]/=float(decimation_); // cx
info.K[5]/=float(decimation_); // cy
info.K[0]/=float(decimation_); // fx
info.K[4]/=float(decimation_); // fy
info.P[2]/=float(decimation_); // cx
info.P[6]/=float(decimation_); // cy
info.P[0]/=float(decimation_); // fx
info.P[5]/=float(decimation_); // fy
infoPub_.publish(info);
}
else
{
infoPub_.publish(camInfo);
}
}
}
@@ -154,6 +211,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int decimation_;
};