int main(int argc, char **argv) {
ros::init(argc, argv, APP_NAME);
ros::NodeHandle nh;
cv::namedWindow(APP_NAME);
cv::startWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("/video", 1, imageCallback);
ros::ServiceServer addColor_service = nh.advertiseService("addColor", addColor);
ros::ServiceServer addPercept_service = nh.advertiseService("addPercept", addPercept);
ros::Publisher percepts_pub = nh.advertise<obj_rec::Percepts>("/percepts", 100);
// Color table management
colorTable.setColorClassSpecifics( 0, cv::Scalar(0,0,0), false);
for( int i = 1; i < 7; i++) {
colorTable.setColorClassSpecifics( i, getColorFromId(i), true);
}
colorTable.setColorClassSpecifics( 7, cv::Scalar(255,255,255), false);
// The main recognition loop
ros::Rate loop_rate(100);
while (ros::ok()) {
if( 0 == frame.empty()) {
boost::mutex::scoped_lock lock(data_locker);
cv::Mat image, seg;
frame.copyTo(image);
frame.copyTo(seg);
// cv::rectangle(seg,cv::Point(0,0), cv::Point(seg.cols, seg.rows), cv::Scalar(0,0,0), -1);
IplImage _lastImage = IplImage(image);
IplImage * lastImage = &_lastImage;
IplImage _segImage = IplImage(seg);
IplImage * segImage = &_segImage;
colorTable.segment(lastImage, segImage, SEG_X, SEG_Y); // segment using the current CT
// Recognition
std::list < Blob > blobs;
blobs = Blob::extractBlobs( lastImage, colorTable, MERGING_DISTANCE_X, MERGING_DISTANCE_Y);
Blob::drawBlobs( colorTable, blobs, segImage, MIN_AREA);
objs = Object::extractObjects( blobs, MIN_AREA, MERGING_DISTANCE_X *2, MERGING_DISTANCE_Y *2);
recognized.clear();
recognized = Object::recognizeObjects( objs, objects_memory);
obj_rec::Percepts percepts_msg;
RecognizedObjects::iterator feat_it;
for( feat_it = recognized.begin(); feat_it != recognized.end(); feat_it++) {
CvScalar color = getColorFromId(feat_it->first);
Object::drawObjects( feat_it->second, segImage, color);
for( Objects::iterator obj_it = feat_it->second.begin();
obj_it != feat_it->second.end();
obj_it++) {
// compose msg
obj_rec::Percept percept_msg;
percept_msg.id = feat_it->first;
percept_msg.u = obj_it->getAvg().x;
percept_msg.v = obj_it->getAvg().y;
percept_msg.width = obj_it->getBottomRight().x - obj_it->getTopLeft().x;
percept_msg.height = obj_it->getBottomRight().y - obj_it->getTopLeft().y;
percept_msg.area = obj_it->getArea();
percepts_msg.percepts.push_back(percept_msg);
}
}
percepts_msg.header = header;
percepts_pub.publish(percepts_msg);
cv::imshow(APP_NAME, seg);
}
ros::spinOnce();
loop_rate.sleep();
}
cv::destroyWindow(APP_NAME);
}
