void CobKinectImageFlip::inputCallback(const sensor_msgs::PointCloud2::ConstPtr& point_cloud_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());
}
if (turnAround == false)
{
// image upright
//sensor_msgs::Image color_image_turned_msg = *color_image_msg;
//color_image_turned_msg.header.stamp = ros::Time::now();
//color_camera_image_pub_.publish(color_image_turned_msg);
//sensor_msgs::PointCloud2::ConstPtr point_cloud_turned_msg = point_cloud_msg;
point_cloud_pub_.publish(point_cloud_msg);
}
else
{
// image upside down
// point cloud
pcl::PointCloud<T> point_cloud_src;
pcl::fromROSMsg(*point_cloud_msg, point_cloud_src);
boost::shared_ptr<pcl::PointCloud<T> > point_cloud_turned(new pcl::PointCloud<T>);
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 = true; //point_cloud_msg->is_dense;
point_cloud_turned->resize(point_cloud_src.height*point_cloud_src.width);
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)(point_cloud_src.width-1 - u, point_cloud_src.height-1 - v) = point_cloud_src(u, v);
}
}
// publish turned data
sensor_msgs::PointCloud2::Ptr point_cloud_turned_msg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*point_cloud_turned, *point_cloud_turned_msg);
//point_cloud_turned_msg->header.stamp = ros::Time::now();
point_cloud_pub_.publish(point_cloud_turned_msg);
// cv::namedWindow("test");
// cv::imshow("test", color_image_turned);
// cv::waitKey(10);
}
if (display_timing_ == true)
ROS_INFO("%d ImageFlip: Time stamp of pointcloud message: %f. Delay: %f.", point_cloud_msg->header.seq, point_cloud_msg->header.stamp.toSec(), ros::Time::now().toSec()-point_cloud_msg->header.stamp.toSec());
// ROS_INFO("Pointcloud 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);
}
}
