cleanup code

CMakeLists.txt

@@ -19,7 +19,7 @@ armarx_find_package(PUBLIC armarx_control REQUIRED)
 armarx_find_package(PUBLIC ArmarXGui QUIET)
 
 # Required system dependencies.
-armarx_find_package(PUBLIC OpenCV REQUIRED)
+#armarx_find_package(PUBLIC OpenCV REQUIRED)
 
 # Optional system dependencies.
 # armarx_find_package(PUBLIC PCL QUIET)

source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp

@@ -89,8 +89,8 @@ void Scheduler::onConnectComponent() {
           onMemoryUpdated(updatedSnapshotIDs);
         });
   }
-  // submitIdleTarget();
-  // testMemoryRequest();
+  resetPriorityQueue();
+  submitIdleTarget();
 }
 
 void Scheduler::onDisconnectComponent() {
@@ -100,66 +100,6 @@ void Scheduler::onDisconnectComponent() {
 
 void Scheduler::onExitComponent() {}
 
-void Scheduler::testMemoryRequest() {
-  // test submitTarget
-  auto oneSecond = armarx::Duration::MilliSeconds(1000);
-  FramedPosition viewCenter(Eigen::Vector3f(0, 1000, 1000),
-                            properties.robotFrame, properties.robotName);
-  FramedPosition viewRight(Eigen::Vector3f(-1000, 1000, 1000),
-                           properties.robotFrame, properties.robotName);
-  FramedPosition viewLeft(Eigen::Vector3f(1000, 1000, 1000),
-                          properties.robotFrame, properties.robotName);
-  auto targetCenter = armarx::view_selection::gaze_targets::GazeTarget(
-      "targetCenter", viewCenter,
-      gaze_targets::TargetPriority(gaze_targets::AttentionType::StimulusDriven,
-                                   0.0),
-      oneSecond, true);
-  auto targetRight = armarx::view_selection::gaze_targets::GazeTarget(
-      "targetRight", viewRight,
-      gaze_targets::TargetPriority(gaze_targets::AttentionType::StimulusDriven,
-                                   2.0),
-      oneSecond, true);
-  auto targetLeft = armarx::view_selection::gaze_targets::GazeTarget(
-      "targetLeft", viewLeft,
-      gaze_targets::TargetPriority(gaze_targets::AttentionType::StimulusDriven,
-                                   1.0),
-      oneSecond, true);
-
-  namespace armem = armarx::armem;
-  const armem::MemoryID coreSegmentID("View", "GazeTarget");
-  armem::client::Writer writer = memoryNameSystem().getWriter(coreSegmentID);
-  armem::EntityUpdate update;
-
-  gaze_targets::arondto::GazeTarget dto;
-  toAron(dto, targetCenter);
-  // report to memory, GazeTargets are identified by their name
-  update.entityID = coreSegmentID.withProviderSegmentName("TestProvider")
-                        .withEntityName(dto.name);
-  update.timeCreated = armarx::Clock::Now();
-  update.instancesData = {dto.toAron()};
-  writer.commit(update);
-  // controller->submitTarget(dto.toAronDTO());
-  armarx::TimeUtil::Sleep(1);
-  toAron(dto, targetRight);
-  // report to memory, GazeTargets are identified by their name
-  update.entityID = coreSegmentID.withProviderSegmentName("TestProvider")
-                        .withEntityName(dto.name);
-  update.timeCreated = armarx::Clock::Now();
-  update.instancesData = {dto.toAron()};
-  writer.commit(update);
-  // controller->submitTarget(dto.toAronDTO());
-  armarx::TimeUtil::Sleep(1);
-  toAron(dto, targetLeft);
-  // report to memory, GazeTargets are identified by their name
-  update.entityID = coreSegmentID.withProviderSegmentName("TestProvider")
-                        .withEntityName(dto.name);
-  update.timeCreated = armarx::Clock::Now();
-  update.instancesData = {dto.toAron()};
-  writer.commit(update);
-  // controller->submitTarget(dto.toAronDTO());
-  armarx::TimeUtil::Sleep(1);
-}
-
 void Scheduler::submitIdleTarget() {
   const FramedPosition viewCenter(Eigen::Vector3f(0, 1500, 1500),
                                   properties.robotFrame, properties.robotName);
@@ -167,12 +107,12 @@ void Scheduler::submitIdleTarget() {
   const gaze_targets::TargetPriority priority(
       gaze_targets::AttentionType::RandomEvent, 0.0);
   // this target has lowest priority and will never be removed from the queue
-  submitToQueue(
+  std::lock_guard<std::mutex> targetLock(targetMutex);
+  requestedTargets.emplace(
       gaze_targets::GazeTarget("idle", viewCenter, priority, infinite, true));
 }
 
 void Scheduler::submitToQueue(const gaze_targets::GazeTarget &target) {
-  ARMARX_INFO << "submitting";
   std::lock_guard<std::mutex> targetLock(targetMutex);
   // remove target if found in queue
   for (auto it = requestedTargets.cbegin(); it != requestedTargets.cend();
@@ -189,16 +129,13 @@ void Scheduler::submitToQueue(const gaze_targets::GazeTarget &target) {
 }
 
 void Scheduler::resetPriorityQueue(const Ice::Current &) {
-  std::lock_guard<std::mutex> targetLock(targetMutex);
 
-  ARMARX_WARNING
-      << "Resetting PriorityQueue for targets, old requests are discarded";
+  ARMARX_INFO << "Resetting PriorityQueue, old requests are discarded";
+  std::lock_guard<std::mutex> targetLock(targetMutex);
   requestedTargets.clear();
-  submitIdleTarget();
 }
 
 void Scheduler::scheduleNextTarget() {
-  ARMARX_INFO << "scheduling";
   // prevent communicaton loss by waiting for controller feedback
   if (!openControlRequest) {
 
@@ -213,7 +150,6 @@ void Scheduler::scheduleNextTarget() {
                   << *nextTarget;
       // keep track of submittedTargets, copy requestedTarget to submitted
       submittedTargets.push_back(*nextTarget);
-      ARMARX_INFO << submittedTargets;
 
       openControlRequest = true;
       gaze_targets::arondto::GazeTarget dto;
@@ -274,7 +210,7 @@ void Scheduler::handleTargetUpdate(
       }
       // targets with negative Duration continue until preemption
       if (memoryTarget.isReached() &&
-          memoryTarget.duration.toMicroSeconds() > 0) {
+          memoryTarget.duration.toMicroSeconds() >= 0) {
         ARMARX_INFO << memoryTarget.name << " reached, fixating";
         armarx::core::time::dto::Duration duration;
         armarx::core::time::toIce(duration, memoryTarget.duration);
@@ -284,10 +220,7 @@ void Scheduler::handleTargetUpdate(
       }
       // if none of the cases above match, the target has just been successfully
       // submitted and needs to wait until it is reached
-      ARMARX_INFO << memoryTarget.name << " successfully submitted";
       openControlRequest = false;
-    } else {
-      ARMARX_INFO << "miss.";
     }
   }
 }
@@ -301,8 +234,6 @@ void Scheduler::reportGazeTarget(const ::armarx::aron::data::dto::DictPtr &dto,
   // convert DictPtr to arondto::GazeTarget
   const auto arondto = gaze_targets::arondto::GazeTarget::FromAron(dto);
 
-  ARMARX_IMPORTANT << "reporting TargetUpdate";
-
   // handle target update
   gaze_targets::GazeTarget bo;
   gaze_targets::fromAron(arondto, bo);
@@ -335,24 +266,20 @@ void Scheduler::onMemoryUpdated(
       for (const auto &memId : updatedSnapshotIDs) {
         if (not armem::contains(schedulerProviderID, memId)) {
           // foreign memory updates are targetRequests
-          ARMARX_IMPORTANT << "Foreign update:" << memId.str();
+          ARMARX_INFO << "new Request received: " << memId.str();
           const auto *snapshot = memory.findLatestInstance(memId);
 
           if (snapshot != nullptr) {
-            ARMARX_INFO << "Extract snapshot";
             gaze_targets::GazeTarget gazeTarget =
                 armarx::aron::fromAron<gaze_targets::GazeTarget>(
                     gaze_targets::arondto::GazeTarget::FromAron(
                         snapshot->data()));
 
-            ARMARX_INFO << "submit target";
             submitToQueue(gazeTarget);
-
           } else {
             ARMARX_INFO << "No memory found with id:" << memId;
           }
         } else {
-          ARMARX_IMPORTANT << "Ignoring own update:" << memId.str();
         }
       }
     } else {

source/armarx/view_selection/gaze_targets/GazeTarget.h

@@ -90,8 +90,8 @@ public:
   };
 
   friend std::ostream &operator<<(std::ostream &os, const GazeTarget &target) {
-    return os << "{ " << target.name << target.creationTimestamp << ", "
-              << target.priority << ", " << target.duration << ","
+    return os << "{ " << target.name << ", " << target.creationTimestamp << ", "
+              << target.priority << ", " << target.duration << ", "
               << target.status << " }";
   };
 };