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