Skip to content
Snippets Groups Projects
Commit 1bf6d82e authored by Fabian Reister's avatar Fabian Reister
Browse files

Merge branch '45-follow-up-from-feature-armem-object-memory-articulated-objects' into 'armem/dev' 

Resolve "Follow-up from "Feature/armem object memory articulated objects""

See merge request ArmarX/RobotAPI!149
parents 4320b7c1 c0e2ea46
No related branches found
No related tags found
2 merge requests!157armem/dev => master,!149Resolve "Follow-up from "Feature/armem object memory articulated objects""
Hide whitespace changes
Inline Side-by-side
Showing
with 106 additions and 147 deletions
source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+ 26
− 38
View file @ 1bf6d82e
......@@ -20,8 +20,8 @@
*/
#include "ArticulatedObjectLocalizerExample.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "RobotAPI/libraries/armem_objects/types.h"
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <memory>
......@@ -33,6 +33,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
......@@ -43,6 +44,7 @@
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::articulated_object
......@@ -58,7 +60,7 @@ namespace armarx::articulated_object
defs->topic(debugObserver);
// defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
articulatedObjectWriter->registerPropertyDefinitions(defs);
articulatedObjectReader->registerPropertyDefinitions(defs);
......@@ -78,7 +80,10 @@ namespace armarx::articulated_object
articulatedObjectWriter->connect();
articulatedObjectReader->connect();
task = new RunningTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run);
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::now();
task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency);
task->start();
}
......@@ -134,51 +139,34 @@ namespace armarx::articulated_object
void ArticulatedObjectLocalizerExample::run()
{
ARMARX_IMPORTANT << "Running example.";
std::shared_ptr<VirtualRobot::Robot> dishwasher;
CycleUtil cycle(IceUtil::Time::milliSeconds(100));
IceUtil::Time start = TimeUtil::GetTime();
CycleUtil c(100);
while (not task->isStopped())
if (dishwasher == nullptr)
{
if (dishwasher == nullptr)
{
dishwasher = createDishwasher();
}
dishwasher = createDishwasher();
if (dishwasher == nullptr) // still
{
c.waitForCycleDuration();
continue;
return;
}
}
ARMARX_DEBUG << "Reporting articulated objects";
ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
const float t = float((now - start).toSecondsDouble());
const IceUtil::Time now = TimeUtil::GetTime();
const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
const std::map<std::string, float> jointValues
{
{"dishwasher_door_joint", M_PIf32 / 2 * k},
{"drawer_joint", 350 * k}
};
dishwasher->setJointValues(jointValues);
const std::map<std::string, float> jointValues
{
{"dishwasher_door_joint", M_PIf32 / 2 * k},
{"drawer_joint", 350 * k}
};
armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
articulatedObjectWriter->store(armemDishwasher);
dishwasher->setJointValues(jointValues);
c.waitForCycleDuration();
}
armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
articulatedObjectWriter->store(armemDishwasher);
}
} // namespace armarx::articulated_object
source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+ 11
− 1
View file @ 1bf6d82e
......@@ -3,6 +3,8 @@
// ArmarX
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "RobotAPI/libraries/armem/core/Time.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
......@@ -57,15 +59,23 @@ namespace armarx::articulated_object
private:
VirtualRobot::RobotPtr createDishwasher();
std::shared_ptr<VirtualRobot::Robot> dishwasher;
/// Reference timestamp for object movement
armem::Time start;
armarx::RunningTask<ArticulatedObjectLocalizerExample>::pointer_type task;
armarx::PeriodicTask<ArticulatedObjectLocalizerExample>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
struct Properties
{
float updateFrequency{25.F};
} p;
};
} // namespace armarx::articulated_object
source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt
+ 0
− 50
View file @ 1bf6d82e
......@@ -3,40 +3,11 @@ set(LIB_NAME ArticulatedObjectLocalizerExample)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
# If your component needs a special ice interface, define it here:
# armarx_add_component_interface_lib(
# SLICE_FILES
# ArticulatedObjectLocalizerExample.ice
# ICE_LIBS
# # RobotAPI
#)
# Add the component
armarx_add_component(
COMPONENT_LIBS
# ArmarXCore
ArmarXCore
ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
RobotAPICore
RobotAPIInterfaces
armem
armem_objects
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## ArticulatedObjectLocalizerExampleInterfaces # If you defined a component ice interface above.
SOURCES
ArticulatedObjectLocalizerExample.cpp
......@@ -44,27 +15,6 @@ armarx_add_component(
ArticulatedObjectLocalizerExample.h
)
# Add dependencies
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
# All target_include_directories must be guarded by if(Xyz_FOUND)
# For multiple libraries write: if(X_FOUND AND Y_FOUND) ...
#if(MyLib_FOUND)
# target_include_directories(ArticulatedObjectLocalizerExample PUBLIC ${MyLib_INCLUDE_DIRS})
#endif()
# Add ARON files
#armarx_enable_aron_file_generation_for_target(
# TARGET_NAME
# ${ARMARX_COMPONENT_NAME}
# ARON_FILES
# aron/ExampleData.xml
#)
# Add unit tests
add_subdirectory(test)
......
source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+ 44
− 54
View file @ 1bf6d82e
#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
source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+ 4
− 4
View file @ 1bf6d82e
......@@ -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
......
source/RobotAPI/libraries/armem_objects/types.h
+ 21
− 0
View file @ 1bf6d82e
/*
* 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>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment