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());
}
Exemplo n.º 2
0
    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);
      }
    }