00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include "door_handle_detector/doors_detector.h"
00032 #include <pr2_doors_common/door_functions.h>
00033
00034
00035 using namespace std;
00036 using namespace door_msgs;
00037 using namespace mapping_msgs;
00038 using namespace ros;
00039 using namespace door_handle_detector;
00040 using namespace pr2_doors_common;
00041
00042 #define DEBUG_FAILURES 0
00043
00044
00045
00046 DoorDetector::DoorDetector ()
00047 : node_tilde_("~")
00048 {
00049
00050 {
00051 node_tilde_.param ("parameter_frame", parameter_frame_, string ("base_footprint"));
00052 node_tilde_.param ("fixed_frame", fixed_frame_, string ("odom_combined"));
00053
00054 node_tilde_.param ("door_min_height", door_min_height_, 1.2);
00055 node_tilde_.param ("door_max_height", door_max_height_, 3.0);
00056 node_tilde_.param ("door_min_width", door_min_width_, 0.7);
00057 node_tilde_.param ("door_max_width", door_max_width_, 1.4);
00058 node_tilde_.param ("door_max_dist_from_prior", max_dist_from_prior_, 0.5);
00059 ROS_DEBUG ("Using the following thresholds for door detection [min-max height / min-max width]: %f-%f / %f-%f.",
00060 door_min_height_, door_max_height_, door_min_width_, door_max_width_);
00061
00062
00063 node_tilde_.param ("maximum_search_radius", maximum_search_radius_, 8.0);
00064 maximum_search_radius_ *= maximum_search_radius_;
00065
00066
00067 node_tilde_.param ("maximum_scan_angle", maximum_scan_angle_limit_, 70.0);
00068 maximum_scan_angle_limit_ = angles::from_degrees (maximum_scan_angle_limit_);
00069
00070
00071
00072
00073 node_tilde_.param ("rectangle_constrain_edge_height", rectangle_constrain_edge_height_, 0.4);
00074
00075 node_tilde_.param ("rectangle_constrain_edge_angle", rectangle_constrain_edge_angle_, 10.0);
00076 rectangle_constrain_edge_angle_ = angles::from_degrees (rectangle_constrain_edge_angle_);
00077 }
00078
00079
00080 leaf_width_ = 0.02;
00081 k_search_ = 10;
00082 z_axis_.x = 0; z_axis_.y = 0; z_axis_.z = 1;
00083
00084 minimum_z_ = 0.05;
00085 maximum_z_ = 2.7;
00086
00087 normal_angle_tolerance_ = angles::from_degrees (15.0);
00088
00089 euclidean_cluster_angle_tolerance_ = angles::from_degrees (25.0);
00090 euclidean_cluster_min_pts_ = 1000;
00091 euclidean_cluster_distance_tolerance_ = 0.05;
00092
00093
00094 sac_distance_threshold_ = leaf_width_ * 2;
00095
00096
00097 door_min_z_ = 0.35;
00098
00099
00100 maximum_search_radius_limit_ = maximum_search_radius_ - 0.2;
00101
00102
00103
00104 minimum_region_density_ = (1 / leaf_width_) * (1 / leaf_width_) * 2.0 / 3.0;
00105
00106
00107
00108 node_tilde_.param ("input_cloud_topic", input_cloud_topic_, string ("/full_cloud"));
00109
00110 detect_srv_ = node_.advertiseService ("doors_detector", &DoorDetector::detectDoorSrv, this);
00111 detect_cloud_srv_ = node_.advertiseService ("doors_detector_cloud", &DoorDetector::detectDoorCloudSrv, this);
00112
00113 viz_marker_pub_ = node_tilde_.advertise<visualization_msgs::Marker> ("visualization_marker", 100);
00114 door_frames_pub_ = node_tilde_.advertise<PolygonalMap> ("door_frames", 1);
00115 door_regions_pub_ = node_tilde_.advertise<sensor_msgs::PointCloud> ("door_regions", 1);
00116
00117 global_marker_id_ = 1;
00118 }
00119
00120
00121
00123
00124
00125 bool
00126 DoorDetector::detectDoors(const door_msgs::Door& door, sensor_msgs::PointCloud pointcloud, std::vector<door_msgs::Door>& result) const
00127 {
00128 ROS_INFO ("Start detecting doors in a point cloud of size %i", (int)pointcloud.points.size ());
00129
00130
00131 if (door.rot_dir == door_msgs::Door::UNKNOWN){
00132 ROS_ERROR("Door rotation direction not specified");
00133 return false;
00134 }
00135 if (door.hinge == door_msgs::Door::UNKNOWN){
00136 ROS_ERROR("Door hinge side not specified");
00137 return false;
00138 }
00139
00140 Time ts = Time::now();
00141 Duration duration;
00142 Duration timeout = Duration().fromSec(5.0);
00143
00144
00145 if (!tf_.waitForTransform(parameter_frame_, pointcloud.header.frame_id, pointcloud.header.stamp, timeout)){
00146 ROS_ERROR ("Could not transform point cloud from frame '%s' to frame '%s' at time %f.",
00147 pointcloud.header.frame_id.c_str (), parameter_frame_.c_str (), pointcloud.header.stamp.toSec());
00148 return false;
00149 }
00150 tf_.transformPointCloud (parameter_frame_, pointcloud, pointcloud);
00151 ROS_INFO("Pointcloud transformed to parameter frame");
00152
00153
00154 Door door_tr;
00155 if (!transformTo(tf_, parameter_frame_, door, door_tr, fixed_frame_)){
00156 ROS_ERROR ("Could not transform door message from '%s' to '%s' at time %f.",
00157 door.header.frame_id.c_str (), parameter_frame_.c_str (), door.header.stamp.toSec());
00158 return false;
00159 }
00160 ROS_INFO("door message transformed to parameter frame");
00161
00162
00163 geometry_msgs::PointStamped viewpoint_cloud_;
00164 getCloudViewPoint (parameter_frame_, viewpoint_cloud_, &tf_);
00165
00166
00167
00168 int nr_p = 0;
00169 vector<int> indices_in_bounds (pointcloud.points.size ());
00170 for (unsigned int i = 0; i < pointcloud.points.size (); i++)
00171 {
00172 double dist = cloud_geometry::distances::pointToPointDistanceSqr (viewpoint_cloud_.point, pointcloud.points[i]);
00173 if (dist < maximum_search_radius_ && pointcloud.points[i].z > minimum_z_ && pointcloud.points[i].z < maximum_z_)
00174 indices_in_bounds[nr_p++] = i;
00175 }
00176 indices_in_bounds.resize (nr_p);
00177
00178
00179 sensor_msgs::PointCloud cloud_down;
00180 vector<cloud_geometry::Leaf> leaves;
00181 try
00182 {
00183 geometry_msgs::Point leaf_width_xyz;
00184 leaf_width_xyz.x = leaf_width_xyz.y = leaf_width_xyz.z = leaf_width_;
00185 cloud_geometry::downsamplePointCloud (pointcloud, indices_in_bounds, cloud_down, leaf_width_xyz, leaves, -1);
00186 ROS_INFO ("Number of points after downsampling with a leaf of size [%f,%f,%f]: %d.", leaf_width_xyz.x, leaf_width_xyz.y, leaf_width_xyz.z, (int)cloud_down.points.size ());
00187 }
00188 catch (std::bad_alloc)
00189 {
00190
00191 return (false);
00192 }
00193 leaves.resize (0);
00194
00195 #if DEBUG_FAILURES
00196 sendMarker (viewpoint_cloud_.point.x, viewpoint_cloud_.point.y, viewpoint_cloud_.point.z, parameter_frame_, viz_marker_pub_, global_marker_id_, 0, 0, 0);
00197 #endif
00198
00199 estimatePointNormals (pointcloud, indices_in_bounds, cloud_down, k_search_, viewpoint_cloud_);
00200
00201
00202 vector<int> indices_xy;
00203 cloud_geometry::getPointIndicesAxisPerpendicularNormals (cloud_down, 0, 1, 2, normal_angle_tolerance_, z_axis_, indices_xy);
00204
00205
00206 vector<vector<int> > clusters;
00207 findClusters (cloud_down, indices_xy, euclidean_cluster_distance_tolerance_, clusters, 0, 1, 2,
00208 euclidean_cluster_angle_tolerance_, euclidean_cluster_min_pts_);
00209 sort (clusters.begin (), clusters.end (), compareRegions);
00210 reverse (clusters.begin (), clusters.end ());
00211
00212 PolygonalMap pmap_;
00213 if (clusters.size () == 0)
00214 {
00215 ROS_ERROR ("did not find a door");
00216 door_frames_pub_.publish (pmap_);
00217 return (false);
00218 }
00219
00220
00221 sensor_msgs::PointCloud cloud_regions;
00222 cloud_regions.channels.resize (1);
00223 cloud_regions.channels[0].name = "rgb";
00224 cloud_regions.header = cloud_down.header;
00225 cloud_regions.points.resize (0);
00226 cloud_regions.channels[0].values.resize (0);
00227 for (unsigned int cc = 0; cc < clusters.size (); cc++)
00228 {
00229 float r = rand () / (RAND_MAX + 1.0);
00230 float g = rand () / (RAND_MAX + 1.0);
00231 float b = rand () / (RAND_MAX + 1.0);
00232 for (unsigned int j = 0; j < clusters[cc].size (); j++)
00233 {
00234 cloud_regions.points.push_back (cloud_down.points[clusters[cc][j]]);
00235 cloud_regions.channels[0].values.push_back (getRGB (r, g, b));
00236 }
00237 }
00238 door_regions_pub_.publish (cloud_regions);
00239
00240
00241 vector<vector<double> > coeff (clusters.size ());
00242 pmap_.header = pointcloud.header;
00243 pmap_.polygons.resize (clusters.size ());
00244
00245 ROS_INFO (" - Process all clusters (%i)", (int)clusters.size ());
00246 vector<double> goodness_factor (clusters.size());
00247
00248 #pragma omp parallel for schedule(dynamic)
00249
00250 for (int cc = 0; cc < (int)clusters.size (); cc++)
00251 {
00252 bool bad_candidate = false;
00253
00254 goodness_factor[cc] = 1;
00255
00256
00257 vector<int> inliers;
00258 if (!fitSACPlane (cloud_down, clusters[cc], inliers, coeff[cc], viewpoint_cloud_, sac_distance_threshold_, euclidean_cluster_min_pts_))
00259 {
00260 goodness_factor[cc] = 0;
00261 ROS_DEBUG ("R: Could not find planar model for cluster %d (%d hull points, %d points).", cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size ());
00262 continue;
00263 }
00264
00265 if (inliers.size () == 0)
00266 {
00267 ROS_DEBUG ("R: Could not find planar model for cluster %d (%d hull points, %d points).", cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size ());
00268 goodness_factor[cc] = 0;
00269 continue;
00270 }
00271
00272
00273 cloud_geometry::areas::convexHull2D (cloud_down, inliers, coeff[cc], pmap_.polygons[cc]);
00274
00275
00276 for (int i = 0; i < (int)pmap_.polygons[cc].points.size (); i++)
00277 {
00278 double dist = cloud_geometry::distances::pointToPointDistanceSqr (viewpoint_cloud_.point, pmap_.polygons[cc].points[i]);
00279 if (dist < maximum_search_radius_limit_)
00280 continue;
00281
00282 goodness_factor[cc] = 0;
00283 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because the bounds (distance = %g) are near the edges of the cloud!",
00284 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), dist);
00285 bad_candidate = true;
00286 break;
00287 }
00288 if (bad_candidate) continue;
00289
00290
00291
00292 for (int i = 0; i < (int)pmap_.polygons[cc].points.size (); i++)
00293 {
00294
00295 double angle = cloud_geometry::angles::getAngle2D (pmap_.polygons[cc].points[i].x, pmap_.polygons[cc].points[i].y);
00296 if (angle > M_PI / 2.0) angle -= 2*M_PI;
00297 if (fabs (angle) < maximum_scan_angle_limit_)
00298 continue;
00299
00300 goodness_factor[cc] = 0;
00301 #if DEBUG_FAILURES
00302 geometry_msgs::Point32 centroid;
00303 cloud_geometry::nearest::computeCentroid (&pmap_.polygons[cc], centroid);
00304 sendMarker (centroid.x, centroid.y, centroid.z, parameter_frame_, viz_marker_pub_, global_marker_id_, 255, 0, 0);
00305 #endif
00306 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because the bounds (angle = %g, %d, <%g,%g>) are near the edges of the cloud!",
00307 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), angles::to_degrees (angle), i, pmap_.polygons[cc].points[i].x, pmap_.polygons[cc].points[i].y);
00308 bad_candidate = true;
00309 break;
00310 }
00311 if (bad_candidate) continue;
00312
00313
00314 geometry_msgs::Point32 min_p, max_p;
00315 cloud_geometry::statistics::getMinMax (pmap_.polygons[cc], min_p, max_p);
00316
00317
00318 if (min_p.z > door_min_z_)
00319 {
00320 goodness_factor[cc] = 0;
00321 #if DEBUG_FAILURES
00322 geometry_msgs::Point32 centroid;
00323 cloud_geometry::nearest::computeCentroid (&pmap_.polygons[cc], centroid);
00324 sendMarker (centroid.x, centroid.y, centroid.z, parameter_frame_, viz_marker_pub_, global_marker_id_, 0, 255, 0);
00325 #endif
00326 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because min.Z (%g) > than (%g)!",
00327 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), min_p.z, door_min_z_);
00328 continue;
00329 }
00330
00331
00332 double height_df1 = 0.0, height_df2 = 0.0;
00333 if (!checkDoorEdges (pmap_.polygons[cc], z_axis_, rectangle_constrain_edge_height_, rectangle_constrain_edge_angle_,
00334 height_df1, height_df2))
00335 {
00336 goodness_factor[cc] = 0;
00337 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because the length of the door edges (%g / %g) < than %g!",
00338 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), height_df1, height_df2, rectangle_constrain_edge_height_);
00339 continue;
00340 }
00341
00342
00343 double door_frame = sqrt ( (max_p.x - min_p.x) * (max_p.x - min_p.x) + (max_p.y - min_p.y) * (max_p.y - min_p.y) );
00344 double door_height = fabs (max_p.z - min_p.z);
00345
00346 if (door_frame < door_min_width_ || door_height < door_min_height_ || door_frame > door_max_width_ || door_height > door_max_height_)
00347 {
00348 goodness_factor[cc] = 0;
00349 #if DEBUG_FAILURES
00350 geometry_msgs::Point32 centroid;
00351 cloud_geometry::nearest::computeCentroid (&pmap_.polygons[cc], centroid);
00352 sendMarker (centroid.x, centroid.y, centroid.z, parameter_frame_, viz_marker_pub_, global_marker_id_, 0, 0, 255);
00353 #endif
00354 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because of bad width/height (%g < %g < %g / %g < %g < %g).",
00355 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), door_min_width_, door_frame, door_min_height_, door_max_width_, door_height, door_max_height_);
00356 continue;
00357 }
00358 double area = cloud_geometry::areas::compute2DPolygonalArea (pmap_.polygons[cc], coeff[cc]);
00359
00360 double density = (double)inliers.size () / area;
00361
00362
00363 if (density < minimum_region_density_)
00364 {
00365 goodness_factor[cc] = 0;
00366 #if DEBUG_FAILURES
00367 geometry_msgs::Point32 centroid;
00368 cloud_geometry::nearest::computeCentroid (&pmap_.polygons[cc], centroid);
00369 sendMarker (centroid.x, centroid.y, centroid.z, parameter_frame_, viz_marker_pub_, global_marker_id_, 255, 255, 255);
00370 #endif
00371 ROS_DEBUG ("R: Door candidate (%d, %d hull points, %d points) rejected because the average point density (%g) < than %g.",
00372 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), density, minimum_region_density_);
00373 continue;
00374 }
00375 goodness_factor[cc] *= (area / (door_frame * door_height));
00376
00377 ROS_INFO ("A: Door candidate (%d, %d hull points, %d points) accepted with: average point density (%g), area (%g), width (%g), height (%g).\n Planar coefficients: [%g %g %g %g]",
00378 cc, (int)pmap_.polygons[cc].points.size (), (int)inliers.size (), density, area, door_frame, door_height, coeff[cc][0], coeff[cc][1], coeff[cc][2], coeff[cc][3]);
00379
00380
00381 }
00382
00383
00384
00385 double similar_door_eps = 0.1;
00386 geometry_msgs::Point32 min_pcc, max_pcc, min_pdd, max_pdd;
00387 for (unsigned int cc = 0; cc < clusters.size (); cc++)
00388 {
00389 if (goodness_factor[cc] == 0)
00390 continue;
00391 cloud_geometry::statistics::getMinMax (pmap_.polygons[cc], min_pcc, max_pcc);
00392 double area_cc = cloud_geometry::areas::compute2DPolygonalArea (pmap_.polygons[cc], coeff[cc]);
00393
00394 for (unsigned int dd = cc; dd < clusters.size (); dd++)
00395 {
00396 if (cc == dd || goodness_factor[dd] == 0)
00397 continue;
00398 cloud_geometry::statistics::getMinMax (pmap_.polygons[dd], min_pdd, max_pdd);
00399
00400
00401 if ((fabs (min_pcc.x - min_pdd.x) < similar_door_eps && fabs (min_pcc.y - min_pdd.y) < similar_door_eps) ||
00402 (fabs (max_pcc.x - max_pdd.x) < similar_door_eps && fabs (max_pcc.y - max_pdd.y) < similar_door_eps)
00403 )
00404 {
00405
00406 double angle = acos ( cloud_geometry::dot (coeff[cc], coeff[dd]) );
00407 if ( (angle < normal_angle_tolerance_) || ( (M_PI - angle) < normal_angle_tolerance_ ) )
00408 {
00409
00410 double area_dd = cloud_geometry::areas::compute2DPolygonalArea (pmap_.polygons[dd], coeff[dd]);
00411 if (area_dd < area_cc)
00412 goodness_factor[dd] = 0;
00413 else
00414 goodness_factor[cc] = 0;
00415 }
00416 }
00417 }
00418 }
00419
00420
00421 int doors_found_cnt = 0;
00422 for (int cc = 0; cc < (int)clusters.size (); cc++)
00423 {
00424 if (goodness_factor[cc] != 0)
00425 doors_found_cnt++;
00426 else
00427 pmap_.polygons[cc].points.resize (0);
00428 }
00429
00430 ROS_INFO (" - Found %d / %d potential door candidates.", doors_found_cnt, (int)clusters.size ());
00431 result.resize(doors_found_cnt);
00432
00433
00434 int nr_d = 0;
00435 geometry_msgs::Point32 min_p, max_p;
00436 for (int cc = 0; cc < (int)clusters.size (); cc++)
00437 {
00438 if (goodness_factor[cc] == 0)
00439 continue;
00440
00441
00442 result[nr_d] = door_tr;
00443
00444
00445 result[nr_d].header.stamp = pointcloud.header.stamp;
00446
00447
00448 result[nr_d].weight = goodness_factor[cc];
00449
00450
00451 cloud_geometry::statistics::getLargestXYPoints (pmap_.polygons[cc], min_p, max_p);
00452
00453
00454
00455 double d_min_p = distToHinge(result[nr_d], min_p);
00456 double d_max_p = distToHinge(result[nr_d], max_p);
00457 if ( (d_min_p < d_max_p && result[nr_d].hinge == Door::HINGE_P1) || (d_min_p >= d_max_p && result[nr_d].hinge == Door::HINGE_P2) ){
00458 result[nr_d].door_p1 = min_p;
00459 result[nr_d].door_p2 = max_p;
00460 result[nr_d].door_p2.z = min_p.z;
00461 }
00462 else{
00463 result[nr_d].door_p1 = max_p;
00464 result[nr_d].door_p2 = min_p;
00465 result[nr_d].door_p2.z = min_p.z;
00466 }
00467 double d_hinge = fmax(0.001, fmin(d_min_p, d_max_p));
00468 ROS_INFO("Dist from hinge is %f", d_hinge);
00469 if (d_hinge > max_dist_from_prior_){
00470 result[nr_d].weight = 0;
00471 continue;
00472 }
00473 result[nr_d].weight /= d_hinge;
00474
00475
00476
00477
00478 if (fabs(getDoorAngle(result[nr_d])) > (M_PI/2.0 + M_PI/10.0)){
00479 result[nr_d].weight = 0;
00480 continue;
00481 }
00482
00483
00484 geometry_msgs::Point32 frame_vec;
00485 double door_width = cloud_geometry::distances::pointToPointXYDistance( result[nr_d].door_p1, result[nr_d].door_p2);
00486 double frame_width = cloud_geometry::distances::pointToPointXYDistance( result[nr_d].frame_p1, result[nr_d].frame_p2);
00487 frame_vec.x = (result[nr_d].frame_p1.x - result[nr_d].frame_p2.x)*door_width/frame_width;
00488 frame_vec.y = (result[nr_d].frame_p1.y - result[nr_d].frame_p2.y)*door_width/frame_width;
00489 if (result[nr_d].hinge == Door::HINGE_P1){
00490 result[nr_d].frame_p1 = result[nr_d].door_p1;
00491 result[nr_d].frame_p2 = result[nr_d].door_p1;
00492 result[nr_d].frame_p2.x = result[nr_d].frame_p2.x - frame_vec.x;
00493 result[nr_d].frame_p2.y = result[nr_d].frame_p2.y - frame_vec.y;
00494 }
00495 else{
00496 result[nr_d].frame_p2 = result[nr_d].door_p2;
00497 result[nr_d].frame_p1 = result[nr_d].door_p2;
00498 result[nr_d].frame_p1.x = result[nr_d].frame_p1.x + frame_vec.x;
00499 result[nr_d].frame_p1.y = result[nr_d].frame_p1.y + frame_vec.y;
00500 }
00501
00502
00503 cloud_geometry::statistics::getMinMax (pmap_.polygons[cc], min_p, max_p);
00504 result[nr_d].height = fabs (max_p.z - min_p.z);
00505
00506
00507 double angle = getDoorAngle(result[nr_d]);
00508 ROS_INFO("Door angle relative to frame is %f [deg]", angle*180.0/M_PI);
00509 if (fabs(angle) > 10.0*M_PI/180.0)
00510 result[nr_d].latch_state = Door::UNLATCHED;
00511 else
00512 result[nr_d].latch_state = Door::LATCHED;
00513
00514
00515 if (!transformTo(tf_, fixed_frame_, result[nr_d], result[nr_d], fixed_frame_)){
00516 ROS_ERROR ("could not tranform door from '%s' to '%s' at time %f",
00517 result[nr_d].header.frame_id.c_str(), fixed_frame_.c_str(), result[nr_d].header.stamp.toSec());
00518 return false;
00519 }
00520
00521 nr_d++;
00522 }
00523
00524
00525 if (nr_d == 0)
00526 {
00527 ROS_ERROR ("did not find a door");
00528 door_frames_pub_.publish (pmap_);
00529 return (false);
00530 }
00531
00532
00533 sort (result.begin (), result.end (), compareDoorsWeight);
00534 reverse (result.begin (), result.end ());
00535
00536 duration = ros::Time::now () - ts;
00537 ROS_INFO ("Door(s) found and ordered by weight. Result in frame %s", result[0].header.frame_id.c_str ());
00538 for (int cd = 0; cd < nr_d; cd++)
00539 {
00540 ROS_INFO (" %d -> P1 = [%g, %g, %g]. P2 = [%g, %g, %g]. Width = %g. Height = %g. Weight = %g.", cd,
00541 result[cd].door_p1.x, result[cd].door_p1.y, result[cd].door_p1.z, result[cd].door_p2.x, result[cd].door_p2.y, result[cd].door_p2.z,
00542 sqrt ((result[cd].door_p1.x - result[cd].door_p2.x) *
00543 (result[cd].door_p1.x - result[cd].door_p2.x) +
00544 (result[cd].door_p1.y - result[cd].door_p2.y) *
00545 (result[cd].door_p1.y - result[cd].door_p2.y)
00546 ), result[cd].height, result[cd].weight);
00547 }
00548 ROS_INFO (" Total time: %g.", duration.toSec ());
00549
00550 door_frames_pub_.publish (pmap_);
00551
00552 ROS_INFO ("Finished detecting door.");
00553
00554 return true;
00555 }
00556
00557
00558
00559
00561
00562
00563 bool DoorDetector::detectDoorSrv (door_handle_detector::DoorsDetector::Request &req,
00564 door_handle_detector::DoorsDetector::Response &resp)
00565 {
00566 ROS_INFO("Transforming door message to fixed frame");
00567 Door door_tr;
00568 if (!transformTo(tf_, fixed_frame_, req.door, door_tr, fixed_frame_)){
00569 ROS_ERROR ("Could not transform door message from '%s' to '%s' at time %f.",
00570 req.door.header.frame_id.c_str (), fixed_frame_.c_str (), req.door.header.stamp.toSec());
00571 return false;
00572 }
00573
00574
00575 ROS_INFO("Door detection waiting for pointcloud to come in on topic %s", input_cloud_topic_.c_str());
00576
00577 num_clouds_received_ = 0;
00578 ros::Subscriber cloud_sub = node_.subscribe(input_cloud_topic_, 1, &DoorDetector::cloud_cb, this);
00579 ros::Duration tictoc = ros::Duration ().fromSec (0.1);
00580 while ((int)num_clouds_received_ < 1)
00581 tictoc.sleep ();
00582 cloud_sub.shutdown();
00583
00584 return detectDoors(door_tr, pointcloud_, resp.doors);
00585 }
00586
00587
00589
00590
00591 bool DoorDetector::detectDoorCloudSrv (door_handle_detector::DoorsDetectorCloud::Request &req,
00592 door_handle_detector::DoorsDetectorCloud::Response &resp)
00593 {
00594 return detectDoors(req.door, req.cloud, resp.doors);
00595 }
00596
00597
00598
00600
00601
00602 void DoorDetector::cloud_cb (const sensor_msgs::PointCloudConstPtr& cloud)
00603 {
00604 pointcloud_ = *cloud;
00605 ROS_INFO ("Received %d data points in frame %s with %d channels (%s).", (int)pointcloud_.points.size (), pointcloud_.header.frame_id.c_str (),
00606 (int)pointcloud_.channels.size (), cloud_geometry::getAvailableChannels (pointcloud_).c_str ());
00607 num_clouds_received_++;
00608 }
00609
00610
00611 double DoorDetector::distToHinge(const door_msgs::Door& door, geometry_msgs::Point32& pnt) const
00612 {
00613 double dist=-1;
00614 if (door.hinge == Door::HINGE_P1)
00615 dist = cloud_geometry::distances::pointToPointXYDistance(door.frame_p1, pnt);
00616 else if (door.hinge == Door::HINGE_P2)
00617 dist = cloud_geometry::distances::pointToPointXYDistance(door.frame_p2, pnt);
00618 else
00619 ROS_ERROR("Hinge side is not specified");
00620 return dist;
00621 }
00622
00623
00624
00625
00626
00627
00628 int main (int argc, char** argv)
00629 {
00630 ros::init (argc, argv, "doors_detector_node");
00631
00632 door_handle_detector::DoorDetector p;
00633
00634 ros::MultiThreadedSpinner s(2);
00635 s.spin();
00636
00637
00638 return (0);
00639 }
00640
00641