void ImageBuffer::buf(){
if(bufferList.size() > 0 && 0.1 < computeTimeDiff(bufferList.front().header.stamp, lastImageMsg.header.stamp)){
//add new image to buffer
bufferList.push_front(lastImageMsg);
//remove too old messages from buffer
while(bufferList.size() > 0 && computeTimeDiff(bufferList.back().header.stamp, ros::Time::now()) > IMAGE_BUFFER_TIME_S){
bufferList.pop_back();
}
}
return;
}
void ImageBuffer::imageCallback(const sensor_msgs::Image::ConstPtr &msg){
lastImageMsg = *msg;
// ROS_INFO("New Image");
// ROS_INFO("Time diff to HEAD = %f",computeTimeDiff(bufferList.front().header.stamp, lastImageMsg.header.stamp));
if(bufferList.size()==0 || (bufferList.size() > 0 && 0.0 < computeTimeDiff(bufferList.front().header.stamp, lastImageMsg.header.stamp))){
//add new image to buffer
bufferList.push_front(lastImageMsg);
// ROS_INFO("Size of bufferList= %lu", bufferList.size());
// ROS_INFO("Image TimeStamp = %d", lastImageMsg.header.stamp.sec );
//remove too old messages from buffer
while(bufferList.size() > 0 && computeTimeDiff(bufferList.back().header.stamp, ros::Time::now()) > IMAGE_BUFFER_TIME_S){
bufferList.pop_back();
}
// ROS_INFO("Size of bufferList= %lu", bufferList.size());
}
return;
}
void ImageBuffer::evidenceCommandCallback(const image_buffer::EvidenceCommand::ConstPtr &msg){
ras_msgs::RAS_Evidence evidence;
evidence.stamp = msg->header.stamp;
evidence.group_number = 2;
evidence.object_id = msg->object_name.data;
//find best picture
if (bufferList.empty()) {
// ROS_INFO("BL Empty");
evidencePub.publish(evidence);
return;
}
double smallestDiff = 100.0; //10000000000000.0;
std::list<sensor_msgs::Image>::iterator bestIt;
// ROS_INFO("2 Size of bufferList= %lu", bufferList.size());
bestIt = bufferList.end();
for(std::list<sensor_msgs::Image>::iterator it = bufferList.begin(); it != bufferList.end(); ++it){
double currDiff = computeTimeDiff(it->header.stamp, msg->header.stamp);
if(currDiff < smallestDiff){
smallestDiff = currDiff;
bestIt = it;
// ROS_INFO("BestIt");
}
else {
//break;
}
}
// ROS_INFO("Should Publish now");
if(bestIt != bufferList.end()){
evidence.image_evidence = *bestIt;
// ROS_INFO("Publishing");
evidencePub.publish(evidence);
}
return;
}
void ImageBuffer::evidenceCommandCallback(const image_buffer::EvidenceCommand::ConstPtr &msg){
ras_msgs::RAS_Evidence evidence;
evidence.stamp = msg->header.stamp;
evidence.group_number = 2;
evidence.object_id = msg->object_name.data;
//find best picture
double smallestDiff = 100;
std::list<sensor_msgs::Image>::iterator bestIt;
for(std::list<sensor_msgs::Image>::iterator it = bufferList.begin(), end = bufferList.end(); it != end; it++){
double currDiff = computeTimeDiff(it->header.stamp, msg->header.stamp);
if(currDiff < smallestDiff){
smallestDiff = currDiff;
bestIt = it;
}
//else break??
}
evidence.image_evidence = *bestIt;
evidencePub.publish(evidence);
return;
}
