예제 #1
0
	/// 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;
	}
예제 #2
0
 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);
        }
예제 #4
0
  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));
  }