diff --git a/robosherlock/launch/rs.launch b/robosherlock/launch/rs.launch index f8546235..2a19b33d 100644 --- a/robosherlock/launch/rs.launch +++ b/robosherlock/launch/rs.launch @@ -10,7 +10,9 @@ - + + + @@ -30,6 +32,7 @@ launch-prefix="$(arg launch-prefix)"> + diff --git a/robosherlock/src/io/include/robosherlock/io/MongoDBBridge.h b/robosherlock/src/io/include/robosherlock/io/MongoDBBridge.h index db8cdd30..b3260e72 100644 --- a/robosherlock/src/io/include/robosherlock/io/MongoDBBridge.h +++ b/robosherlock/src/io/include/robosherlock/io/MongoDBBridge.h @@ -33,6 +33,7 @@ class MongoDBBridge : public CamInterface std::vector frames; size_t actualFrame; + size_t index; bool continual; bool loop; double playbackSpeed; diff --git a/robosherlock/src/io/src/MongoDBBridge.cpp b/robosherlock/src/io/src/MongoDBBridge.cpp index 8f8169ea..facf87dc 100644 --- a/robosherlock/src/io/src/MongoDBBridge.cpp +++ b/robosherlock/src/io/src/MongoDBBridge.cpp @@ -35,7 +35,7 @@ MongoDBBridge::MongoDBBridge(const boost::property_tree::ptree &pt) : CamInterfa storage = rs::Storage(host, db); actualFrame = 0; - + index=0; storage.getScenes(frames); if(continual) { @@ -81,79 +81,114 @@ void MongoDBBridge::readConfig(const boost::property_tree::ptree &pt) bool MongoDBBridge::setData(uima::CAS &tcas, uint64_t timestamp) { - MEASURE_TIME; - const bool isNextFrame = timestamp == std::numeric_limits::max(); - - if(actualFrame >= frames.size()) { - if(continual) { - storage.getScenes(frames); - if(actualFrame >= frames.size()) { - return false; - } - } - else if(loop) { - actualFrame = 0; - } - else { - outInfo("last frame. shuting down."); - cv::waitKey(); - ros::shutdown(); - return false; - } - } - if(isNextFrame) { - outInfo("default behaviour"); - timestamp = frames[actualFrame]; - outInfo("setting data from frame: " << actualFrame << " (" << timestamp << ")"); - } - else if(timestamp < frames.size()) { - timestamp = frames[timestamp]; - outInfo("setting data from frame with timestamp: (" << timestamp << ")"); - } - else if(timestamp >= frames.size()) { - if(std::find(frames.begin(), frames.end(), timestamp) == frames.end()) { - outWarn("timestamp asked for is not in database"); - return false; - } - } - - if(!storage.loadScene(*tcas.getBaseCas(), timestamp)) { - if(timestamp == 0) { - outInfo("loading frame failed. shuting down."); - ros::shutdown(); - return false; - } - else { - outInfo("No frame with that timestamp"); - return false; - } - } - - - if(playbackSpeed > 0.0 && isNextFrame) { - if(lastTimestamp > timestamp) { - lastTimestamp = timestamp; - simTimeLast = frames[actualFrame]; - lastRun = ros::Time::now().toNSec(); - } - - uint64_t now = ros::Time::now().toNSec(); - uint64_t simTime = (uint64_t)((now - lastRun) * playbackSpeed) + simTimeLast; - if(simTime <= timestamp) { - uint64_t sleepTime = (timestamp - simTime) / playbackSpeed; - outDebug("waiting for " << sleepTime / 1000000.0 << " ms."); - std::this_thread::sleep_for(std::chrono::nanoseconds(sleepTime)); - } - - now = ros::Time::now().toNSec(); - simTimeLast = (uint64_t)((now - lastRun) * playbackSpeed) + simTimeLast; - lastRun = now; - } - - - ++actualFrame; - if(!continual && !loop && actualFrame == frames.size()) { - _newData = false; - } + MEASURE_TIME; + ros::NodeHandle n_("~"); + + double_t time_stamp; // Variable which store the value of queued time stamp + if(n_.getParam("ts",time_stamp) ){ + n_.deleteParam("ts"); // deleting the "ts" variable. + auto it= std::find(frames.begin(), frames.end(), time_stamp); + if(it== frames.end()){ + if(!continual && !loop) { + _newData = false; + } + outWarn("Required Timestamp is not in the frame."); + return false; + } + else{ + index= it- frames.begin(); // getting index value where time_stamp present + outInfo("index is="<::max(); + if(actualFrame >= frames.size()) { + if(continual) { + storage.getScenes(frames); + if(actualFrame >= frames.size()) { + return false; + } + } + else if(loop) { + actualFrame = 0; + } + else { + outInfo("last frame. shuting down."); + cv::waitKey(); + ros::shutdown(); + return false; + } + } + if(isNextFrame) { + outInfo("default behaviour"); + timestamp = frames[actualFrame]; + outInfo("setting data from frame: " << actualFrame << " (" << timestamp << ")"); + } + else if(timestamp < frames.size()) { + timestamp = frames[timestamp]; + outInfo("setting data from frame with timestamp: (" << timestamp << ")"); + } + else if(timestamp >= frames.size()) { + if(std::find(frames.begin(), frames.end(), timestamp) == frames.end()) { + outWarn("timestamp asked for is not in database"); + return false; + } + } + + if(!storage.loadScene(*tcas.getBaseCas(), timestamp)) { + if(timestamp == 0) { + outInfo("loading frame failed. shuting down."); + ros::shutdown(); + return false; + } + else { + outInfo("No frame with that timestamp"); + return false; + } + } + + + if(playbackSpeed > 0.0 && isNextFrame) { + if(lastTimestamp > timestamp) { + lastTimestamp = timestamp; + simTimeLast = frames[actualFrame]; + lastRun = ros::Time::now().toNSec(); + } + + uint64_t now = ros::Time::now().toNSec(); + uint64_t simTime = (uint64_t)((now - lastRun) * playbackSpeed) + simTimeLast; + if(simTime <= timestamp) { + uint64_t sleepTime = (timestamp - simTime) / playbackSpeed; + outDebug("waiting for " << sleepTime / 1000000.0 << " ms."); + std::this_thread::sleep_for(std::chrono::nanoseconds(sleepTime)); + } + + now = ros::Time::now().toNSec(); + simTimeLast = (uint64_t)((now - lastRun) * playbackSpeed) + simTimeLast; + lastRun = now; + } + + + ++actualFrame; + if(!continual && !loop && actualFrame == frames.size()) { + _newData = false; + } + } return true; }