/// Initialize sensor fusion node.
/// Setup publisher of point cloud and corresponding color data,
/// setup camera toolboxes and colored point cloud toolbox
/// @return <code>true</code> on success, <code>false</code> otherwise
bool init()
{
if (loadParameters() == false) return false;
image_transport::SubscriberStatusCallback imgConnect = boost::bind(&AllCameraViewer::connectCallback, this);
image_transport::SubscriberStatusCallback imgDisconnect = boost::bind(&AllCameraViewer::disconnectCallback, this);
save_camera_images_service_ = node_handle_.advertiseService("save_camera_images", &AllCameraViewer::saveCameraImagesServiceCallback, this);
// Synchronize inputs of incoming image data.
// Topic subscriptions happen on demand in the connection callback.
// TODO: Dynamically determine, which cameras are available
if(use_right_color_camera_ && use_tof_camera_ && !use_left_color_camera_)
{
ROS_INFO("[all_camera_viewer] Setting up subscribers for right color and tof camera");
shared_sub_sync_.connectInput(right_color_camera_image_sub_, tof_camera_grey_image_sub_);
shared_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::sharedModeSrvCallback, this, _1, _2));
}
else if(use_right_color_camera_ && !use_tof_camera_ && use_left_color_camera_)
{
ROS_INFO("[all_camera_viewer] Setting up subscribers left and right color camera");
stereo_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_);
stereo_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::stereoModeSrvCallback, this, _1, _2));
}
else if(use_right_color_camera_ && use_tof_camera_ && use_left_color_camera_)
{
ROS_INFO("[all_camera_viewer] Setting up subscribers for left color, right color and tof camera");
all_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_, tof_camera_grey_image_sub_);
all_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::allModeSrvCallback, this, _1, _2, _3));
}
else
{
ROS_ERROR("[all_camera_viewer] Specified camera configuration not available");
return false;
}
connectCallback();
ROS_INFO("[all_camera_viewer] Initializing [OK]");
return true;
}
unsigned long init()
{
color_camera_image_sub_.subscribe(*it_, "colorimage", 1);
point_cloud_sub_.subscribe(node_handle_, "pointcloud", 1);
sync_pointcloud_->connectInput(point_cloud_sub_, color_camera_image_sub_);
sync_pointcloud_callback_connection_ = sync_pointcloud_->registerCallback(boost::bind(&CobKinectImageFlipNodelet::inputCallback, this, _1, _2));
return ipa_Utils::RET_OK;
}
// Constructor
PcPublisher()
: image_transport_(n_),
tof_sync_(SyncPolicy(3)),
c_xyz_image_32F3_(0),
c_grey_image_32F1_(0)
{
topicPub_pointCloud_ = n_.advertise<sensor_msgs::PointCloud>("point_cloud", 1);
xyz_image_subscriber_.subscribe(image_transport_, "image_xyz", 1);
grey_image_subscriber_.subscribe(image_transport_,"image_grey", 1);
tof_sync_.connectInput(xyz_image_subscriber_, grey_image_subscriber_);
tof_sync_.registerCallback(boost::bind(&PcPublisher::syncCallback, this, _1, _2));
// topicSub_xyzImage_ = n_.subscribe("xyz_image", 1, &PcPublisher::topicCallback_xyzImage, this);
}
StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {
std::string left_raw = nh_.resolveName("left_raw");
std::string right_raw = nh_.resolveName("right_raw");
image_sub_l_.subscribe(it_, left_raw + "/image_raw", 4);
info_sub_l_ .subscribe(nh_, left_raw + "/camera_info", 4);
image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);
left_ns_ = nh_.resolveName("left");
right_ns_ = nh_.resolveName("right");
ros::NodeHandle cam_l_nh(left_ns_);
ros::NodeHandle cam_r_nh(right_ns_);
it_l_ = new image_transport::ImageTransport(cam_l_nh);
it_r_ = new image_transport::ImageTransport(cam_r_nh);
pub_left_ = it_l_->advertiseCamera("image_raw", 1);
pub_right_ = it_r_->advertiseCamera("image_raw", 1);
sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
}