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
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 #ifndef _PEOPLE_DETECTION_
00058 #define _PEOPLE_DETECTION_
00059
00060
00061
00062
00063
00064
00065
00066
00067 #include <ros/ros.h>
00068 #include <nodelet/nodelet.h>
00069 #include <ros/package.h>
00070
00071
00072 #include <sensor_msgs/Image.h>
00073 #include <sensor_msgs/PointCloud2.h>
00074 #include <cob_perception_msgs/DetectionArray.h>
00075
00076
00077 #include <cob_people_detection/DetectPeople.h>
00078
00079
00080 #include <message_filters/subscriber.h>
00081 #include <message_filters/synchronizer.h>
00082 #include <message_filters/sync_policies/approximate_time.h>
00083 #include <image_transport/image_transport.h>
00084 #include <image_transport/subscriber_filter.h>
00085
00086
00087 #include <opencv/cv.h>
00088 #include <opencv/highgui.h>
00089 #include <cv_bridge/cv_bridge.h>
00090 #include <sensor_msgs/image_encodings.h>
00091
00092
00093 #include <pcl/point_types.h>
00094 #include <pcl_ros/point_cloud.h>
00095
00096
00097 #include <boost/bind.hpp>
00098 #include <boost/thread/mutex.hpp>
00099
00100
00101 #include "cob_vision_utils/GlobalDefines.h"
00102
00103 #include <sstream>
00104 #include <string>
00105 #include <vector>
00106
00107 #include <pluginlib/class_list_macros.h>
00108
00109 namespace ipa_PeopleDetector
00110 {
00111
00112
00113
00114 class CobPeopleDetectionNodelet: public nodelet::Nodelet
00115 {
00116 protected:
00117
00118 image_transport::ImageTransport* it_;
00119 image_transport::SubscriberFilter people_segmentation_image_sub_;
00120 image_transport::SubscriberFilter color_image_sub_;
00121 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >* sync_input_2_;
00122 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image, sensor_msgs::Image> >
00123 * sync_input_3_;
00124 message_filters::Subscriber<cob_perception_msgs::DetectionArray> face_position_subscriber_;
00125 ros::Publisher face_position_publisher_;
00126 image_transport::Publisher people_detection_image_pub_;
00127 ros::ServiceServer service_server_detect_people_;
00128
00129
00130 ros::NodeHandle node_handle_;
00131
00132 std::vector<cob_perception_msgs::Detection> face_position_accumulator_;
00133 boost::timed_mutex face_position_accumulator_mutex_;
00134 std::vector<std::map<std::string, double> > face_identification_votes_;
00135
00136
00137 bool display_;
00138 bool use_people_segmentation_;
00139 double face_redetection_time_;
00140 double min_segmented_people_ratio_color_;
00141 double min_segmented_people_ratio_range_;
00142 double tracking_range_m_;
00143 double face_identification_score_decay_rate_;
00144 double min_face_identification_score_to_publish_;
00145 bool fall_back_to_unknown_identification_;
00146
00147 public:
00148
00149 CobPeopleDetectionNodelet()
00150 {
00151 it_ = 0;
00152 sync_input_2_ = 0;
00153 sync_input_3_ = 0;
00154 }
00155
00156 ~CobPeopleDetectionNodelet()
00157 {
00158 if (it_ != 0)
00159 delete it_;
00160 if (sync_input_2_ != 0)
00161 delete sync_input_2_;
00162 if (sync_input_3_ != 0)
00163 delete sync_input_3_;
00164 }
00165
00167 void onInit()
00168 {
00169 node_handle_ = getNodeHandle();
00170
00171
00172 std::cout << "\n---------------------------\nPeople Detection Parameters:\n---------------------------\n";
00173 node_handle_.param("display", display_, true);
00174 std::cout << "display = " << display_ << "\n";
00175 node_handle_.param("face_redetection_time", face_redetection_time_, 2.0);
00176 std::cout << "face_redetection_time = " << face_redetection_time_ << "\n";
00177 node_handle_.param("min_segmented_people_ratio_color", min_segmented_people_ratio_color_, 0.7);
00178 std::cout << "min_segmented_people_ratio_color = " << min_segmented_people_ratio_color_ << "\n";
00179 node_handle_.param("min_segmented_people_ratio_range", min_segmented_people_ratio_range_, 0.2);
00180 std::cout << "min_segmented_people_ratio_range = " << min_segmented_people_ratio_range_ << "\n";
00181 node_handle_.param("use_people_segmentation", use_people_segmentation_, true);
00182 std::cout << "use_people_segmentation = " << use_people_segmentation_ << "\n";
00183 node_handle_.param("tracking_range_m", tracking_range_m_, 0.3);
00184 std::cout << "tracking_range_m = " << tracking_range_m_ << "\n";
00185 node_handle_.param("face_identification_score_decay_rate", face_identification_score_decay_rate_, 0.9);
00186 std::cout << "face_identification_score_decay_rate = " << face_identification_score_decay_rate_ << "\n";
00187 node_handle_.param("min_face_identification_score_to_publish", min_face_identification_score_to_publish_, 0.9);
00188 std::cout << "min_face_identification_score_to_publish = " << min_face_identification_score_to_publish_ << "\n";
00189 node_handle_.param("fall_back_to_unknown_identification", fall_back_to_unknown_identification_, true);
00190 std::cout << "fall_back_to_unknown_identification = " << fall_back_to_unknown_identification_ << "\n";
00191
00192
00193 it_ = new image_transport::ImageTransport(node_handle_);
00194 people_segmentation_image_sub_.subscribe(*it_, "people_segmentation_image", 1);
00195 if (display_ == true)
00196 color_image_sub_.subscribe(*it_, "colorimage", 1);
00197 face_position_subscriber_.subscribe(node_handle_, "face_position_array_in", 1);
00198
00199
00200 sensor_msgs::Image::ConstPtr nullPtr;
00201 if (use_people_segmentation_ == true)
00202 {
00203 if (display_ == false)
00204 {
00205 sync_input_2_
00206 = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >(2);
00207 sync_input_2_->connectInput(face_position_subscriber_, people_segmentation_image_sub_);
00208 sync_input_2_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, _2, nullPtr));
00209 }
00210 else
00211 {
00212 sync_input_3_ = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image,
00213 sensor_msgs::Image> >(3);
00214 sync_input_3_->connectInput(face_position_subscriber_, people_segmentation_image_sub_, color_image_sub_);
00215 sync_input_3_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, _2, _3));
00216 }
00217 }
00218 else
00219 {
00220 if (display_ == true)
00221 {
00222 sync_input_2_
00223 = new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<cob_perception_msgs::DetectionArray, sensor_msgs::Image> >(2);
00224 sync_input_2_->connectInput(face_position_subscriber_, color_image_sub_);
00225 sync_input_2_->registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, nullPtr, _2));
00226 }
00227 else
00228 {
00229 face_position_subscriber_.registerCallback(boost::bind(&CobPeopleDetectionNodelet::inputCallback, this, _1, nullPtr, nullPtr));
00230 }
00231 }
00232
00233
00234 service_server_detect_people_ = node_handle_.advertiseService("detect_people", &CobPeopleDetectionNodelet::detectPeopleCallback, this);
00235
00236
00237 face_position_publisher_ = node_handle_.advertise<cob_perception_msgs::DetectionArray>("face_position_array", 1);
00238 people_detection_image_pub_ = it_->advertise("people_detection_image", 1);
00239
00240 std::cout << "CobPeopleDetectionNodelet initialized.\n";
00241
00242
00243 }
00244
00246 unsigned long convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& image_msg, cv_bridge::CvImageConstPtr& image_ptr, cv::Mat& image)
00247 {
00248 try
00249 {
00250 image_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8);
00251 } catch (cv_bridge::Exception& e)
00252 {
00253 ROS_ERROR("PeopleDetection: cv_bridge exception: %s", e.what());
00254 return ipa_Utils::RET_FAILED;
00255 }
00256 image = image_ptr->image;
00257
00258 return ipa_Utils::RET_OK;
00259 }
00260
00267 unsigned long copyDetection(const cob_perception_msgs::Detection& src, cob_perception_msgs::Detection& dest, bool update = false,
00268 unsigned int updateIndex = UINT_MAX)
00269 {
00270
00271 dest.mask.roi.x = src.mask.roi.x;
00272 dest.mask.roi.y = src.mask.roi.y;
00273 dest.mask.roi.width = src.mask.roi.width;
00274 dest.mask.roi.height = src.mask.roi.height;
00275
00276
00277 const geometry_msgs::Point* pointIn = &(src.pose.pose.position);
00278 geometry_msgs::Point* pointOut = &(dest.pose.pose.position);
00279 pointOut->x = pointIn->x;
00280 pointOut->y = pointIn->y;
00281 pointOut->z = pointIn->z;
00282
00283
00284 if (update == true)
00285 {
00286
00287
00288
00289 if (face_identification_votes_[updateIndex].find(src.label) == face_identification_votes_[updateIndex].end())
00290 face_identification_votes_[updateIndex][src.label] = 1.0;
00291 else
00292 face_identification_votes_[updateIndex][src.label] += 1.0;
00293
00294
00295
00296 double max_score = 0;
00297 dest.label = src.label;
00298 for (std::map<std::string, double>::iterator face_identification_votes_it = face_identification_votes_[updateIndex].begin(); face_identification_votes_it
00299 != face_identification_votes_[updateIndex].end(); face_identification_votes_it++)
00300 {
00301 face_identification_votes_it->second *= face_identification_score_decay_rate_;
00302 std::string label = face_identification_votes_it->first;
00303 if (((label != "Unknown" && fall_back_to_unknown_identification_ == false) || fall_back_to_unknown_identification_ == true) && label != "UnknownRange"
00304 && face_identification_votes_it->second > max_score)
00305 {
00306 max_score = face_identification_votes_it->second;
00307 dest.label = label;
00308 }
00309 }
00310
00311
00312 if (face_identification_votes_[updateIndex][dest.label] > face_identification_votes_[updateIndex]["UnknownRange"])
00313 face_identification_votes_[updateIndex]["UnknownRange"] = face_identification_votes_[updateIndex][dest.label];
00314 if (fall_back_to_unknown_identification_ == false)
00315 {
00316 if (face_identification_votes_[updateIndex]["Unknown"] > face_identification_votes_[updateIndex]["UnknownRange"])
00317 face_identification_votes_[updateIndex]["UnknownRange"] = face_identification_votes_[updateIndex]["Unknown"];
00318 }
00319 }
00320 else
00321 dest.label = src.label;
00322
00323 if (dest.label == "UnknownRange")
00324 dest.label = "Unknown";
00325
00326
00327
00328
00329
00330
00331
00332 dest.detector = src.detector;
00333
00334 dest.header.stamp = ros::Time::now();
00335
00336 return ipa_Utils::RET_OK;
00337 }
00338
00339 inline double abs(double x)
00340 {
00341 return ((x < 0) ? -x : x);
00342 }
00343
00347 double computeFacePositionDistance(const cob_perception_msgs::Detection& previous_detection, const cob_perception_msgs::Detection& current_detection)
00348 {
00349 const geometry_msgs::Point* point_1 = &(previous_detection.pose.pose.position);
00350 const geometry_msgs::Point* point_2 = &(current_detection.pose.pose.position);
00351
00352 double dx = abs(point_1->x - point_2->x);
00353 double dy = abs(point_1->y - point_2->y);
00354 double dz = abs(point_1->z - point_2->z);
00355
00356
00357 if (dx > tracking_range_m_ || dy > tracking_range_m_ || dz > tracking_range_m_)
00358 {
00359
00360 return DBL_MAX;
00361 }
00362
00363
00364 double dist = dx * dx + dy * dy + dz * dz;
00365 return dist;
00366 }
00367
00370 unsigned long removeMultipleInstancesOfLabel()
00371 {
00372
00373 for (int i = 0; i < (int)face_position_accumulator_.size(); i++)
00374 {
00375
00376 std::string label = face_position_accumulator_[i].label;
00377
00378
00379 if (label != "Unknown" && label != "No face")
00380 {
00381 for (int j = 0; j < (int)face_position_accumulator_.size(); j++)
00382 {
00383 if (j == i)
00384 continue;
00385
00386 if (face_position_accumulator_[j].label == label)
00387 {
00388 if (display_)
00389 std::cout << "face_identification_votes_[i][" << label << "] = " << face_identification_votes_[i][label] << " face_identification_votes_[j][" << label
00390 << "] = " << face_identification_votes_[j][label] << "\n";
00391
00392
00393 if (face_identification_votes_[i][label] < face_identification_votes_[j][label])
00394 {
00395 face_position_accumulator_[i].label = "Unknown";
00396
00397 if (face_identification_votes_[i][label] > face_identification_votes_[i]["Unknown"])
00398 face_identification_votes_[i]["Unknown"] = face_identification_votes_[i][label];
00399 }
00400 }
00401 }
00402 }
00403 }
00404
00405 return ipa_Utils::RET_OK;
00406 }
00407
00408 unsigned long prepareFacePositionMessage(cob_perception_msgs::DetectionArray& face_position_msg_out)
00409 {
00410
00411 std::vector<cob_perception_msgs::Detection> faces_to_publish;
00412 for (int i = 0; i < (int)face_position_accumulator_.size(); i++)
00413 {
00414 if (display_)
00415 std::cout << "'UnknownRange' score: " << face_identification_votes_[i]["UnknownRange"] << " label '" << face_position_accumulator_[i].label << "' score: "
00416 << face_identification_votes_[i][face_position_accumulator_[i].label] << " - ";
00417 if (face_identification_votes_[i][face_position_accumulator_[i].label] > min_face_identification_score_to_publish_ || face_identification_votes_[i]["UnknownRange"]
00418 > min_face_identification_score_to_publish_)
00419 {
00420 faces_to_publish.push_back(face_position_accumulator_[i]);
00421 if (display_)
00422 std::cout << "published\n";
00423 }
00424 else if (display_)
00425 std::cout << "not published\n";
00426 }
00427 face_position_msg_out.detections = faces_to_publish;
00428
00429
00430
00431
00432
00433
00434 face_position_msg_out.header.stamp = ros::Time::now();
00435
00436 return ipa_Utils::RET_OK;
00437 }
00438
00440 void inputCallback(const cob_perception_msgs::DetectionArray::ConstPtr& face_position_msg_in, const sensor_msgs::Image::ConstPtr& people_segmentation_image_msg,
00441 const sensor_msgs::Image::ConstPtr& color_image_msg)
00442 {
00443
00444 cv_bridge::CvImageConstPtr people_segmentation_image_ptr;
00445 cv::Mat people_segmentation_image;
00446 if (use_people_segmentation_ == true)
00447 convertColorImageMessageToMat(people_segmentation_image_msg, people_segmentation_image_ptr, people_segmentation_image);
00448
00449 if (display_)
00450 std::cout << "detections: " << face_position_msg_in->detections.size() << "\n";
00451
00452
00453 std::stringstream ss;
00454 ss << face_position_msg_in->header.frame_id;
00455 int width, height;
00456 ss >> width;
00457 ss >> height;
00458
00459
00460 ros::Duration timeSpan(face_redetection_time_);
00461 for (int i = 0; i < (int)face_position_accumulator_.size(); i++)
00462 {
00463 if ((ros::Time::now() - face_position_accumulator_[i].header.stamp) > timeSpan)
00464 {
00465 face_position_accumulator_.erase(face_position_accumulator_.begin() + i);
00466 face_identification_votes_.erase(face_identification_votes_.begin() + i);
00467 }
00468 }
00469 if (display_)
00470 std::cout << "Old face positions deleted.\n";
00471
00472
00473 boost::timed_mutex::scoped_timed_lock lock(face_position_accumulator_mutex_, boost::posix_time::milliseconds(2000));
00474 if (lock.owns_lock() == false)
00475 {
00476 ROS_ERROR("cob_people_detection::CobPeopleDetectionNodelet: Mutex was not freed during response time.");
00477 return;
00478 }
00479 if (display_)
00480 std::cout << "Got mutex.\n";
00481
00482
00483 std::vector<int> face_detection_indices;
00484 if (use_people_segmentation_ == true)
00485 {
00486 for (int i = 0; i < (int)face_position_msg_in->detections.size(); i++)
00487 {
00488 const cob_perception_msgs::Detection* const det_in = &(face_position_msg_in->detections[i]);
00489 cv::Rect face;
00490 face.x = det_in->mask.roi.x;
00491 face.y = det_in->mask.roi.y;
00492 face.width = det_in->mask.roi.width;
00493 face.height = det_in->mask.roi.height;
00494
00495 int numberBlackPixels = 0;
00496
00497 for (int v = face.y; v < face.y + face.height; v++)
00498 {
00499 uchar* data = people_segmentation_image.ptr(v);
00500 for (int u = face.x; u < face.x + face.width; u++)
00501 {
00502 int index = 3 * u;
00503 if (data[index] == 0 && data[index + 1] == 0 && data[index + 2] == 0)
00504 numberBlackPixels++;
00505 }
00506 }
00507 int faceArea = face.height * face.width;
00508 double segmentedPeopleRatio = (double)(faceArea - numberBlackPixels) / (double)faceArea;
00509
00510
00511 if ((det_in->detector == "color" && segmentedPeopleRatio < min_segmented_people_ratio_color_) || (det_in->detector == "range" && segmentedPeopleRatio
00512 < min_segmented_people_ratio_range_))
00513 {
00514
00515 if (display_)
00516 std::cout << "False detection\n";
00517 }
00518 else
00519 {
00520
00521 face_detection_indices.push_back(i);
00522 }
00523 }
00524 if (display_)
00525 std::cout << "Verification with people segmentation done.\n";
00526 }
00527 else
00528 {
00529 for (unsigned int i = 0; i < face_position_msg_in->detections.size(); i++)
00530 face_detection_indices.push_back(i);
00531 }
00532
00533
00534 std::map<int, std::map<int, double> > distance_matrix;
00535 std::map<int, std::map<int, double> >::iterator distance_matrix_it;
00536 for (unsigned int previous_det = 0; previous_det < face_position_accumulator_.size(); previous_det++)
00537 {
00538 std::map<int, double> distance_row;
00539 for (unsigned int i = 0; i < face_detection_indices.size(); i++)
00540 distance_row[face_detection_indices[i]] = computeFacePositionDistance(face_position_accumulator_[previous_det],
00541 face_position_msg_in->detections[face_detection_indices[i]]);
00542 distance_matrix[previous_det] = distance_row;
00543 }
00544 if (display_)
00545 std::cout << "Distance matrix.\n";
00546
00547
00548 unsigned int number_matchings = (face_position_accumulator_.size() < face_detection_indices.size()) ? face_position_accumulator_.size() : face_detection_indices.size();
00549 std::vector<bool> current_detection_has_matching(face_detection_indices.size(), false);
00550 for (unsigned int m = 0; m < number_matchings; m++)
00551 {
00552
00553 double min_dist = DBL_MAX;
00554 int previous_min_index, current_min_index;
00555 for (distance_matrix_it = distance_matrix.begin(); distance_matrix_it != distance_matrix.end(); distance_matrix_it++)
00556 {
00557 for (std::map<int, double>::iterator distance_row_it = distance_matrix_it->second.begin(); distance_row_it != distance_matrix_it->second.end(); distance_row_it++)
00558 {
00559 if (distance_row_it->second < min_dist)
00560 {
00561 min_dist = distance_row_it->second;
00562 previous_min_index = distance_matrix_it->first;
00563 current_min_index = distance_row_it->first;
00564 }
00565 }
00566 }
00567
00568
00569
00570
00571 if (min_dist == DBL_MAX)
00572 break;
00573
00574
00575 copyDetection(face_position_msg_in->detections[current_min_index], face_position_accumulator_[previous_min_index], true, previous_min_index);
00576
00577 for (unsigned int i = 0; i < face_detection_indices.size(); i++)
00578 if (face_detection_indices[i] == current_min_index)
00579 current_detection_has_matching[i] = true;
00580
00581
00582 distance_matrix.erase(previous_min_index);
00583 for (distance_matrix_it = distance_matrix.begin(); distance_matrix_it != distance_matrix.end(); distance_matrix_it++)
00584 distance_matrix_it->second.erase(current_min_index);
00585 }
00586 if (display_)
00587 std::cout << "Matches found.\n";
00588
00589
00590 for (unsigned int i = 0; i < face_detection_indices.size(); i++)
00591 {
00592 if (current_detection_has_matching[i] == false)
00593 {
00594 const cob_perception_msgs::Detection* const det_in = &(face_position_msg_in->detections[face_detection_indices[i]]);
00595 if (det_in->detector == "color")
00596 {
00597
00598 if (display_)
00599 std::cout << "\n***** New detection *****\n\n";
00600 cob_perception_msgs::Detection det_out;
00601 copyDetection(*det_in, det_out, false);
00602 det_out.pose.header.frame_id = "head_cam3d_link";
00603 face_position_accumulator_.push_back(det_out);
00604
00605 std::map<std::string, double> new_identification_data;
00606
00607 new_identification_data["UnknownRange"] = 0.0;
00608 new_identification_data[det_in->label] = 1.0;
00609 face_identification_votes_.push_back(new_identification_data);
00610 }
00611 }
00612 }
00613 if (display_)
00614 std::cout << "New detections.\n";
00615
00616
00617 removeMultipleInstancesOfLabel();
00618
00619
00620 cob_perception_msgs::DetectionArray face_position_msg_out;
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641 prepareFacePositionMessage(face_position_msg_out);
00642 face_position_publisher_.publish(face_position_msg_out);
00643
00644
00645 if (display_ == true)
00646 {
00647
00648 cv_bridge::CvImageConstPtr colorImagePtr;
00649 cv::Mat colorImage;
00650 convertColorImageMessageToMat(color_image_msg, colorImagePtr, colorImage);
00651
00652
00653 for (int i = 0; i < (int)face_position_msg_out.detections.size(); i++)
00654 {
00655 cv::Rect face;
00656 cob_perception_msgs::Rect& faceRect = face_position_msg_out.detections[i].mask.roi;
00657 face.x = faceRect.x;
00658 face.width = faceRect.width;
00659 face.y = faceRect.y;
00660 face.height = faceRect.height;
00661
00662 if (face_position_msg_out.detections[i].detector == "range")
00663 cv::rectangle(colorImage, cv::Point(face.x, face.y), cv::Point(face.x + face.width, face.y + face.height), CV_RGB(0, 0, 255), 2, 8, 0);
00664 else
00665 cv::rectangle(colorImage, cv::Point(face.x, face.y), cv::Point(face.x + face.width, face.y + face.height), CV_RGB(0, 255, 0), 2, 8, 0);
00666
00667 if (face_position_msg_out.detections[i].label == "Unknown")
00668
00669 cv::putText(colorImage, "Unknown", cv::Point(face.x, face.y + face.height + 25), cv::FONT_HERSHEY_PLAIN, 2, CV_RGB( 255, 0, 0 ), 2);
00670 else if (face_position_msg_out.detections[i].label == "No face")
00671
00672 cv::putText(colorImage, "No face", cv::Point(face.x, face.y + face.height + 25), cv::FONT_HERSHEY_PLAIN, 2, CV_RGB( 255, 0, 0 ), 2);
00673 else
00674
00675 cv::putText(colorImage, face_position_msg_out.detections[i].label.c_str(), cv::Point(face.x, face.y + face.height + 25), cv::FONT_HERSHEY_PLAIN, 2,
00676 CV_RGB( 0, 255, 0 ), 2);
00677 }
00678
00679 cv_bridge::CvImage cv_ptr;
00680 cv_ptr.image = colorImage;
00681 cv_ptr.encoding = "bgr8";
00682 people_detection_image_pub_.publish(cv_ptr.toImageMsg());
00683 }
00684 }
00685
00686 bool detectPeopleCallback(cob_people_detection::DetectPeople::Request &req, cob_people_detection::DetectPeople::Response &res)
00687 {
00688
00689 boost::timed_mutex::scoped_timed_lock lock(face_position_accumulator_mutex_, boost::posix_time::milliseconds(2000));
00690 if (lock.owns_lock() == false)
00691 {
00692 ROS_ERROR("cob_people_detection::CobPeopleDetectionNodelet: Mutex was not freed during response time.");
00693 return false;
00694 }
00695
00696 prepareFacePositionMessage(res.people_list);
00697
00698
00699 return true;
00700 }
00701 };
00702
00703 }
00704 ;
00705
00706 PLUGINLIB_DECLARE_CLASS(ipa_PeopleDetector, CobPeopleDetectionNodelet, ipa_PeopleDetector::CobPeopleDetectionNodelet, nodelet::Nodelet)
00707 ;
00708
00709 #endif // _PEOPLE_DETECTION_