bool XtionGrabber::setupDepth(const std::string& device)
{
ros::NodeHandle depth_nh(getPrivateNodeHandle(), "depth");
m_depth_it.reset(new image_transport::ImageTransport(depth_nh));
m_pub_depth = m_depth_it->advertiseCamera("image_raw", 1);
m_depth_fd = open(device.c_str(), O_RDONLY);
if(m_depth_fd < 0)
{
perror("Could not open depth device");
return false;
}
struct v4l2_requestbuffers reqbuf;
memset(&reqbuf, 0, sizeof(reqbuf));
reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
reqbuf.memory = V4L2_MEMORY_USERPTR;
reqbuf.count = NUM_BUFS;
if(ioctl(m_depth_fd, VIDIOC_REQBUFS, &reqbuf) != 0)
{
perror("Could not request buffers");
return false;
}
for(size_t i = 0; i < NUM_BUFS; ++i)
{
DepthBuffer* buffer = &m_depth_buffers[i];
buffer->image = createDepthImage();
memset(&buffer->buf, 0, sizeof(buffer->buf));
buffer->buf.index = i;
buffer->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buffer->buf.memory = V4L2_MEMORY_USERPTR;
buffer->buf.m.userptr = (long unsigned int)buffer->image->data.data();
buffer->buf.length = buffer->image->data.size();
if(ioctl(m_depth_fd, VIDIOC_QBUF, &buffer->buf) != 0)
{
perror("Could not queue buffer");
return false;
}
}
if(ioctl(m_depth_fd, VIDIOC_STREAMON, &reqbuf.type) != 0)
{
perror("Could not start streaming");
return false;
}
return true;
}
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));
}
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);
}
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);
};
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);
}
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);
}
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);
}
}
void XtionGrabber::setupDepthInfo()
{
ros::NodeHandle depth_nh(getPrivateNodeHandle(), "depth");
std::string info_url;
getPrivateNodeHandle().param("info_url", info_url, std::string(""));
v4l2_capability caps;
memset(&caps, 0, sizeof(caps));
if(ioctl(m_depth_fd, VIDIOC_QUERYCAP, &caps) != 0)
{
perror("Could not get camera information");
return;
}
std::string card((const char*)caps.card);
std::replace(card.begin(), card.end(), ' ', '_');
card.erase(std::remove(card.begin(), card.end(), ':'));
std::stringstream ss;
ss << card << "_" << m_depthWidth;
m_depth_infoMgr.reset(
new camera_info_manager::CameraInfoManager(
depth_nh, ss.str(), info_url
)
);
if(m_depth_infoMgr->isCalibrated())
{
ROS_INFO("Using saved calibration...");
m_depth_info = m_depth_infoMgr->getCameraInfo();
}
else
{
/* We are reporting information about the *depth* sensor here. */
m_depth_info.width = m_depthWidth;
m_depth_info.height = m_depthHeight;
// No distortion
m_depth_info.D.resize(5, 0.0);
m_depth_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
// Simple camera matrix: square pixels (fx = fy), principal point at center
m_depth_info.K.assign(0.0);
m_depth_info.K[0] = m_depth_info.K[4] = m_depthFocalLength;
m_depth_info.K[2] = (m_depthWidth /2.0) - 0.5;
m_depth_info.K[5] = (m_depthHeight/2.0) - 0.5;
m_depth_info.K[8] = 1.0;
// No separate rectified image plane, so R = I
m_depth_info.R.assign(0.0);
m_depth_info.R[0] = m_depth_info.R[4] = m_depth_info.R[8] = 1.0;
// Then P=K(I|0) = (K|0)
m_depth_info.P.assign(0.0);
m_depth_info.P[0] = m_depth_info.P[5] = m_depthFocalLength; // fx, fy
m_depth_info.P[2] = m_depth_info.K[2]; // cx
m_depth_info.P[6] = m_depth_info.K[5]; // cy
m_depth_info.P[10] = 1.0;
}
m_depth_info.header.frame_id = m_deviceName + "/rgb_optical";
}
