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); }
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); } }