void XtionGrabber::setupRGBInfo()
{
ros::NodeHandle color_nh(getPrivateNodeHandle(), "rgb");
std::string info_url;
getPrivateNodeHandle().param("info_url", info_url, std::string(""));
v4l2_capability caps;
memset(&caps, 0, sizeof(caps));
if(ioctl(m_color_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_colorWidth;
m_color_infoMgr.reset(
new camera_info_manager::CameraInfoManager(
color_nh, ss.str(), info_url
)
);
if(m_color_infoMgr->isCalibrated())
{
ROS_INFO("Using saved calibration...");
m_color_info = m_color_infoMgr->getCameraInfo();
}
else
{
/* We are reporting information about the *color* sensor here. */
m_color_info.width = m_colorWidth;
m_color_info.height = m_colorHeight;
// No distortion
m_color_info.D.resize(5, 0.0);
m_color_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
// Simple camera matrix: square pixels (fx = fy), principal point at center
m_color_info.K.assign(0.0);
m_color_info.K[0] = m_color_info.K[4] = m_colorFocalLength;
m_color_info.K[2] = (m_colorWidth /2.0) - 0.5;
m_color_info.K[5] = (m_colorHeight/2.0) - 0.5;
m_color_info.K[8] = 1.0;
// No separate rectified image plane, so R = I
m_color_info.R.assign(0.0);
m_color_info.R[0] = m_color_info.R[4] = m_color_info.R[8] = 1.0;
// Then P=K(I|0) = (K|0)
m_color_info.P.assign(0.0);
m_color_info.P[0] = m_color_info.P[5] = m_colorFocalLength; // fx, fy
m_color_info.P[2] = m_color_info.K[2]; // cx
m_color_info.P[6] = m_color_info.K[5]; // cy
m_color_info.P[10] = 1.0;
}
m_color_info.header.frame_id = m_deviceName + "/rgb_optical";
}
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);
}
bool XtionGrabber::setupColor(const std::string& device)
{
ros::NodeHandle color_nh(getPrivateNodeHandle(), "rgb");
m_color_it.reset(new image_transport::ImageTransport(color_nh));
m_pub_color = m_color_it->advertiseCamera("image_raw", 1);
m_color_fd = open(device.c_str(), O_RDONLY);
if(m_color_fd < 0)
{
perror("Could not open color device");
return false;
}
struct v4l2_format fmt;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = m_colorWidth;
fmt.fmt.pix.height = m_colorHeight;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
if(ioctl(m_color_fd, VIDIOC_S_FMT, &fmt) != 0)
{
perror("Could not set image format");
return false;
}
m_colorWidth = fmt.fmt.pix.width;
m_colorHeight = fmt.fmt.pix.height;
struct v4l2_streamparm parms;
parms.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
parms.parm.capture.timeperframe.numerator = 1;
parms.parm.capture.timeperframe.denominator = 30;
if(ioctl(m_color_fd, VIDIOC_S_PARM, &parms) != 0)
{
perror("Could not set image interval");
return false;
}
/* Enable flicker filter for 50 Hz */
struct v4l2_control ctrl;
memset(&ctrl, 0, sizeof(ctrl));
ctrl.id = V4L2_CID_POWER_LINE_FREQUENCY;
ctrl.value = V4L2_CID_POWER_LINE_FREQUENCY_50HZ;
if(ioctl(m_color_fd, VIDIOC_S_CTRL, &ctrl) != 0)
{
perror("Could not set flicker control");
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_color_fd, VIDIOC_REQBUFS, &reqbuf) != 0)
{
perror("Could not request buffers");
return false;
}
for(size_t i = 0; i < NUM_BUFS; ++i)
{
ColorBuffer* buffer = &m_color_buffers[i];
buffer->data.resize(m_colorWidth*m_colorHeight*2);
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->data.data();
buffer->buf.length = buffer->data.size();
if(ioctl(m_color_fd, VIDIOC_QBUF, &buffer->buf) != 0)
{
perror("Could not queue buffer");
return false;
}
}
if(ioctl(m_color_fd, VIDIOC_STREAMON, &reqbuf.type) != 0)
{
perror("Could not start streaming");
return false;
}
return true;
}
