void imageCallback(const sensor_msgs::ImageConstPtr& depth_msg)
{
std::cout << "imageCallback" <<std::endl;
//Convert to opencv
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(depth_msg, enc::TYPE_32FC1);
}
catch (cv_bridge::Exception & e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::imshow("Depth Image", cv_ptr->image);
cv::waitKey(3);
// std::cout << this << " " << &point_cloud_pub_ << std::endl;
PointCloud::Ptr cloud_msg(new PointCloud);
cloud_msg->header = depth_msg->header;
cloud_msg->height = depth_msg->height;
cloud_msg->width = depth_msg->width;
cloud_msg->is_dense = false;
cloud_msg->is_bigendian = false;
sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
if (depth_msg->encoding == enc::TYPE_16UC1)
{
convert<uint16_t>(depth_msg, cloud_msg);
}
else if (depth_msg->encoding == enc::TYPE_32FC1)
{
convert<float>(depth_msg, cloud_msg);
}
else
{
ROS_ERROR("Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
return;
}
pointCloudPub.publish (cloud_msg);
newImagePub.publish(depth_msg);
}
void DepthCloudDisplay::processMessage(const sensor_msgs::ImageConstPtr& depth_msg,
const sensor_msgs::ImageConstPtr& rgb_msg)
{
++messages_received_;
setStatus(StatusProperty::Ok, "Depth Map", QString::number(messages_received_) + " depth maps received");
// Bit depth of image encoding
int bitDepth = enc::bitDepth(depth_msg->encoding);
int numChannels = enc::numChannels(depth_msg->encoding);
sensor_msgs::CameraInfo::ConstPtr camInfo;
{
boost::mutex::scoped_lock lock(camInfo_mutex_);
camInfo = camInfo_;
}
// output pointcloud2 message
sensor_msgs::PointCloud2Ptr cloud_msg ( new sensor_msgs::PointCloud2 );
if ((bitDepth == 32) && (numChannels == 1))
{
// floating point encoded depth map
convert<float>(depth_msg, rgb_msg, camInfo, cloud_msg);
}
else if ((bitDepth == 16) && (numChannels == 1))
{
// 32bit integer encoded depth map
convert<uint16_t>(depth_msg, rgb_msg, camInfo, cloud_msg);
}
else
{
std::stringstream errorMsg;
errorMsg << "Input image must be encoded in 32bit floating point format or 16bit integer format (input format is: "
<< depth_msg->encoding << ")";
setStatus(StatusProperty::Error, "Message", QString(errorMsg.str().c_str()));
return;
}
// add point cloud message to pointcloud_common to be visualized
pointcloud_common_->addMessage(cloud_msg);
setStatus(StatusProperty::Ok, "Message", QString("Ok"));
}
