virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(private_nh, "rgb");
ros::NodeHandle depth_pnh(private_nh, "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);
int queueSize = 10;
bool approxSync = true;
if(private_nh.getParam("max_rate", rate_))
{
NODELET_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);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
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", 1);
imageDepthPub_ = depth_it.advertise("image_out", 1);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
};
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
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_);
ROS_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<MyApproxSyncDisparityDispPolicy>(MyApproxSyncDisparityDispPolicy(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<MyExactSyncDisparityDispPolicy>(MyExactSyncDisparityDispPolicy(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);
}
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
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);
}
