void
inputCallback(const sensor_msgs::Image::ConstPtr& color_image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg)
{
//ROS_INFO("Input Callback");
//Elements needed for recognition
// convert color image to cv::Mat
cv_bridge::CvImageConstPtr color_image_ptr;
cv::Mat color_image;
convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);
// get color image from point cloud
pcl::PointCloud<pcl::PointXYZRGB> point_cloud_src;
pcl::fromROSMsg(*pointcloud_msg, point_cloud_src);
// cv::Mat color_image = cv::Mat::zeros(point_cloud_src.height, point_cloud_src.width, CV_8UC3);
// for (unsigned int v=0; v<point_cloud_src.height; v++)
// {
// for (unsigned int u=0; u<point_cloud_src.width; u++)
// {
// pcl::PointXYZRGB point = point_cloud_src(u,v);
// if (isnan_(point.z) == false)
// color_image.at<cv::Point3_<unsigned char> >(v,u) = cv::Point3_<unsigned char>(point.b, point.g, point.r);
// }
// }
cv::Mat color_image_copy = color_image.clone();
int half_side_length = 50;
cv::rectangle(color_image_copy, cv::Point(color_image.cols/2-half_side_length, color_image.rows/2-half_side_length),
cv::Point(color_image.cols/2+half_side_length, color_image.rows/2+half_side_length), CV_RGB(0, 255, 0), 3);
cv::imshow("color image", color_image_copy);
char key= cv::waitKey(20);
//std::cout<<key;
//char key = 'c';
//std::cout<<"gut"<<std::endl;
//std::cout<< "How do you want to name the image? (The name of the data cloud is the same as the one of the image"<<std::endl;
if (key=='c')
{
//The image captured is in 'color_image_copy' and then classified
Classifier C;
C.imageExtractROI(color_image_copy);
C.classify();
}
else if (key=='q')
ros::shutdown();
}
void CobKinectImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg)
{
// Timer tim;
// tim.start();
// check camera link orientation and decide whether image must be turned around
bool turnAround = false;
tf::StampedTransform transform;
try
{
transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
btScalar roll, pitch, yaw;
transform.getBasis().getRPY(roll, pitch, yaw, 1);
if (cob3Number_ == 2)
roll *= -1.;
if (roll > 0.0)
turnAround = true;
// std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
// std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
// std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
} catch (tf::TransformException ex)
{
if (display_warnings_ == true)
ROS_WARN("%s",ex.what());
}
// now turn if necessary
if (turnAround == false)
{
// image upright
sensor_msgs::Image color_image_turned_msg = *color_image_msg;
color_camera_image_pub_.publish(color_image_turned_msg);
}
else
{
// image upside down
cv_bridge::CvImageConstPtr color_image_ptr;
cv::Mat color_image;
convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);
// rotate images by 180 degrees
cv::Mat color_image_turned(color_image.rows, color_image.cols, color_image.type());
if (color_image.type() != CV_8UC3)
{
std::cout << "CobKinectImageFlipNodelet::inputCallback: Error: The image format of the color image is not CV_8UC3.\n";
return;
}
for (int v = 0; v < color_image.rows; v++)
{
uchar* src = color_image.ptr(v);
uchar* dst = color_image_turned.ptr(color_image.rows - v - 1);
dst += 3 * (color_image.cols - 1);
for (int u = 0; u < color_image.cols; u++)
{
for (int k = 0; k < 3; k++)
{
*dst = *src;
src++;
dst++;
}
dst -= 6;
}
}
cv_bridge::CvImage cv_ptr;
cv_ptr.image = color_image_turned;
cv_ptr.encoding = "bgr8";
sensor_msgs::Image::Ptr color_image_turned_msg = cv_ptr.toImageMsg();
color_image_turned_msg->header = color_image_msg->header;
color_camera_image_pub_.publish(color_image_turned_msg);
}
//ROS_INFO("Image callback in image flip took %f ms.", tim.getElapsedTimeInMilliSec());
}
void inputCallback(const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg, const sensor_msgs::Image::ConstPtr& color_image_msg)
{
// check camera link orientation and decide whether image must be turned around
bool turnAround = false;
tf::StampedTransform transform;
try
{
transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
btScalar roll, pitch, yaw;
transform.getBasis().getRPY(roll, pitch, yaw, 1);
if (roll > 0.0) turnAround = true;
// std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
// std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
// std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
}
catch (tf::TransformException ex)
{
ROS_WARN("%s",ex.what());
}
if (turnAround==false)
{
// image upright, robot is watching backwards
color_camera_image_pub_.publish(color_image_msg);
point_cloud_pub_.publish(point_cloud_msg);
}
else
{
// image upside down, robot is watching forwards
// read input
// color
cv_bridge::CvImageConstPtr color_image_ptr;
cv::Mat color_image;
convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);
// point cloud
pcl::PointCloud<pcl::PointXYZ> point_cloud_src;
pcl::fromROSMsg(*point_cloud_msg, point_cloud_src);
// rotate images by 180 degrees
// color
cv::Mat color_image_turned(color_image.rows, color_image.cols, color_image.type());
if (color_image.type() != CV_8UC3)
{
std::cout << "CobKinectImageFlipNodelet::inputCallback: Error: The image format of the color image is not CV_8UC3.\n";
return;
}
for (int v=0; v<color_image.rows; v++)
{
uchar* src = color_image.ptr(v);
uchar* dst = color_image_turned.ptr(color_image.rows-v-1);
dst += 3*(color_image.cols-1);
for (int u=0; u<color_image.cols; u++)
{
for (int k=0; k<3; k++)
{
*dst = *src;
src++; dst++;
}
dst -= 6;
}
}
// point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_turned(new pcl::PointCloud<pcl::PointXYZ>);
for (int v=(int)point_cloud_src.height-1; v>=0; v--)
{
for (int u=(int)point_cloud_src.width-1; u>=0; u--)
{
point_cloud_turned->push_back(point_cloud_src(u,v));
}
}
// publish turned data
// color
cv_bridge::CvImage cv_ptr;
cv_ptr.image = color_image_turned;
cv_ptr.encoding = "bgr8";
sensor_msgs::Image::Ptr color_image_turned_msg = cv_ptr.toImageMsg();
color_image_turned_msg->header.stamp = color_image_msg->header.stamp;
color_camera_image_pub_.publish(color_image_turned_msg);
// point cloud
point_cloud_turned->header = point_cloud_msg->header;
point_cloud_turned->height = point_cloud_msg->height;
point_cloud_turned->width = point_cloud_msg->width;
//point_cloud_turned->sensor_orientation_ = point_cloud_msg->sensor_orientation_;
//point_cloud_turned->sensor_origin_ = point_cloud_msg->sensor_origin_;
point_cloud_turned->is_dense = point_cloud_msg->is_dense;
point_cloud_pub_.publish(point_cloud_turned);
// cv::namedWindow("test");
// cv::imshow("test", color_image_turned);
// cv::waitKey(10);
}
}
