From b35d58ad67cb5e63f30b3e90647b0868d3fe0326 Mon Sep 17 00:00:00 2001 From: AB <55253316+abmuslim@users.noreply.github.com> Date: Sun, 20 Mar 2022 20:34:39 +0100 Subject: [PATCH 1/5] Update MongoDBBridge.cpp --- robosherlock/src/io/src/MongoDBBridge.cpp | 176 +++++++++++++--------- 1 file changed, 106 insertions(+), 70 deletions(-) diff --git a/robosherlock/src/io/src/MongoDBBridge.cpp b/robosherlock/src/io/src/MongoDBBridge.cpp index 8f8169ea..857db08a 100644 --- a/robosherlock/src/io/src/MongoDBBridge.cpp +++ b/robosherlock/src/io/src/MongoDBBridge.cpp @@ -35,7 +35,9 @@ MongoDBBridge::MongoDBBridge(const boost::property_tree::ptree &pt) : CamInterfa storage = rs::Storage(host, db); actualFrame = 0; - + + index=-1; + storage.getScenes(frames); if(continual) { @@ -81,79 +83,113 @@ void MongoDBBridge::readConfig(const boost::property_tree::ptree &pt) bool MongoDBBridge::setData(uima::CAS &tcas, uint64_t timestamp) { + ros::NodeHandle n_("~"); + double_t time_stamp; // Variable which store the value of queued time stamp + + n_.getParam("ts",time_stamp); // getting the value of "ts" from the command arg and store in time_stamp. + n_.deleteParam("ts"); // deleting the "ts" variable. + 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 << ")"); + + auto it= std::find(frames.begin(), frames.end(), time_stamp); // finding if the queued timestamp is in the frames vector + + if(it== frames.end()){ + outWarn("Timestamp Queried is not in the frame or Timestamp Query not enabled"); } - else if(timestamp < frames.size()) { - timestamp = frames[timestamp]; - outInfo("setting data from frame with timestamp: (" << timestamp << ")"); + else{ + index= it- frames.begin(); // getting index value where time_stamp present } - 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; + if(index!=-1){ + timestamp = frames[index]; + 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(!continual && !loop) { + _newData = false; + } + } + else{ + 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; } From 3a98369c1f7d4ceb70fd878e8b4c084f272af0e1 Mon Sep 17 00:00:00 2001 From: AB <55253316+abmuslim@users.noreply.github.com> Date: Sun, 20 Mar 2022 22:06:35 +0100 Subject: [PATCH 2/5] Update MongoDBBridge.h --- robosherlock/src/io/include/robosherlock/io/MongoDBBridge.h | 1 + 1 file changed, 1 insertion(+) 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; From b6252250697031bbf4dcac515e465c45ebbbc78d Mon Sep 17 00:00:00 2001 From: AB <55253316+abmuslim@users.noreply.github.com> Date: Sun, 20 Mar 2022 22:10:14 +0100 Subject: [PATCH 3/5] Update rs.launch --- robosherlock/launch/rs.launch | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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)"> + From 02f9348f6bb36ab95d0fc7e43f5e79351574fd97 Mon Sep 17 00:00:00 2001 From: abmuslim Date: Wed, 30 Mar 2022 16:55:35 +0200 Subject: [PATCH 4/5] updated MongoDBBridge.cpp --- robosherlock/src/io/src/MongoDBBridge.cpp | 185 +++++++++++++--------- 1 file changed, 110 insertions(+), 75 deletions(-) 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; } From e5c104710730330a3c3238fd9e05a95593ca766d Mon Sep 17 00:00:00 2001 From: abmuslim Date: Wed, 30 Mar 2022 17:15:12 +0200 Subject: [PATCH 5/5] Updated MongoDBBridge.cpp --- robosherlock/src/io/src/MongoDBBridge.cpp | 45 ----------------------- 1 file changed, 45 deletions(-) diff --git a/robosherlock/src/io/src/MongoDBBridge.cpp b/robosherlock/src/io/src/MongoDBBridge.cpp index 67bc3530..facf87dc 100644 --- a/robosherlock/src/io/src/MongoDBBridge.cpp +++ b/robosherlock/src/io/src/MongoDBBridge.cpp @@ -35,13 +35,7 @@ MongoDBBridge::MongoDBBridge(const boost::property_tree::ptree &pt) : CamInterfa storage = rs::Storage(host, db); actualFrame = 0; -<<<<<<< HEAD index=0; -======= - - index=-1; - ->>>>>>> b6252250697031bbf4dcac515e465c45ebbbc78d storage.getScenes(frames); if(continual) { @@ -87,7 +81,6 @@ void MongoDBBridge::readConfig(const boost::property_tree::ptree &pt) bool MongoDBBridge::setData(uima::CAS &tcas, uint64_t timestamp) { -<<<<<<< HEAD MEASURE_TIME; ros::NodeHandle n_("~"); @@ -125,44 +118,6 @@ bool MongoDBBridge::setData(uima::CAS &tcas, uint64_t timestamp) } else{ const bool isNextFrame = timestamp == std::numeric_limits::max(); -======= - ros::NodeHandle n_("~"); - double_t time_stamp; // Variable which store the value of queued time stamp - - n_.getParam("ts",time_stamp); // getting the value of "ts" from the command arg and store in time_stamp. - n_.deleteParam("ts"); // deleting the "ts" variable. - - MEASURE_TIME; - const bool isNextFrame = timestamp == std::numeric_limits::max(); - - auto it= std::find(frames.begin(), frames.end(), time_stamp); // finding if the queued timestamp is in the frames vector - - if(it== frames.end()){ - outWarn("Timestamp Queried is not in the frame or Timestamp Query not enabled"); - } - else{ - index= it- frames.begin(); // getting index value where time_stamp present - } - if(index!=-1){ - timestamp = frames[index]; - 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(!continual && !loop) { - _newData = false; - } - - } - else{ ->>>>>>> b6252250697031bbf4dcac515e465c45ebbbc78d if(actualFrame >= frames.size()) { if(continual) { storage.getScenes(frames);