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;
}