$search
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 }