sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(
const ImageBuffer& image, ros::Time time) const {
// The projector info is simply the depth info with the baseline encoded in the P matrix.
// It's only purpose is to be the "right" camera info to the depth camera's "left" for
// processing disparity images.
sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(image, time);
// Tx = -baseline * fx
info->P[3] = -device_->getBaseline() * info->P[0];
return info;
}
void AstraDriver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
{
if ((++data_skip_depth_counter_)%data_skip_==0)
{
data_skip_depth_counter_ = 0;
if (depth_raw_subscribers_||depth_subscribers_)
{
image->header.stamp = image->header.stamp + depth_time_offset_;
if (z_offset_mm_ != 0)
{
uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
for (unsigned int i = 0; i < image->width * image->height; ++i)
if (data[i] != 0)
data[i] += z_offset_mm_;
}
if (fabs(z_scaling_ - 1.0) > 1e-6)
{
uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
for (unsigned int i = 0; i < image->width * image->height; ++i)
if (data[i] != 0)
data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
}
sensor_msgs::CameraInfoPtr cam_info;
if (depth_registration_)
{
image->header.frame_id = color_frame_id_;
cam_info = getColorCameraInfo(image->width,image->height, image->header.stamp);
} else
{
image->header.frame_id = depth_frame_id_;
cam_info = getDepthCameraInfo(image->width,image->height, image->header.stamp);
}
if (depth_raw_subscribers_)
{
pub_depth_raw_.publish(image, cam_info);
}
if (depth_subscribers_ )
{
sensor_msgs::ImageConstPtr floating_point_image = rawToFloatingPointConversion(image);
pub_depth_.publish(floating_point_image, cam_info);
}
}
}
}
void DriverNodelet::publishDepthImage(const ImageBuffer& depth, ros::Time time) const
{
bool registered = depth.is_registered;
sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
depth_msg->header.stamp = time;
depth_msg->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depth_msg->height = depth.metadata.height;
depth_msg->width = depth.metadata.width;
depth_msg->step = depth_msg->width * sizeof(short);
depth_msg->data.resize(depth_msg->height * depth_msg->step);
fillImage(depth, reinterpret_cast<void*>(&depth_msg->data[0]));
if (z_offset_mm_ != 0)
{
uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
if (data[i] != 0)
data[i] += z_offset_mm_;
}
if (registered)
{
// Publish RGB camera info and raw depth image to depth_registered/ ns
depth_msg->header.frame_id = rgb_frame_id_;
pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth, time));
}
else
{
// Publish depth camera info and raw depth image to depth/ ns
depth_msg->header.frame_id = depth_frame_id_;
pub_depth_.publish(depth_msg, getDepthCameraInfo(depth, time));
}
if (enable_depth_diagnostics_)
pub_depth_freq_->tick();
// Projector "info" probably only useful for working with disparity images
if (pub_projector_info_.getNumSubscribers() > 0)
{
pub_projector_info_.publish(getProjectorCameraInfo(depth, time));
}
}
