pointcloud_colorizer_with_head_movement.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <image_transport/image_transport.h>
00003 #include <opencv/cv.h>
00004 #include <opencv/highgui.h>
00005 #include <cv_bridge/CvBridge.h>
00006 #include <image_geometry/pinhole_camera_model.h>
00007 //for pcl::transformPointCloud
00008 #include <pcl_ros/transforms.h>
00009 #include <tf/transform_listener.h>
00010 #include <tf/transform_broadcaster.h>
00011 #include <angles/angles.h>
00012 #include <sensor_msgs/PointCloud2.h>
00013 //boost
00014 #include <boost/thread/mutex.hpp>
00015 
00016 //for point_cloud::fromROSMsg
00017 #include <pcl/ros/conversions.h>
00018 #include <sensor_msgs/point_cloud_conversion.h>
00019 #include <pcl/point_types.h>
00020 #include <pcl/io/pcd_io.h>
00021 
00022 //for moving of the head
00023 #include <actionlib/client/simple_action_client.h>
00024 #include <pr2_controllers_msgs/PointHeadAction.h>
00025 
00026 // Our Action interface type, provided as a typedef for convenience
00027 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
00028 
00029 class PointCloudColorizer
00030 {
00031   //subscribers/publishers
00032   ros::NodeHandle nh_;
00033   image_transport::CameraSubscriber image_sub_;
00034   ros::Subscriber cloud_sub_;
00035 
00036   tf::TransformListener tf_listener_;
00037   //opencv bridge
00038   image_transport::ImageTransport it_;
00039   sensor_msgs::CvBridge bridge_;
00040   std::string input_image_topic_, input_cloud_topic_, cloud_rgb_topic_;
00041   bool monochrome_, save_image_, cloud_received_, image_received_, move_head_;
00042   //pcl clouds
00043   pcl::PointCloud<pcl::PointXYZ> cloud_in_, cloud_in_tranformed_;
00044   cv::Mat ros_image_, image_;
00045   
00046   boost::mutex  cloud_lock_;
00047   std::vector <pcl::PointCloud<pcl::PointXYZ> > cloud_queue_;
00048   //pinhole cam model
00049   image_geometry::PinholeCameraModel cam_model_;
00050   sensor_msgs::PointCloud2::Ptr output_cloud_ptr_;
00051   ros::Publisher pub_output_;
00052 
00053   //point head client
00054   PointHeadClient* point_head_client_;
00055   //move offset
00056   double move_offset_y_min_, move_offset_y_max_, step_y_; 
00057   double move_offset_z_min_, move_offset_z_max_, step_z_;
00058   double move_offset_x_;
00059   ros::Time stamp_;
00060   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > cloud_xyzrgb_;
00061   boost::thread spin_thread_;
00062   std::map<int, std::pair<int, int32_t> > rgb_average_;
00063 public:
00064   PointCloudColorizer(ros::NodeHandle &n)
00065     :  nh_(n), it_(nh_)
00066   {
00067     //    nh_.param("input_image_topic", input_image_topic_, std::string("/wide_stereo/right/image_rect_color"));
00068     nh_.param("input_image_topic", input_image_topic_, std::string("/wide_stereo/right/image_rect_color_throttle"));
00069     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/points2_out"));
00070     nh_.param("monochrome", monochrome_, false);
00071     nh_.param("save_image", save_image_, false);
00072     nh_.param("cloud_rgb_topic", cloud_rgb_topic_, std::string("cloud_rgb"));
00073     image_sub_ = it_.subscribeCamera(input_image_topic_, 10, &PointCloudColorizer::image_cb, this);
00074     cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudColorizer::cloud_cb, this);
00075     pub_output_ = nh_.advertise<sensor_msgs::PointCloud2> (cloud_rgb_topic_, 1);
00076     ROS_INFO("[PointCloudColorizer:] Subscribing to image topic %s", input_image_topic_.c_str());
00077     ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str());
00078 
00079     point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
00080     //wait for head controller action server to come up 
00081     while(!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
00082     {
00083       ROS_INFO("Waiting for the point_head_action server to come up");
00084     }
00085     image_received_ = cloud_received_ = false;
00086     move_head_ = true;
00087     //TODO: parametrize
00088     move_offset_y_min_ = -2.0;
00089     move_offset_y_max_ = 2.0;
00090     step_y_ = 0.3;
00091     move_offset_z_max_ = 1.5;
00092     move_offset_z_min_ = 0.0;
00093     step_z_ = 0.3;
00094     move_offset_x_ = 1.0;
00095     move_head("base_link", move_offset_x_, move_offset_y_max_, move_offset_z_max_);
00096 
00097     //thread ROS spinner
00098     spin_thread_ = boost::thread (boost::bind (&ros::spin));
00099   }
00100 
00101   ~PointCloudColorizer()
00102   {
00103     delete point_head_client_;
00104   }
00105 
00106   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00107   {
00108     if (!cloud_received_)
00109     {
00110       pcl::PointXYZRGB point_xyzrgb;
00111       unsigned long size = pc->height*pc->width;
00112       ROS_INFO("[PointCloudColorizer: ] cloud received with points: %ld in frame %s", 
00113                size, pc->header.frame_id.c_str());
00114       pcl::fromROSMsg(*pc, cloud_in_);
00115       //TODO: check that the frame is valid
00116       
00117       cloud_xyzrgb_ = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZRGB> >(new pcl::PointCloud<pcl::PointXYZRGB>());
00118       cloud_xyzrgb_->width    = cloud_in_.points.size();
00119       cloud_xyzrgb_->height   = 1;
00120       cloud_xyzrgb_->is_dense = true;
00121       cloud_xyzrgb_->header = cloud_in_.header;
00122       for (unsigned long i = 0; i < cloud_in_.points.size(); i++)
00123       {
00124         point_xyzrgb.x = cloud_in_.points[i].x;
00125         point_xyzrgb.y = cloud_in_.points[i].y;
00126         point_xyzrgb.z = cloud_in_.points[i].z;
00127         //initialize RGB channel to red
00128         point_xyzrgb.rgb = getRGB(0.9, 0.0, 0.0);
00129         cloud_xyzrgb_->points.push_back(point_xyzrgb);
00130       }
00131       cloud_xyzrgb_->width =  cloud_xyzrgb_->points.size();
00132       ROS_INFO_STREAM("width, height: " << cloud_xyzrgb_->width << ", " << cloud_xyzrgb_->width 
00133                       << " is dense: " << cloud_xyzrgb_->is_dense);
00134       cloud_received_ = true;
00135     }
00136   }
00137 
00138   float
00139   getRGB (float r, float g, float b)
00140   {
00141     int32_t res = (int (r * 255) << 16) | (int (g*255) << 8) | int (b*255);
00142     float rgb = *reinterpret_cast<float*>(&res);
00143     return (rgb);
00144   }
00145 
00146   void image_cb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00147   {
00148     // ROS_INFO("[PointCloudColorizer: ] Image received in frame %s", 
00149     //       image_msg->header.frame_id.c_str());
00150     if (!image_received_ && cloud_received_)
00151       {
00152         ROS_INFO("[PointCloudColorizer:] Image received with encoding %s", image_msg->encoding.c_str());
00153         //image_ = NULL;
00154         try 
00155           {
00156             ros_image_ = bridge_.imgMsgToCv(image_msg, "passthrough");
00157           }
00158         catch (sensor_msgs::CvBridgeException& ex) 
00159           {
00160             ROS_ERROR("[PointCloudColorizer:] Failed to convert image");
00161             return;
00162           }
00163         ros_image_.copyTo(image_);
00164         cam_model_.fromCameraInfo(info_msg);
00165         stamp_ = info_msg->header.stamp;
00166         image_received_ = true;
00167 
00168         if (save_image_)
00169           {
00170             std::stringstream stamp;
00171             stamp << stamp_;
00172             ROS_INFO("Saving image to %s", ("cb_" + stamp.str()).c_str());
00173             cv::imwrite("cb_" + stamp.str() + ".png", image_);
00174           }
00175       }
00176   }
00177 
00178   void move_head (std::string frame_id, double x, double y, double z)
00179   {
00180     //the goal message we will be sending
00181     pr2_controllers_msgs::PointHeadGoal goal;
00182     
00183     //the target point, expressed in the requested frame
00184     geometry_msgs::PointStamped point;
00185     point.header.frame_id = frame_id;
00186     point.point.x = x; point.point.y = y; point.point.z = z;
00187     goal.target = point;
00188     
00189     //we are pointing the wide_stereo camera frame 
00190     //(pointing_axis defaults to X-axis)
00191     goal.pointing_frame = "narrow_stereo_optical_frame";
00192     
00193     //take at least 2 seconds to get there
00194     goal.min_duration = ros::Duration(1);
00195     
00196     //and go no faster than 0.1 rad/s
00197     goal.max_velocity = 0.5;
00198     
00199     //send the goal
00200     point_head_client_->sendGoal(goal);
00201     
00202     //wait for it to get there
00203     bool finished_within_time = point_head_client_->waitForResult(ros::Duration(20.0));
00204     if (!finished_within_time)
00205       {
00206         ROS_ERROR("Head did not move to pose: %f %f %f", point.point.x, point.point.y, point.point.z);
00207       }
00208     else
00209       {
00210         actionlib::SimpleClientGoalState state = point_head_client_->getState();
00211         bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00212         if(success)
00213           ROS_INFO("Action move head finished: %s position: %f %f %f", state.toString().c_str(), point.point.x, point.point.y, point.point.z);
00214         else
00215           ROS_ERROR("Action move head failed: %s", state.toString().c_str());
00216       }
00217   }
00218 
00219   void process (cv::Mat image)
00220   {
00221     //create a sequence storage for projected points 
00222     CvMemStorage* stor = cvCreateMemStorage (0);
00223     CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00224     // pcl::transformPointCloud(cam_model_.tfFrame(), cloud_in_, cloud_in_tranformed_, tf_listener_);
00225     ros::Time now = ros::Time::now();
00226     tf_listener_.waitForTransform(cam_model_.tfFrame(), cloud_in_.header.frame_id, now, ros::Duration(20.0));
00227     tf::StampedTransform transform;
00228     tf_listener_.lookupTransform(cam_model_.tfFrame(), cloud_in_.header.frame_id, stamp_, transform);
00229     pcl_ros::transformPointCloud(cloud_in_, cloud_in_tranformed_, transform);
00230 
00231     ROS_INFO("[PointCloudColorizer:] cloud transformed to %s", cam_model_.tfFrame().c_str());
00232 
00233 
00234     if (cloud_in_tranformed_.points.size() == 0)
00235     {
00236       ROS_WARN("[PointCloudColorizer:] Point cloud points with size 0!!! Returning.");
00237       return;
00238     }
00239 
00240     //pcl::PointXYZRGB point_xyzrgb;
00241     for (unsigned long i = 0; i < cloud_in_tranformed_.points.size(); i++)
00242     {
00243       cv::Point3d pt_cv(cloud_in_tranformed_.points[i].x, cloud_in_tranformed_.points[i].y, 
00244                         cloud_in_tranformed_.points[i].z);
00245       cv::Point2d uv;
00246       cam_model_.project3dToPixel(pt_cv, uv);
00247       ROS_DEBUG_STREAM("points projected: " << round(uv.x) << " " << round(uv.y));
00248       //ROS_INFO_STREAM("uv.x: " << round(uv.x) << "uv.y: " << round(uv.y));
00249    
00250       //if point projection outside of image assign rgb white value
00251       if (round(uv.x) < 200 || round(uv.x) > (image.cols-200) || round(uv.y) < 150 || round(uv.y) > (image.rows-150))
00252       {
00253         //        ROS_WARN("[PointCloudColorizer:] width out of bounds");
00254         //point_xyzrgb.rgb = getRGB(0.9, 0.0, 0.0);
00255         //continue;
00256       }
00257       else
00258       {
00259         ROS_DEBUG_STREAM("uv.x: " << round(uv.x) << "uv.y: " << round(uv.y));
00260 
00261         //uint8_t g = image.at<uint8_t>(round(uv.x), round(uv.y)*image.channels() + 3);
00262         //int32_t rgb = g | (g << 8) |  (g << 16);
00263         
00264         //VERSION for MONO
00265         if (monochrome_)
00266         {
00267           uint8_t g = image.at<uint8_t>(round(uv.y), round(uv.x));
00268           //int32_t rgb = g | (g << 8) |  (g << 16);
00269           int32_t rgb = (g << 16) | (g << 8) | g;
00270           //          point_xyzrgb.rgb = *reinterpret_cast<float*>(&rgb);
00271           if (cloud_xyzrgb_->points[i].rgb == getRGB(0.9, 0.0, 0.0))
00272             cloud_xyzrgb_->points[i].rgb = *reinterpret_cast<float*>(&rgb);
00273         }
00274         //VERSION for BGR8
00275         else
00276         {
00277           const cv::Vec3b& bgr = image.at<cv::Vec3b>(round(uv.y), round(uv.x));
00278           int32_t rgb_packed = (bgr[0] << 16) | (bgr[1] << 8) | bgr[2];
00279           //point_xyzrgb.rgb = *reinterpret_cast<float*>(&rgb_packed);
00280           if (cloud_xyzrgb_->points[i].rgb ==  getRGB(0.9, 0.0, 0.0))
00281             {
00282               std::pair <int, int32_t> freq_rgb;
00283               freq_rgb.first = 1;
00284               freq_rgb.second = rgb_packed;
00285               rgb_average_[i] = freq_rgb;
00286               cloud_xyzrgb_->points[i].rgb = *reinterpret_cast<float*>(&(rgb_packed));
00287             }
00288           else
00289             {
00290                 std::pair <int, int32_t> freq_rgb = rgb_average_[i];
00291                 freq_rgb.first = freq_rgb.first + 1;
00292                 freq_rgb.second = freq_rgb.second + rgb_packed;
00293                 rgb_average_[i] = freq_rgb;
00294                 rgb_packed = freq_rgb.second/freq_rgb.first;
00295                 cloud_xyzrgb_->points[i].rgb = *reinterpret_cast<float*>(&rgb_packed);
00296             }
00297           //   cloud_xyzrgb_->points[i].rgb = *reinterpret_cast<float*>(&rgb_packed);
00298           // else
00299           //   cloud_xyzrgb_->points[i].rgb = (cloud_xyzrgb_->points[i].rgb + *reinterpret_cast<float*>(&rgb_packed))/2;
00300         }
00301       }
00302       //fill in points
00303       //point_xyzrgb.x = cloud_in_tranformed_.points[i].x;
00304       //point_xyzrgb.y = cloud_in_tranformed_.points[i].y;
00305       //point_xyzrgb.z = cloud_in_tranformed_.points[i].z;
00306       //TODO:fix me
00307       //if (point_xyzrgb.rgb == getRGB(0.9, 0.0, 0.0))
00308       //cloud_xyzrgb_->points.push_back(point_xyzrgb);
00309     }
00310     if (save_image_)
00311     {
00312       std::stringstream stamp;
00313       stamp << stamp_;
00314       ROS_INFO("Saving image to %s", stamp.str().c_str());
00315       cv::imwrite(stamp.str() + ".png", image);
00316     }
00317     //cloud_xyzrgb_->width =  cloud_xyzrgb_->points.size();
00318     boost::shared_ptr< sensor_msgs::PointCloud2> output_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2()); 
00319     pcl::toROSMsg (*cloud_xyzrgb_, *output_cloud);
00320     ROS_INFO("Publishing PointXZYRGB cloud with %ld points in frame %s", cloud_xyzrgb_->points.size(), 
00321              output_cloud->header.frame_id.c_str());
00322     output_cloud->header.stamp = ros::Time::now();
00323     pub_output_.publish (output_cloud);
00324     cvClearSeq(seq);
00325     cvClearMemStorage(stor);
00326     move_head_ = true;
00327   } 
00328 
00329 void spin ()
00330 {
00331   ros::Rate loop_rate(5);
00332   while (ros::ok())
00333     {
00334       //HEAD moving part
00335       if (move_head_ && move_offset_y_max_ >= move_offset_y_min_ && move_offset_z_max_ >= move_offset_z_min_)
00336         {
00337           move_head("base_link", 1.0, move_offset_y_max_, move_offset_z_max_);
00338           sleep(5);
00339           move_offset_y_max_ = move_offset_y_max_ - step_y_;
00340           if (move_offset_y_max_ <= move_offset_y_min_ && move_offset_z_max_ != move_offset_z_min_)
00341             {
00342               move_offset_z_max_ = move_offset_z_max_ - step_z_;
00343               //TODO: only works if min/max offsets are symetrical
00344               move_offset_y_max_ = -move_offset_y_min_;
00345             }
00346           //sleep(0.1);
00347           //get the image
00348           move_head_ = false; 
00349           image_received_ = false;
00350         }
00351       else if (move_offset_y_max_ <= move_offset_y_min_ && move_offset_z_max_ == move_offset_z_min_)
00352         {
00353           ROS_INFO("DONE - exiting");
00354           break;
00355         }
00356                   
00357       //register cloud and texture
00358       else if (cloud_received_ && image_received_)
00359         {
00360           process(image_);
00361         }
00362       else
00363         {
00364           //TODO: ros::shutdown()??
00365           ROS_INFO("Done or waiting!");
00366         }  
00367       ros::spinOnce();
00368       loop_rate.sleep();
00369     }
00370 }
00371 };
00372 
00373 int main(int argc, char** argv)
00374 {
00375   ros::init(argc, argv, "pointcloud_colorizer_node");
00376   ros::NodeHandle n("~");
00377   PointCloudColorizer pp(n);
00378   pp.spin();
00379   //ros::spin();
00380   //pp.spin();
00381 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31