diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 3ab4e0530b54331305cdbc8f8ad05603607b0ac9..fb834d52c07e86f500d05c097d5a5d19616ee2c5 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -185,7 +185,7 @@ namespace armarx
{
if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
{
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Missing Iterations or iterations out of order! "
<< "This should not happen. " << VAROUT(_last_iteration_id) << ", "
<< VAROUT(step.iterationId);
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index c805d2ee70a55356bfd46c463cd2f9b315aec583..cb9ac5c50080f6fababdf1e7ed7914c2c6e4af83 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -1,5 +1,6 @@
#include "KnownGraspProviderSegment.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/XML/ObjectIO.h>
@@ -63,7 +64,9 @@ namespace armarx::armem::grasping::segment
arondto::KnownGraspSet retGraspSet;
retGraspSet.name = graspSet->getName();
- retGraspSet.robot = graspSet->getRobotType();
+
+ retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
+
retGraspSet.endeffector = graspSet->getEndEffector();
for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index a702857316a46fb59fd581c95e92b8b2413d371e..f0ec3136995b80a3a8efedb5132e73df04801849 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -7,6 +7,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -32,37 +33,27 @@ namespace armarx::armem::articulated_object
const std::string& instanceName,
VirtualRobot::RobotIO::RobotDescription loadMode)
{
- const auto descriptions = queryDescriptions(timestamp, providerName);
+ ARMARX_VERBOSE << "Getting `" << typeName + "/" + instanceName << "`";
- ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ const std::optional<ArticulatedObject> articulatedObjectDescription =
+ get(typeName + "/" + instanceName, timestamp, providerName);
+ ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
+ << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
- const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
- { return desc.name == typeName; });
-
- if (it == descriptions.end())
- {
- ARMARX_WARNING << "Description for articulate object with type <" << typeName
- << "> not (yet) available!";
- return nullptr;
- }
-
- ARMARX_DEBUG << "Description for articulate object with type <" << typeName
- << "> available!";
-
- auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+ auto obj = VirtualRobot::RobotIO::loadRobot(
+ articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
if (not obj)
{
- ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
- << ">!";
+ ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
+ << instanceName << "` from file `"
+ << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
+
return nullptr;
}
obj->setName(instanceName);
- obj->setType(it->name);
+ obj->setType(typeName);
return obj;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index f293741f9ba9baa56853717f1984389ce30ad1d1..0085fd3fade1e29d046b22b62887196584332508 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -51,7 +51,8 @@ namespace armarx::armem::articulated_object
bool
ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp)
+ const armem::Time& timestamp,
+ const bool isStatic)
{
ARMARX_CHECK_NOT_NULL(articulatedObject);
@@ -59,6 +60,6 @@ namespace armarx::armem::articulated_object
armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
convert(*articulatedObject, Time::Now());
- return store(armemArticulatedObject);
+ return store(armemArticulatedObject, isStatic);
}
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
index ff39d70f049fa670e41157dbb79608b16d98b66c..edc401b6e0d55487f9446c1b5d0129cd623ea5c1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
@@ -14,8 +14,8 @@ namespace armarx::armem::articulated_object
public:
using Writer::Writer;
- bool
- storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp);
+ bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp,
+ bool isStatic = false);
};
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index e0ce311486d06c5dc859422976f4c96aa9d037dc..ac40111242347fc16a1e3557c02206a18377f27b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -171,6 +171,7 @@ namespace armarx::armem::articulated_object
if (providerName.has_value()) // query single provider
{
+ ARMARX_VERBOSE << "Single provider query";
ARMARX_CHECK_NOT_EMPTY(providerName.value());
// clang-format off
@@ -183,6 +184,8 @@ namespace armarx::armem::articulated_object
}
else // query all providers
{
+ ARMARX_VERBOSE << "All provider query";
+
// clang-format off
qb
.coreSegments().withName(objects::constants::CoreClassSegmentName)
@@ -192,7 +195,9 @@ namespace armarx::armem::articulated_object
// clang-format on
}
+ ARMARX_VERBOSE << "Before query";
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ ARMARX_VERBOSE << "After query";
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -201,6 +206,8 @@ namespace armarx::armem::articulated_object
return {};
}
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "getRobotDescriptions";
return getRobotDescriptions(qResult.memory);
}
@@ -380,20 +387,18 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(objects::constants::CoreClassSegmentName);
std::vector<robot_state::description::RobotDescription> descriptions;
- coreSegment.forEachEntity(
- [&descriptions](const wm::Entity& entity)
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
{
- if (not entity.empty())
+ ARMARX_VERBOSE << "Converting ...";
+ if (const auto robotDescription = convertRobotDescription(instance))
{
- const auto robotDescription =
- convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
- if (robotDescription)
- {
- descriptions.push_back(*robotDescription);
- }
+ descriptions.push_back(*robotDescription);
}
});
+ ARMARX_VERBOSE << descriptions.size() << " descriptions";
+
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index d061c692e3399b07279a6cd5420c295396d34e38..f5ae8c13b383662411bc4d33399cc87ec3048a79 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -15,11 +15,11 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include "utils.h"
@@ -191,7 +191,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::storeInstance(const ArticulatedObject& obj) const
+ Writer::storeInstance(const ArticulatedObject& obj, const bool isStatic) const
{
std::lock_guard g{memoryWriterMutex};
@@ -218,7 +218,7 @@ namespace armarx::armem::articulated_object
arondto::ObjectInstance objectInstance;
toAron(objectInstance, obj.config);
- const auto classId = storeOrGetClass(obj);
+ const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
if (not classId)
{
@@ -229,10 +229,17 @@ namespace armarx::armem::articulated_object
// install memory link
toAron(objectInstance.classID, *classId);
+ // set object instance id
+ const MemoryID memoryInstanceId = classId->withEntityName(entityName);
+
armem::MemoryID id;
- id.setEntityID(classId->getEntityID());
+ id.setEntityID(memoryInstanceId.getEntityID());
armarx::ObjectID objectId(id.entityName);
+ ARMARX_INFO << "Object ID: " << objectId;
+
+ ARMARX_CHECK_NOT_EMPTY(objectId.instanceName())
+ << "An object instance name must be provided!";
armarx::arondto::ObjectID cs;
cs.className = objectId.className();
@@ -243,6 +250,8 @@ namespace armarx::armem::articulated_object
objectInstance.pose.providerName = properties.providerName;
objectInstance.pose.attachmentValid = false;
+ objectInstance.pose.isStatic = isStatic;
+
update.instancesData = {objectInstance.toAron()};
update.referencedTime = timestamp;
@@ -260,7 +269,7 @@ namespace armarx::armem::articulated_object
}
bool
- Writer::store(const ArticulatedObject& obj) const
+ Writer::store(const ArticulatedObject& obj, const bool isStatic) const
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
@@ -271,7 +280,7 @@ namespace armarx::armem::articulated_object
return false;
}
- return storeInstance(obj);
+ return storeInstance(obj, isStatic);
}
// TODO this is a duplicate
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index dcc49a2b2dc9e9e32a8edd06efa2026e0bb326c6..568bec35dad164f665cd41f9067d92b16693e380 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -45,9 +45,9 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool store(const ArticulatedObject& obj) const override;
+ bool store(const ArticulatedObject& obj, bool isStatic) const override;
- bool storeInstance(const ArticulatedObject& obj) const;
+ bool storeInstance(const ArticulatedObject& obj, bool isStatic) const;
std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
// const std::string& getPropertyPrefix() const override;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index 899cb1e79ac7ada7991e064b2a1ae1c20310749a..19043a9cc3d4c3409012d7111eee172edb4d579b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -30,7 +30,7 @@ namespace armarx::armem::articulated_object
public:
virtual ~WriterInterface() = default;
- virtual bool store(const ArticulatedObject& obj) const = 0;
+ virtual bool store(const ArticulatedObject& obj, bool isStatic) const = 0;
};
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
index 39604fa3dabac64b806d63001bcb2866e2b421ed..f7a8f7adb35f4eb54dbbb199c076e20f46e0625d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -3,6 +3,7 @@
#include <Eigen/Geometry>
#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
@@ -72,6 +73,9 @@ namespace armarx::armem::server::obj::familiar_object_instance
auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
+ auto confidenceCmap = simox::color::cmaps::viridis();
+ confidenceCmap.set_vlimits(0, 1);
+
for (const auto& familiarObject : familiarObjects)
{
if (not isWithinTimeFrame(familiarObject))
@@ -120,7 +124,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
auto box = viz::Box(objectId.str());
box.pose(global_T_bb.matrix());
box.size(familiarObject.bounding_box.extents);
- box.color(simox::Color::gray(128, alpha));
+
+ auto color = confidenceCmap.at(familiarObject.confidence);
+ color.a = alpha;
+
+ box.color(color);
layerBox.add(box);
}
}