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();
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);
int queueSize = 5;
bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("rate", max_update_rate_, max_update_rate_);
pnh.param("queue_size", queueSize, queueSize);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(max_update_rate_ == 0.0)
{
ROS_WARN("Parameter \"rate\" (Hz) is not set!");
}
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
}
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);
imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
};
