void BlobDetectorNode::update() {
// if more than 2 seconds have passed and no messages have been received,
// stop sending
if((ros::Time::now()-t_depth).toSec()>1.0) {
return;
}
//convert image to openCV image
cv_bridge::CvImagePtr cv_ptr = convertImage();
//normalize image, remove max values
// cv_ptr = normalize(cv_ptr);
//if(1) return;
//detect blobs in image
vector<KeyPoint> points = detectBlobs(cv_ptr);
//pick the closest blob
KeyPoint kp = getClosestBlob(points,cv_ptr);
ROS_INFO_STREAM("x cor: " << kp.pt.x << " distance: " << kp.pt.y);
// calculate kinematics and send twist to robot simulation node
geometry_msgs::Twist twist_msg;
twist_msg.linear.x = kp.pt.x;
twist_msg.linear.y = 0.0;
twist_msg.linear.z = kp.pt.y;
twist_msg.angular.x = 0.0;
twist_msg.angular.y = 0.0;
twist_msg.angular.z = 0.0;
blob_publisher.publish(twist_msg);
}
void ofxBlobTracker::track(vector<ofVec3f> &blobs) {
blobSmoother.setSmoothness(smoothing);
smoothedBlobs.clear();
untouchLastBlobs();
for(int i = 0; i < blobs.size() && i < MAX_NUM_BLOBS; i++) {
// skip any NaN's (causes ofxTuioWrapper to run out of memory)
if(blobs[i].x!=blobs[i].x || blobs[i].y!=blobs[i].y) {
continue;
}
ofxBlob *blob = getClosestBlob(blobs[i]);
// new blob!
if(blob==NULL || blob->distance(blobs[i])>minTrackDistance) {
ofxBlob *b = newBlob(blobs[i]);
updateKalman(b->id, b);
notifyAllListeners(*b, b->id, ofxBlobTracker_entered);
// updated blob
} else {
blob = updateKalman(blob->id, blob);
updateBlob(blob->id, &blobs[i]);
notifyAllListeners(*blob, blob->id, ofxBlobTracker_moved);
}
}
// dead blob
// delete any untouched blobs
for(int i = 0; i < lastBlobs.size(); i++) {
if(lastBlobs[i]->touched==false) {
notifyAllListeners(*lastBlobs[i], lastBlobs[i]->id, ofxBlobTracker_exited);
delete lastBlobs[i];
lastBlobs.erase(lastBlobs.begin()+i);
i--;
}
}
}
