コード例 #1
0
  void onInitImpl()
  {
    ros::NodeHandle& nh = getNodeHandle();
    ros::NodeHandle& pn = getPrivateNodeHandle();

    // Retrieve parameters from server
    std::string device_serial;
    double open_camera_retry_period;
    double update_rate;
    pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame");
    pn.param<std::string>("device_serial", device_serial, "");
    pn.param<double>("open_camera_retry_period", open_camera_retry_period, 3);
    pn.param<double>("update_rate", update_rate, 30);
    pn.param<bool>("points_with_amplitudes", points_with_amplitudes_, false);

    // Open camera
    while (!camera_)
    {
      try
      {
        camera_ = boost::make_shared<PMDCamboardNano>(device_serial);
        NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str());
        loadCalibrationData();
        camera_->update();
        camera_info_ = camera_->getCameraInfo();
      }
      catch (PMDCameraNotOpenedException& e)
      {
        if (device_serial != "")
          NODELET_INFO("Unable to open PMD camera with serial number %s...", device_serial.c_str());
        else
          NODELET_INFO("Unable to open PMD camera...");
      }
      boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period));
    }

    // Advertise topics
    ros::NodeHandle distance_nh(nh, "distance");
    image_transport::ImageTransport distance_it(distance_nh);
    distance_publisher_ = distance_it.advertiseCamera("image", 1);
    ros::NodeHandle depth_nh(nh, "depth");
    image_transport::ImageTransport depth_it(depth_nh);
    depth_publisher_ = depth_it.advertiseCamera("image", 1);
    ros::NodeHandle amplitude_nh(nh, "amplitude");
    image_transport::ImageTransport amplitude_it(amplitude_nh);
    amplitude_publisher_ = amplitude_it.advertiseCamera("image", 1);
    points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points", 1);

    // Setup periodic callback to get new data from the camera
    update_timer_ = nh.createTimer(ros::Rate(update_rate).expectedCycleTime(), &DriverNodelet::updateCallback, this);

    // Setup dynamic reconfigure server
    reconfigure_server_.reset(new ReconfigureServer(pn));
    reconfigure_server_->setCallback(boost::bind(&DriverNodelet::reconfigureCallback, this, _1, _2));
  }
コード例 #2
0
  void onInitImpl()
  {
    ros::NodeHandle& nh = getNodeHandle();
    ros::NodeHandle& pn = getPrivateNodeHandle();


    // Retrieve parameters from server
    update_rate=30;
    pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame");
    NODELET_INFO("Loaded param frame_id: %s", frame_id_.c_str());
    pn.param<std::string>("device_serial", device_serial, "");
    NODELET_INFO("Loaded param device_serial: %s", device_serial.c_str());
    pn.param<double>("open_camera_retry_period", open_camera_retry_period, 3.);
    pn.param<std::string>("plugin_dir", plugin_dir, "/usr/local/pmd/plugins");
    NODELET_INFO("Loaded param plugin_dir: %s", plugin_dir.c_str());
    pn.param<std::string>("source_plugin", source_plugin, "camboardnano");
    NODELET_INFO("Loaded param source_plugin: %s", source_plugin.c_str());
    pn.param<std::string>("process_plugin", process_plugin, "camboardnanoproc");
    NODELET_INFO("Loaded param process_plugin: %s", process_plugin.c_str());

    // Setup updater
    camera_state_ = OPENING;
    state_info_ = "opening camera " + device_serial;
    updater.setHardwareIDf("%s", device_serial.c_str());
    if(device_serial.empty())
    {
        updater.setHardwareID("unknown");
    }
    updater.add(getName().c_str(), this, &DriverNodelet::getCurrentState);

    // Setup periodic callback to get new data from the camera
    update_timer_ = nh.createTimer(ros::Rate(update_rate).expectedCycleTime(), &DriverNodelet::updateCallback, this, false ,false);

    // Open camera
    openCamera(update_timer_);

    // Advertise topics
    ros::NodeHandle distance_nh(nh, "distance");
    image_transport::ImageTransport distance_it(distance_nh);
    distance_publisher_ = distance_it.advertiseCamera("image", 1);
    ros::NodeHandle depth_nh(nh, "depth");
    image_transport::ImageTransport depth_it(depth_nh);
    depth_publisher_ = depth_it.advertiseCamera("image", 1);
    ros::NodeHandle amplitude_nh(nh, "amplitude");
    image_transport::ImageTransport amplitude_it(amplitude_nh);
    amplitude_publisher_ = amplitude_it.advertiseCamera("image", 1);
    points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points_unrectified", 1);

    // Setup dynamic reconfigure server
    reconfigure_server_.reset(new ReconfigureServer(config_mutex_, pn));
    ReconfigureServer::CallbackType f = boost::bind(&DriverNodelet::reconfigureCallback, this, _1, _2);
    reconfigure_server_->setCallback(f);
  }
コード例 #3
0
VisualOdometry::VisualOdometry(
        const ros::NodeHandle& nh,
        const ros::NodeHandle& nh_private):
    nh_(nh),
    nh_private_(nh_private),
    initialized_(false),
    frame_count_(0)
{
    ROS_INFO("Starting RGBD Visual Odometry");

    // **** initialize ROS parameters

    initParams();

    // **** inititialize state variables

    f2b_.setIdentity();

    // **** publishers

    odom_publisher_ = nh_.advertise<OdomMsg>(
                "vo", queue_size_);
    pose_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseStamped>(
                "pose", queue_size_);
    path_pub_ = nh_.advertise<PathMsg>(
                "path", queue_size_);
    
    feature_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
                "feature/cloud", 1);
    feature_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
                "feature/covariances", 1);
    
    model_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
                "model/cloud", 1);
    model_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
                "model/covariances", 1);
    reset_pos_ = nh_.advertiseService(
                "reset_pos_", &VisualOdometry::resetposSrvCallback, this);
    // **** subscribers

    ImageTransport rgb_it(nh_);
    ImageTransport depth_it(nh_);

    sub_rgb_.subscribe(rgb_it,     "/camera/rgb/image_rect_color",   queue_size_);
    sub_depth_.subscribe(depth_it, "/camera/depth_registered/image_rect_raw", queue_size_);
    sub_info_.subscribe(nh_,       "/camera/rgb/camera_info",  queue_size_);

    // Synchronize inputs.
    sync_.reset(new RGBDSynchronizer3(
                    RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));

    sync_->registerCallback(boost::bind(&VisualOdometry::RGBDCallback, this, _1, _2, _3));
}
コード例 #4
0
	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);
	};
コード例 #5
0
	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);
	}
コード例 #6
0
FeatureViewer::FeatureViewer(
  const ros::NodeHandle& nh, 
  const ros::NodeHandle& nh_private):
  nh_(nh), 
  nh_private_(nh_private),
  frame_count_(0)
{
  ROS_INFO("Starting RGBD Feature Viewer");
  
  // dynamic reconfigure
  FeatureDetectorConfigServer::CallbackType f = boost::bind(
    &FeatureViewer::reconfigCallback, this, _1, _2);
  config_server_.setCallback(f);
  
  // **** initialize ROS parameters
  
  initParams();
  
  mutex_.lock();
  resetDetector();
  mutex_.unlock();

  // **** publishers
  
  cloud_publisher_ = nh_.advertise<PointCloudFeature>(
    "feature/cloud", 1);
  covariances_publisher_ = nh_.advertise<visualization_msgs::Marker>(
    "feature/covariances", 1);
  
  // **** subscribers
  
  ImageTransport rgb_it(nh_);
  ImageTransport depth_it(nh_);

  sub_rgb_.subscribe(rgb_it,     "/rgbd/rgb",   queue_size_);
  sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_);
  sub_info_.subscribe(nh_,       "/rgbd/info",  queue_size_);

  // Synchronize inputs.
  sync_.reset(new RGBDSynchronizer3(
                RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));
  
  sync_->registerCallback(boost::bind(&FeatureViewer::RGBDCallback, this, _1, _2, _3));  
}
コード例 #7
0
ファイル: rgbd_sync.cpp プロジェクト: willdzeng/rtabmap_ros
	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);
	}
コード例 #8
0
void AstraDriver::advertiseROSTopics()
{

  // Allow remapping namespaces rgb, ir, depth, depth_registered
  ros::NodeHandle color_nh(nh_, "rgb");
  image_transport::ImageTransport color_it(color_nh);
  ros::NodeHandle ir_nh(nh_, "ir");
  image_transport::ImageTransport ir_it(ir_nh);
  ros::NodeHandle depth_nh(nh_, "depth");
  image_transport::ImageTransport depth_it(depth_nh);
  ros::NodeHandle depth_raw_nh(nh_, "depth");
  image_transport::ImageTransport depth_raw_it(depth_raw_nh);
  // Advertise all published topics

  // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
  // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
  // assign to pub_depth_raw_. Then pub_depth_raw_.getNumSubscribers() returns 0, and we fail to start
  // the depth generator.
  boost::lock_guard<boost::mutex> lock(connect_mutex_);

  // Asus Xtion PRO does not have an RGB camera
  //ROS_WARN("-------------has color sensor is %d----------- ", device_->hasColorSensor());
  if (device_->hasColorSensor())
  {
    image_transport::SubscriberStatusCallback itssc = boost::bind(&AstraDriver::colorConnectCb, this);
    ros::SubscriberStatusCallback rssc = boost::bind(&AstraDriver::colorConnectCb, this);
    pub_color_ = color_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
  }

  if (device_->hasIRSensor())
  {
    image_transport::SubscriberStatusCallback itssc = boost::bind(&AstraDriver::irConnectCb, this);
    ros::SubscriberStatusCallback rssc = boost::bind(&AstraDriver::irConnectCb, this);
    pub_ir_ = ir_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
  }

  if (device_->hasDepthSensor())
  {
    image_transport::SubscriberStatusCallback itssc = boost::bind(&AstraDriver::depthConnectCb, this);
    ros::SubscriberStatusCallback rssc = boost::bind(&AstraDriver::depthConnectCb, this);
    pub_depth_raw_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
    pub_depth_ = depth_raw_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
  }

  ////////// CAMERA INFO MANAGER

  // Pixel offset between depth and IR images.
  // By default assume offset of (5,4) from 9x7 correlation window.
  // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
  //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
  //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);

  // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
  // camera_info_manager substitutes this for ${NAME} in the URL.
  std::string serial_number = device_->getStringID();
  std::string color_name, ir_name;

  color_name = "rgb_"   + serial_number;
  ir_name  = "depth_" + serial_number;

  // Load the saved calibrations, if they exist
  color_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(color_nh, color_name, color_info_url_);
  ir_info_manager_  = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh,  ir_name,  ir_info_url_);

  get_serial_server = nh_.advertiseService("get_serial", &AstraDriver::getSerialCb,this);

}
コード例 #9
0
ファイル: driver.cpp プロジェクト: jon-weisz/freenect_stack
void DriverNodelet::onInitImpl ()
{
  ros::NodeHandle& nh       = getNodeHandle();        // topics
  ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters

  // Allow remapping namespaces rgb, ir, depth, depth_registered
  image_transport::ImageTransport it(nh);
  ros::NodeHandle rgb_nh(nh, "rgb");
  image_transport::ImageTransport rgb_it(rgb_nh);
  ros::NodeHandle ir_nh(nh, "ir");
  image_transport::ImageTransport ir_it(ir_nh);
  ros::NodeHandle depth_nh(nh, "depth");
  image_transport::ImageTransport depth_it(depth_nh);
  ros::NodeHandle depth_registered_nh(nh, "depth_registered");
  image_transport::ImageTransport depth_registered_it(depth_registered_nh);
  ros::NodeHandle projector_nh(nh, "projector");

  rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
  publish_rgb_ = publish_ir_ = publish_depth_ = true;

  // Check to see if we should enable debugging messages in libfreenect
  // libfreenect_debug_ should be set before calling setupDevice
  param_nh.param("debug" , libfreenect_debug_, false);

  // Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
  updateModeMaps();
  setupDevice();

  // Initialize dynamic reconfigure
  reconfigure_server_.reset( new ReconfigureServer(param_nh) );
  reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));

  // Camera TF frames
  param_nh.param("rgb_frame_id",   rgb_frame_id_,   string("/openni_rgb_optical_frame"));
  param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
  NODELET_INFO("rgb_frame_id = '%s' ",   rgb_frame_id_.c_str());
  NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());

  // Pixel offset between depth and IR images.
  // By default assume offset of (5,4) from 9x7 correlation window.
  // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
  //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
  //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);

  // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
  // camera_info_manager substitutes this for ${NAME} in the URL.
  std::string serial_number = device_->getSerialNumber();
  std::string rgb_name, ir_name;
  if (serial_number.empty())
  {
    rgb_name = "rgb";
    ir_name  = "depth"; /// @todo Make it ir instead?
  }
  else
  {
    rgb_name = "rgb_"   + serial_number;
    ir_name  = "depth_" + serial_number;
  }

  std::string rgb_info_url, ir_info_url;
  param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
  param_nh.param("depth_camera_info_url", ir_info_url, std::string());

  double diagnostics_max_frequency, diagnostics_min_frequency;
  double diagnostics_tolerance, diagnostics_window_time;
  param_nh.param("enable_rgb_diagnostics", enable_rgb_diagnostics_, false);
  param_nh.param("enable_ir_diagnostics", enable_ir_diagnostics_, false);
  param_nh.param("enable_depth_diagnostics", enable_depth_diagnostics_, false);
  param_nh.param("diagnostics_max_frequency", diagnostics_max_frequency, 30.0);
  param_nh.param("diagnostics_min_frequency", diagnostics_min_frequency, 30.0);
  param_nh.param("diagnostics_tolerance", diagnostics_tolerance, 0.05);
  param_nh.param("diagnostics_window_time", diagnostics_window_time, 5.0);

  // Suppress some of the output from loading camera calibrations (kinda hacky)
  log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
  log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
  logger_ccp->setLevel(log4cxx::Level::getFatal());
  logger_cim->setLevel(log4cxx::Level::getWarn());
  // Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
  // depth_registered/foo with Freenect registration disabled, the rectify nodelet for depth_registered/
  // will complain because it receives depth_registered/camera_info (from the register nodelet), but
  // the driver is not publishing depth_registered/image_raw.
  log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
  logger_its->setLevel(log4cxx::Level::getError());
  ros::console::notifyLoggerLevelsChanged();
  
  // Load the saved calibrations, if they exist
  rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
  ir_info_manager_  = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh,  ir_name,  ir_info_url);

  if (!rgb_info_manager_->isCalibrated())
    NODELET_WARN("Using default parameters for RGB camera calibration.");
  if (!ir_info_manager_->isCalibrated())
    NODELET_WARN("Using default parameters for IR camera calibration.");

  // Advertise all published topics
  {
    // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
    // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
    // assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
    // the depth generator.
    boost::lock_guard<boost::mutex> lock(connect_mutex_);

    // Instantiate the diagnostic updater
    pub_freq_max_ = diagnostics_max_frequency;
    pub_freq_min_ = diagnostics_min_frequency;
    diagnostic_updater_.reset(new diagnostic_updater::Updater);

    // Set hardware id
    std::string hardware_id = std::string(device_->getProductName()) + "-" +
        std::string(device_->getSerialNumber());
    diagnostic_updater_->setHardwareID(hardware_id);
    
    // Asus Xtion PRO does not have an RGB camera
    if (device_->hasImageStream())
    {
      image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
      ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
      pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
      if (enable_rgb_diagnostics_) {
        pub_rgb_freq_.reset(new TopicDiagnostic("RGB Image", *diagnostic_updater_,
            FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
                diagnostics_tolerance, diagnostics_window_time)));
      }
    }

    if (device_->hasIRStream())
    {
      image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
      ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
      pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
      if (enable_ir_diagnostics_) {
        pub_ir_freq_.reset(new TopicDiagnostic("IR Image", *diagnostic_updater_,
            FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
                diagnostics_tolerance, diagnostics_window_time)));
      }
    }

    if (device_->hasDepthStream())
    {
      image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
      ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
      pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
      if (enable_depth_diagnostics_) {
        pub_depth_freq_.reset(new TopicDiagnostic("Depth Image", *diagnostic_updater_,
            FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
                diagnostics_tolerance, diagnostics_window_time)));
      }

      pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
      
      if (device_->isDepthRegistrationSupported()) {
        pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
      }
    }
  }

  // Create separate diagnostics thread
  close_diagnostics_ = false;
  diagnostics_thread_ = boost::thread(boost::bind(&DriverNodelet::updateDiagnostics, this));

  // Create watch dog timer callback
  if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
  {
    time_stamp_ = ros::Time(0,0);
    watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
  }
}