00001 #include <hector_barrel_detection_nodelet/hector_barrel_detection_nodelet.h>
00002 #include <pluginlib/class_list_macros.h>
00003
00004 PLUGINLIB_EXPORT_CLASS(hector_barrel_detection_nodelet::BarrelDetection, nodelet::Nodelet)
00005
00006 namespace hector_barrel_detection_nodelet{
00007 BarrelDetection::BarrelDetection()
00008 {}
00009
00010 BarrelDetection::~BarrelDetection()
00011 {}
00012
00013 void BarrelDetection::onInit(){
00014 NODELET_DEBUG("Initializing hector_barrel_detection_nodelet");
00015 ROS_INFO("Barreldetection started");
00016 ros::NodeHandle pnh_("~");
00017 ros::NodeHandle nh_("");
00018 image_transport::ImageTransport it_(pnh_);
00019
00020 pnh_.param("h_min", h_min, 100);
00021 pnh_.param("h_max", h_max, 140);
00022 pnh_.param("s_min", s_min, 100);
00023 pnh_.param("s_max", s_max, 255);
00024 pnh_.param("v_min", v_min, 50);
00025 pnh_.param("v_max", v_max, 200);
00026 pnh_.param("bluePart", bluePart, 4.0);
00027 pnh_.param("minRadius", minRadius, 0.15);
00028 pnh_.param("maxRadius", maxRadius, 0.4);
00029
00030 pcl_sub = nh_.subscribe("/openni/depth/points", 1, &BarrelDetection::PclCallback, this);
00031 image_sub = it_.subscribeCamera("/openni/rgb/image_color", 10, &BarrelDetection::imageCallback, this);
00032 cloud_filtered_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2> ("cloud_filtered_barrel", 0);
00033 pose_publisher_ = pnh_.advertise<geometry_msgs::PoseStamped> ("pose_filtered_barrel", 0);
00034 barrel_marker_publisher_ = pnh_.advertise<visualization_msgs::MarkerArray> ("marker_filtered_barrel", 0);
00035 imagePercept_pub_ = pnh_.advertise<hector_worldmodel_msgs::ImagePercept> ("/worldmodel/image_percept", 0);
00036 posePercept_pub_= pnh_.advertise<hector_worldmodel_msgs::PosePercept> ("/worldmodel/pose_percept", 0);
00037 pcl_debug_pub_= pnh_.advertise<sensor_msgs::PointCloud2> ("barrel_pcl_debug", 0);
00038 debug_imagePoint_pub_= pnh_.advertise<geometry_msgs::PointStamped>("blaDebugPoseEstimate",0);
00039 black_white_image_pub_= it_.advertiseCamera("black_white_image",1);
00040
00041 worldmodel_srv_client_=nh_.serviceClient<hector_nav_msgs::GetDistanceToObstacle>("/hector_octomap_server/get_distance_to_obstacle");
00042
00043 pub_imageDetection = it_.advertiseCamera("barrelDetectionImage", 1);
00044 dynamic_recf_type = boost::bind(&BarrelDetection::dynamic_recf_cb, this, _1, _2);
00045 dynamic_recf_server.setCallback(dynamic_recf_type);
00046
00047 }
00048
00049 void BarrelDetection::imageCallback(const sensor_msgs::ImageConstPtr& img, const sensor_msgs::CameraInfoConstPtr& info){
00050 ROS_DEBUG("image callback startet");
00051
00052 hector_nav_msgs::GetDistanceToObstacle dist_msgs;
00053 dist_msgs.request.point.header= img->header;
00054
00055
00056
00057
00058
00059
00060 float distance;
00061
00062
00063 cv_bridge::CvImageConstPtr cv_ptr;
00064 cv_ptr = cv_bridge::toCvShare(img, sensor_msgs::image_encodings::BGR8);
00065 cv::Mat img_filtered(cv_ptr->image);
00066
00067 cv::cvtColor(img_filtered, img_filtered, CV_BGR2HSV);
00068
00069
00070 float cutPercentage= 0.2;
00071 cv::Size size= img_filtered.size();
00072 img_filtered = img_filtered(cv::Rect(size.width*cutPercentage,size.height*cutPercentage,size.width*(1-2*cutPercentage),size.height*(1-2*cutPercentage)));
00073
00074
00075
00076
00077 cv::Mat blueOnly;
00078 cv::inRange(img_filtered, cv::Scalar(h_min, s_min, v_min), cv::Scalar(h_max, s_max, v_max), blueOnly);
00079
00080 if(black_white_image_pub_.getNumSubscribers()>0){
00081 cv_bridge::CvImage cvImg;
00082 cvImg.image = blueOnly;
00083
00084 cvImg.header = img->header;
00085 cvImg.encoding = sensor_msgs::image_encodings::MONO8;
00086 black_white_image_pub_.publish(cvImg.toImageMsg() ,info);
00087 }
00088
00089
00090
00091
00092
00093 cv::SimpleBlobDetector::Params params;
00094 params.filterByColor = true;
00095 params.blobColor = 255;
00096 params.minDistBetweenBlobs = 0.5;
00097 params.filterByArea = true;
00098
00099 params.minArea = (blueOnly.rows * blueOnly.cols) /bluePart;
00100
00101 params.maxArea = blueOnly.rows * blueOnly.cols;
00102 params.filterByCircularity = false;
00103 params.filterByColor = false;
00104 params.filterByConvexity = false;
00105 params.filterByInertia = false;
00106
00107 cv::SimpleBlobDetector blob_detector(params);
00108 std::vector<cv::KeyPoint> keypoints;
00109 keypoints.clear();
00110
00111 blob_detector.detect(blueOnly,keypoints);
00112
00113
00114
00115
00116
00117 hector_worldmodel_msgs::ImagePercept ip;
00118
00119 ip.header= img->header;
00120 ip.info.class_id = "barrel";
00121 ip.info.class_support = 1;
00122 ip.camera_info = *info;
00123
00124 if(pub_imageDetection.getNumSubscribers() > 0){
00125 publish_rectangle_for_recf(keypoints, img, info, img_filtered);
00126 }
00127
00128 for(unsigned int i=0; i<keypoints.size();i++)
00129 {
00130 ip.x = keypoints.at(i).pt.x;
00131 ip.y = keypoints.at(i).pt.y;
00132
00133
00134 ROS_DEBUG("Barrel blob found at image coord: (%f, %f)", ip.x, ip.y);
00135
00136 tf::Pose pose;
00137
00138
00139 CameraModelPtr cameraModel;
00140 cameraModel.reset(new image_geometry::PinholeCameraModel());
00141 cameraModel->fromCameraInfo(info);
00142
00143
00144 cv::Point2d rectified = cameraModel->rectifyPoint(cv::Point2d(ip.x+size.width*cutPercentage, ip.y+size.height*cutPercentage));
00145 cv::Point3d direction_cv = cameraModel->projectPixelTo3dRay(rectified);
00146 tf::Point direction(direction_cv.x, direction_cv.y, direction_cv.z);
00147 direction.normalize();
00148
00149
00150 dist_msgs.request.point.header = ip.header;
00151 tf::pointTFToMsg(direction, dist_msgs.request.point.point);
00152
00153 worldmodel_srv_client_.call(dist_msgs);
00154
00155 distance = std::max(dist_msgs.response.distance, 0.0f);
00156
00157 tf::pointTFToMsg(direction.normalized() * distance, dist_msgs.request.point.point);
00158
00159
00160
00161 const geometry_msgs::PointStamped const_point=dist_msgs.request.point;
00162 geometry_msgs::PointStamped point_in_map;
00163 try{
00164
00165 ros::Time time = img->header.stamp;
00166 listener_.waitForTransform("/map", img->header.frame_id,
00167 time, ros::Duration(3.0));
00168 listener_.transformPoint("/map", const_point, point_in_map);
00169 }
00170 catch (tf::TransformException ex){
00171 ROS_ERROR("Lookup Transform failed: %s",ex.what());
00172 return;
00173 }
00174
00175 if(debug_imagePoint_pub_.getNumSubscribers()>0){
00176 debug_imagePoint_pub_.publish(point_in_map);
00177 }
00178
00179 if(current_pc_msg_!=0 && distance>0){
00180 findCylinder(current_pc_msg_, point_in_map.point.x, point_in_map.point.y, const_point);
00181 }
00182
00183 }
00184
00185
00186 }
00187 void BarrelDetection::PclCallback(const sensor_msgs::PointCloud2::ConstPtr& pc_msg){
00188 ROS_DEBUG("pointcloud callback enterd");
00189 current_pc_msg_= pc_msg;
00190 }
00191
00192 void BarrelDetection::findCylinder(const sensor_msgs::PointCloud2::ConstPtr &pc_msg, float xKey, float yKey, const geometry_msgs::PointStamped cut_around_keypoint){
00193 ROS_DEBUG("started cylinder search");
00194 pcl::PCLPointCloud2 pcl_pc2;
00195 pcl_conversions::toPCL(*pc_msg,pcl_pc2);
00196 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00197 pcl::fromPCLPointCloud2(pcl_pc2,*cloud);
00198
00199
00200
00201 float xmin=cut_around_keypoint.point.x-0.3;
00202 float xmax=cut_around_keypoint.point.x+0.3;
00203 pass_.setInputCloud (cloud);
00204 pass_.setFilterFieldName ("x");
00205 pass_.setFilterLimits (xmin, xmax);
00206 pass_.filter (*cloud);
00207
00208 float ymin=cut_around_keypoint.point.y-0.4;
00209 float ymax=cut_around_keypoint.point.y+0.4;
00210 pass_.setInputCloud (cloud);
00211 pass_.setFilterFieldName ("y");
00212 pass_.setFilterLimits (ymin, ymax);
00213 pass_.filter (*cloud);
00214
00215
00216
00217 tf::StampedTransform transform_cloud_to_map;
00218 try{
00219
00220 ros::Time time = pc_msg->header.stamp;
00221 listener_.waitForTransform("/map", pc_msg->header.frame_id,
00222 time, ros::Duration(3.0));
00223 listener_.lookupTransform("/map", pc_msg->header.frame_id,
00224 time, transform_cloud_to_map);
00225 }
00226 catch (tf::TransformException ex){
00227 ROS_ERROR("Lookup Transform failed: %s",ex.what());
00228 return;
00229 }
00230
00231 tf::transformTFToEigen(transform_cloud_to_map, to_map_);
00232
00233
00234 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
00235 pcl::transformPointCloud(*cloud, *cloud_tmp, to_map_);
00236 cloud = cloud_tmp;
00237 cloud->header.frame_id= transform_cloud_to_map.frame_id_;
00238
00239
00240
00241 if (pcl_debug_pub_.getNumSubscribers() > 0){
00242 sensor_msgs::PointCloud2 filtered_msg;
00243 pcl::toROSMsg(*cloud, filtered_msg);
00244 filtered_msg.header.frame_id = cloud->header.frame_id;
00245 pcl_debug_pub_.publish(filtered_msg);
00246 }
00247
00248
00249 ROS_DEBUG("Normal Estimation");
00250
00251 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00252 ROS_DEBUG("building Tree");
00253 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
00254 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
00255 ne.setSearchMethod (tree);
00256 ne.setInputCloud (cloud);
00257 ne.setKSearch (50);
00258 ROS_DEBUG("estimate Normals");
00259 ne.compute (*cloud_normals);
00260
00261
00262 ROS_DEBUG("Set Cylinder coefficients");
00263 pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
00264 pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
00265 pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices);
00266 seg.setOptimizeCoefficients (true);
00267 seg.setModelType (pcl::SACMODEL_CYLINDER);
00268 seg.setMethodType (pcl::SAC_RANSAC);
00269 seg.setNormalDistanceWeight (0.1);
00270 seg.setMaxIterations (50);
00271 seg.setDistanceThreshold (0.01);
00272 seg.setRadiusLimits (minRadius, maxRadius);
00273 seg.setInputCloud (cloud);
00274 seg.setInputNormals (cloud_normals);
00275 ROS_DEBUG("search cylinders");
00276 Eigen::Vector3f v = Eigen::Vector3f(0.0, 0.0, 1.0);
00277 seg.setAxis(v);
00278 seg.segment (*inliers_cylinder, *coefficients_cylinder);
00279 ROS_DEBUG_STREAM("Cylinder coefficients: " << *coefficients_cylinder);
00280
00281 ROS_DEBUG("extract cylinder potins");
00282 pcl::ExtractIndices<pcl::PointXYZ> extract;
00283 extract.setInputCloud (cloud);
00284 extract.setIndices (inliers_cylinder);
00285 extract.setNegative (false);
00286 extract.filter (*cloud);
00287 ROS_DEBUG_STREAM("Extracted: " << cloud->points.size ());
00288
00289
00290 if (cloud_filtered_publisher_.getNumSubscribers() > 0){
00291 sensor_msgs::PointCloud2 cyl_msg;
00292 pcl::toROSMsg(*cloud, cyl_msg);
00293 cyl_msg.header.frame_id = cloud->header.frame_id;
00294 cloud_filtered_publisher_.publish(cyl_msg);
00295 }
00296
00297 geometry_msgs::Point possibleCylinderPoint;
00298 bool inRange= false;
00299 float epsilon= 0.25;
00300 if( cloud->points.size()>0){
00301 possibleCylinderPoint.x= coefficients_cylinder->values[0];
00302 possibleCylinderPoint.y= coefficients_cylinder->values[1];
00303 float square_distance= std::abs(possibleCylinderPoint.x - xKey)*std::abs(possibleCylinderPoint.x - xKey) +
00304 std::abs(possibleCylinderPoint.y - yKey)*std::abs(possibleCylinderPoint.y - yKey);
00305 if(square_distance < epsilon){
00306 inRange=true;
00307 }
00308
00309 }
00310
00311
00312 if (pose_publisher_.getNumSubscribers() > 0){
00313 geometry_msgs::PoseStamped pose_msg;
00314 pose_msg.header.frame_id=cloud->header.frame_id;
00315 pose_msg.header.stamp=pc_msg->header.stamp;
00316 pose_msg.pose.position.x=possibleCylinderPoint.x;
00317 pose_msg.pose.position.y=possibleCylinderPoint.y;
00318 pose_publisher_.publish(pose_msg);
00319 }
00320
00321 if( cloud->points.size()>0 && inRange)
00322 { ROS_DEBUG("publish cylinder ");
00323
00324 geometry_msgs::PointStamped point_in_map;
00325 try{
00326
00327 ros::Time time = cut_around_keypoint.header.stamp;
00328 listener_.waitForTransform("/map", cut_around_keypoint.header.frame_id,
00329 time, ros::Duration(3.0));
00330 listener_.transformPoint("/map", cut_around_keypoint, point_in_map);
00331 }
00332 catch (tf::TransformException ex){
00333 ROS_ERROR("Lookup Transform failed: %s",ex.what());
00334 return;
00335 }
00336
00337 hector_worldmodel_msgs::PosePercept pp;
00338
00339 pp.header.frame_id= cloud->header.frame_id;
00340 pp.header.stamp= pc_msg->header.stamp;
00341 pp.info.class_id= "barrel";
00342 pp.info.class_support=1;
00343 pp.info.object_support=1;
00344 pp.pose.pose.position.x= coefficients_cylinder->values[0];
00345 pp.pose.pose.position.y= coefficients_cylinder->values[1];
00346 pp.pose.pose.position.z= point_in_map.point.z;
00347 pp.pose.pose.orientation.x= pp.pose.pose.orientation.y = pp.pose.pose.orientation.z= 0;
00348 pp.pose.pose.orientation.w= 1;
00349
00350
00351 if(pp.pose.pose.position.z < 1.1 && pp.pose.pose.position.z >0.2){
00352 posePercept_pub_.publish(pp);
00353 ROS_INFO("PosePercept published");
00354 }
00355
00356
00357 ROS_DEBUG("initialize markerArray");
00358 visualization_msgs::MarkerArray markerArray_msg_;
00359 markerArray_msg_.markers.resize(1);
00360 ROS_DEBUG("markerarry created");
00361 markerArray_msg_.markers[0].action = visualization_msgs::Marker::ADD;
00362 ROS_DEBUG("marker added");
00363
00364 markerArray_msg_.markers[0].header.frame_id = cloud->header.frame_id;
00365 markerArray_msg_.markers[0].header.stamp = pc_msg->header.stamp;
00366 markerArray_msg_.markers[0].id = 0;
00367 markerArray_msg_.markers[0].pose.position.x= pp.pose.pose.position.x;
00368 markerArray_msg_.markers[0].pose.position.y = pp.pose.pose.position.y;
00369 markerArray_msg_.markers[0].pose.position.z = pp.pose.pose.position.z;
00370 markerArray_msg_.markers[0].pose.orientation.x=markerArray_msg_.markers[0].pose.orientation.y= markerArray_msg_.markers[0].pose.orientation.z= pp.pose.pose.orientation.x;
00371 markerArray_msg_.markers[0].pose.orientation.w=1;
00372 ROS_DEBUG("cylinder and text added");
00373
00374 markerArray_msg_.markers[0].color.r = 1.0;
00375 markerArray_msg_.markers[0].color.g = 0.0;
00376 markerArray_msg_.markers[0].color.b = 0.0;
00377 ROS_DEBUG("color added");
00378
00379 markerArray_msg_.markers[0].ns = "cylinder";
00380 markerArray_msg_.markers[0].type = visualization_msgs::Marker::CYLINDER;
00381 markerArray_msg_.markers[0].pose.position.z = 0.6;
00382 markerArray_msg_.markers[0].scale.x = 0.6;
00383 markerArray_msg_.markers[0].scale.y = 0.6;
00384 markerArray_msg_.markers[0].scale.z = 1;
00385 markerArray_msg_.markers[0].color.a = 0.4;
00386 ROS_DEBUG("cylinder only added");
00387 ROS_DEBUG("makerArray complete");
00388 barrel_marker_publisher_.publish(markerArray_msg_);
00389 ROS_DEBUG("markerArray published");
00390
00391
00392 }
00393
00394
00395 }
00396
00397 void BarrelDetection::publish_rectangle_for_recf(std::vector<cv::KeyPoint> keypoints, const sensor_msgs::ImageConstPtr& img, const sensor_msgs::CameraInfoConstPtr& info, cv::Mat& img_filtered){
00398 cv::cvtColor(img_filtered, img_filtered, CV_HSV2BGR);
00399
00400 int width = 3;
00401 int height = 3;
00402
00403 IplImage ipl_img = img_filtered;
00404
00405
00406 for(unsigned int i = 0; i < keypoints.size(); i++){
00407
00408 width = (int)(keypoints.at(i).size );
00409 height = (int)(keypoints.at(i).size );
00410 for(int j = -width; j <= width;j++){
00411 if ((keypoints.at(i).pt.x + j) >= 0 && (keypoints.at(i).pt.x + j) < ipl_img.width){
00412
00413 if ((keypoints.at(i).pt.y - height) >= 0){
00414 cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y - height), (int)(keypoints.at(i).pt.x + j),cv::Scalar(0));
00415 }
00416
00417 if ((keypoints.at(i).pt.y + height) < ipl_img.height){
00418 cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y + height), (int)(keypoints.at(i).pt.x + j),cv::Scalar(0));
00419 }
00420 }
00421 }
00422
00423 for(int k = -height; k <= height;k++){
00424 if ((keypoints.at(i).pt.y + k) >= 0 && (keypoints.at(i).pt.y + k) < ipl_img.height){
00425
00426 if ((keypoints.at(i).pt.x - width) >= 0){
00427 cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y +k), (int)(keypoints.at(i).pt.x - width),cv::Scalar(0));
00428 }
00429
00430 if ((keypoints.at(i).pt.x + width) < ipl_img.width){
00431 cvSet2D(&ipl_img,(int)(keypoints.at(i).pt.y +k), (int)(keypoints.at(i).pt.x + width),cv::Scalar(0));
00432 }
00433 }
00434 }
00435 }
00436
00437
00438 cv_bridge::CvImage cvImg;
00439 cvImg.image = img_filtered;
00440
00441
00442 cvImg.header = img->header;
00443 cvImg.encoding = img->encoding;
00444 pub_imageDetection.publish(cvImg.toImageMsg(),info);
00445
00446 }
00447
00448 void BarrelDetection::dynamic_recf_cb(hector_barrel_detection_nodelet::BarrelDetectionConfig &config, uint32_t level) {
00449 ROS_INFO("Reconfigure Callback enterd");
00450
00451 h_min= config.min_H_value;
00452 h_max= config.max_H_value;
00453 s_min= config.min_S_value;
00454 s_max= config.max_S_value;
00455 v_min= config.min_V_value;
00456 v_max= config.max_V_value;
00457 bluePart= config.bluePart;
00458 minRadius= config.minRadius;
00459 maxRadius= config.maxRadius;
00460
00461 }
00462
00463 }