From c0e2ea4677430adacd8a8f001876f7ac71f2bee1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 14 May 2021 19:35:56 +0200
Subject: [PATCH] articulated object visu: periodic task
---
.../articulated_object_instance/Visu.cpp | 98 +++++++++----------
.../server/articulated_object_instance/Visu.h | 8 +-
.../RobotAPI/libraries/armem_objects/types.h | 21 ++++
3 files changed, 69 insertions(+), 58 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
index 54f45eca1..d1ee962ac 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
@@ -1,15 +1,16 @@
#include "Visu.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
#include <algorithm>
-#include <SimoxUtility/math/pose.h>
-
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <SimoxUtility/math/pose.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "Segment.h"
namespace armarx::armem::server::obj::articulated_object_instance
@@ -49,9 +50,9 @@ namespace armarx::armem::server::obj::articulated_object_instance
.file(xmlPath.path, xmlPath.path)
.joints(obj.config.jointMap)
.pose(obj.config.globalPose);
+ // clang-format on
robot.useFullModel();
- // clang-format on
layer.add(robot);
};
@@ -62,14 +63,48 @@ namespace armarx::armem::server::obj::articulated_object_instance
void Visu::init()
{
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
+ updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
updateTask->start();
}
+ void Visu::visualizeRun()
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_DEBUG << "Update task";
+
+ if (not p.enabled)
+ {
+ return;
+ }
+
+ // TIMING_START(Visu);
+
+ const auto articulatedObjects = segment.getArticulatedObjects();
+ ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+
+ viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
+
+ ARMARX_DEBUG << "visualizing objects";
+ visualizeObjects(layer, articulatedObjects);
+
+ ARMARX_DEBUG << "Committing objects";
+ arviz.commit({layer});
+
+ ARMARX_DEBUG << "Done committing";
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+
+ }
+
// void Visu::RemoteGui::setup(const Visu& visu)
// {
// using namespace armarx::RemoteGui::Client;
@@ -121,50 +156,5 @@ namespace armarx::armem::server::obj::articulated_object_instance
// }
- void Visu::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
- while (updateTask && not updateTask->isStopped())
- {
- {
- // std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
-
- if (p.enabled)
- {
- // TIMING_START(Visu);
-
- const auto articulatedObjects = segment.getArticulatedObjects();
-
- ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
-
- viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
-
- ARMARX_DEBUG << "visualizing objects";
- visualizeObjects(layer, articulatedObjects);
-
- ARMARX_DEBUG << "Committing objects";
-
- arviz.commit({layer});
-
- ARMARX_DEBUG << "Done committing";
-
-
- // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
index 54aea5e09..3590270fe 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
@@ -22,7 +22,7 @@
#pragma once
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
@@ -35,13 +35,13 @@ namespace armarx
{
class ObjectFinder;
}
+
namespace armarx::armem::server::obj::articulated_object_instance
{
class Segment;
/**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
+ * @brief Visualizes articulated objects
*/
class Visu : public armarx::Logging
{
@@ -76,7 +76,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
} p;
- SimpleRunningTask<>::pointer_type updateTask;
+ PeriodicTask<Visu>::pointer_type updateTask;
void visualizeRun();
// struct RemoteGui
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index cb348cd3a..855c855c4 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -1,3 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
#pragma once
#include <vector>
--
GitLab