diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index ff55654b049adfe37980d243b1717e69c5938d73..ea5d2df495d8f427695f8778562035439f632397 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -18,12 +18,13 @@
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/forward_declarations.h>
-#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
@@ -48,6 +49,11 @@ namespace armarx::armem::server::grasp
defs->optional(enableVisualizeKnownGraspCandidates,
"p.enableVisualizeKnownGraspCandidates");
+ defs->optional(enableVisualizeKnownGraspCandidatesPreposes,
+ "p.enableVisualizeKnownGraspCandidatesPreposes");
+ defs->optional(enableVisualizeKnownGraspCandidatesApproach,
+ "p.enableVisualizeKnownGraspCandidatesApproach");
+
defs->optional(frequencyHzVisualizeKnownGraspCandidates,
"p.frequencyHzVisualizeKnownGraspCandidates");
@@ -1016,16 +1022,55 @@ namespace armarx::armem::server::grasp
const Eigen::Isometry3f eef_T_hand_root{
handRootNode->getPoseInFrame(tcp)};
- const Eigen::Isometry3f pose =
+ const Eigen::Isometry3f global_T_grasp_pose =
Eigen::Isometry3f{object.objectPoseGlobal} *
- Eigen::Isometry3f{grasp.pose}.inverse() * eef_T_hand_root;
+ Eigen::Isometry3f{grasp.pose}.inverse();
+
+ // visualize grasp pose
+ {
+ viz::Robot graspHandVisu(visuName + "_grasp");
+ graspHandVisu.useCollisionModel()
+ .file(visuRobotPartXML)
+ .pose(
+ Eigen::Isometry3f{global_T_grasp_pose * eef_T_hand_root}
+ .matrix());
+
+ layer.add(graspHandVisu);
+ }
+
+ // visualize prepose if available
+ if (grasp.prepose.has_value())
+ {
+ const Eigen::Isometry3f global_T_prepose =
+ Eigen::Isometry3f{object.objectPoseGlobal} *
+ Eigen::Isometry3f{grasp.prepose.value()}.inverse();
- viz::Robot handVisu(visuName);
- handVisu.useCollisionModel()
- .file(visuRobotPartXML)
- .pose(pose.matrix());
+ // visualize as prepose as hand
+ if (enableVisualizeKnownGraspCandidatesPreposes)
+ {
+ viz::Robot graspHandVisuPrepose(visuName + "_prepose");
+ graspHandVisuPrepose.useCollisionModel()
+ .file(visuRobotPartXML)
+ .pose(Eigen::Isometry3f{global_T_prepose *
+ eef_T_hand_root}
+ .matrix())
+ .overrideColor(viz::Color::blue());
+
+ layer.add(graspHandVisuPrepose);
+ }
+
+ // visualize approach direction (arrow between poses)
+ if (enableVisualizeKnownGraspCandidatesApproach)
+ {
+ viz::Arrow arrow(visuName + "_approach");
+ arrow
+ .fromTo(global_T_prepose.translation(),
+ global_T_grasp_pose.translation())
+ .color(viz::Color::green());
- layer.add(handVisu);
+ layer.add(arrow);
+ }
+ }
}
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 40ef25686fc4c8b545bd0aa49ed787c4035ab7c5..d6ea24e6ecf3f421f62f2b8c904e7e15ff678c3b 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -120,6 +120,10 @@ namespace armarx::armem::server::grasp
bool enableRemoteGui{true};
bool enableVisualizeKnownGraspCandidates = false;
+ bool enableVisualizeKnownGraspCandidatesPreposes = false;
+ bool enableVisualizeKnownGraspCandidatesApproach = false;
+
+
std::size_t frequencyHzVisualizeKnownGraspCandidates = 5;
void visualizeGraspCandidates();
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 1167db200b5cacded7c52f265e8c2b9261b85bed..f2c6108a9ace4740e0add45b27ed7aa827347d41 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,7 +1,5 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <set>
-
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
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/server/ltm/processors/converter/data/image/png/PngConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
index b9d4a92d4514075c72d9f70948ab9d39a7428d65..3294e56c181c8811d7471a7fb01a46db48ee14cf 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp
@@ -3,6 +3,7 @@
// ArmarX
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
#include <opencv2/opencv.hpp>
#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index fcce7ea3a3319aa4b7afeeacae9c02abb7f00482..2f48fb22ba086c0ff82dd096272d2b317d41a086 100644
--- a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -21,6 +21,10 @@
<ObjectChild key='pose'>
<Pose />
</ObjectChild>
+
+ <ObjectChild key='prepose'>
+ <Pose optional="true" />
+ </ObjectChild>
</Object>
<!-- A Grasp set is a set of grasps that need to be executed to grasp an object. The order is important! -->
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index c805d2ee70a55356bfd46c463cd2f9b315aec583..e18c49fcbbefa6a08196536a49705a55adf87866 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,9 +64,46 @@ 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();
+ VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
+ {
+ std::vector<VirtualRobot::GraspPtr> preGrasps;
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
+ if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
+ {
+ preGrasps.emplace_back(grasp);
+ }
+
+ // everything else is assumed to be a grasp
+ }
+
+ // create a new grasp set with the preposes
+ VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
+ preGraspSet->removeAllGrasps(); // just keep the EEF info etc
+ for (const auto& preGrasp : preGrasps)
+ {
+ preGraspSet->addGrasp(preGrasp);
+ }
+
+ return preGraspSet;
+ }();
+
+
+ ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
+
+ // remove all preposes from the grasp set
+ for (const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ graspSet->removeGrasp(preGrasp);
+ }
+
+
for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
ARMARX_CHECK_NOT_NULL(grasp);
@@ -76,8 +114,57 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = grasp->getQuality();
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
-
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+ retGrasp.prepose.reset();
+
+ // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
+ {
+ const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
+ if (// not retGrasp.prepose.has_value() and
+ simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
+ {
+ const std::string prePoseName =
+ retGrasp.name.substr(
+ 0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
+ PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
<< retGraspSet.name << "' for obj '" << objectClassName
<< "' with pose \n"
<< retGrasp.pose;
@@ -85,6 +172,19 @@ namespace armarx::armem::grasping::segment
retGraspSet.grasps.push_back(retGrasp);
}
+ // Now, check if there are any preposes left in the set. This should not have happened.
+ if (preGraspSet->getSize() > 0)
+ {
+ ARMARX_WARNING << "Found " << preGraspSet->getSize()
+ << " preposes in the grasp set '" << retGraspSet.name
+ << "' for obj '" << objectClassName
+ << "' that do not have a corresponding grasp!";
+ for(const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+ }
+ }
+
ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
0)
<< "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
@@ -132,13 +232,13 @@ namespace armarx::armem::grasping::segment
update.instancesData = {knownGraspCandidate->toAron()};
- ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+ ARMARX_DEBUG << VAROUT(knownGraspCandidate->graspSets.size());
for (const auto& gs : knownGraspCandidate->graspSets)
{
- ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+ ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
for (const auto& grasp : gs.second.grasps)
{
- ARMARX_VERBOSE << VAROUT(grasp.name);
+ ARMARX_DEBUG << VAROUT(grasp.name);
}
}
}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
index de92372581076450341fb53e4628c4dd7ae4ba7c..75aadd96aff419a636b802fcc73548b080e4d8d8 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -23,5 +23,9 @@ namespace armarx::armem::grasping::segment
public:
static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";
+
+ private:
+ static const constexpr char* PREPOSE_SUFFIX = "_Prepose";
+ static const constexpr char* GRASP_OPTIONAL_SUFFIX = "_Grasp";
};
}
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/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
index a141b97e4e8b9d4b169ff5b9118c330b71080d5a..fcb2a94487dc7034ce7a02c24478a35890e5ff77 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
@@ -1,6 +1,5 @@
#include "Segment.h"
-#include <filesystem>
#include <sstream>
#include <sys/inotify.h>
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);
}
}
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index 0f1f8047f3f47976a7ce52a928bac858d665b053..05a230a8e02ffab13ae9c2970a817889e56e2982 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -62,7 +62,7 @@ namespace armarx::obstacle_avoidance
if (auto obstacle = finder.loadManipulationObject(objectPose))
{
obstacle->setGlobalPose(objectPose.objectPoseGlobal);
- obstacle->setName(objectPose.objectID.instanceName());
+ obstacle->setName(objectPose.objectID.str());
sceneObjects->addSceneObject(obstacle);
}
}
@@ -104,7 +104,6 @@ namespace armarx::obstacle_avoidance
Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
- // ARMARX_INFO << world_T_obj.translation();
auto cube = factory.createBox(boxSize, boxSize, boxSize);
@@ -177,4 +176,4 @@ namespace armarx::obstacle_avoidance
arviz.commitLayerContaining("CollisionModel", mesh);
}
-} // namespace armarx::obstacle_avoidance
\ No newline at end of file
+} // namespace armarx::obstacle_avoidance
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 62fee239e4bf83fdfe6cb3d9a09428484280d4aa..cf6c57f98d8970194c2f362dcfa122bb19c183a0 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -1,5 +1,7 @@
#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx
{
namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
index 9fffd35a6f258e778200351ef387ba9d3b184480..d9edf529c300968b4b428066836ba106a39e76e8 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.h
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -2,7 +2,6 @@
#include <functional>
#include <mutex>
-#include <queue>
#include <thread>
#include <ArmarXCore/core/logging/Logging.h>
@@ -14,10 +13,8 @@
#include "SkillDescription.h"
#include "SkillID.h"
-#include "SkillPreparationInput.h"
#include "SkillProxy.h"
#include "SkillStatusUpdate.h"
-#include "error/Exception.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.h b/source/RobotAPI/libraries/skills/core/SkillDescription.h
index 784afd9321f4d677109c1f43672f0bdff4846db8..f65ba3f45e5da2c36c223c79797bb3b27ac52450 100644
--- a/source/RobotAPI/libraries/skills/core/SkillDescription.h
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.h
@@ -1,7 +1,6 @@
#pragma once
#include <string>
-#include <vector>
#include <ArmarXCore/core/time/Duration.h>
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
index 91494c78c331272e85babe5052ccd2369e29833e..8bf5eb0f34a4a73ef8255f1ac64e1222ca71692d 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -1,14 +1,12 @@
#pragma once
#include <string>
-#include <vector>
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include "ProviderID.h"
-#include "error/Exception.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
index ea4fc4d1a0192d5ad4819be4248df3ea52566164..fb46cdc96d1e6581afd938a6e5cbd981e3ea9016 100644
--- a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
@@ -3,6 +3,8 @@
#include <chrono>
#include <thread>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx
{
namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 39220a32bc28269232f506d8368cb5394980b87b..8e5ade1866d4d71a389443c1b5ce1fc48485e9ea 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -1,5 +1,7 @@
#include "SkillStatusUpdate.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx
{
namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
index ace31510a8a362ed45289c4facdd51c97ba22fbb..f6318d5c1993fd94d17a6a5e5e16086794d1f34d 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -1,8 +1,5 @@
#pragma once
-#include <string>
-#include <vector>
-
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -10,8 +7,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "SkillExecutionID.h"
-#include "SkillID.h"
-#include "SkillParameterization.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/skills/core/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
index 9e01205b27e74eddc69fa8ff7c28d2646eafc9e2..d9107d8213b9d5d2b1e70f578ee7b376be0f102a 100644
--- a/source/RobotAPI/libraries/skills/core/error/Exception.h
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.h
@@ -24,9 +24,7 @@
#pragma once
// STD/STL
-#include <map>
#include <string>
-#include <vector>
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index fa354d03d242c9e19b95a4a846309227503ae182..4b8a07d07238f87a2cea835ed33b75c0520d8639 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -27,6 +27,7 @@
#include <ArmarXCore/core/time/Metronome.h>
#include "RobotAPI/libraries/skills/core/Skill.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx::skills
{
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 914c6da0221973624309a57666064669e05de14a..b707aee0d5224ab8588c4c95461ec22f5f300fa6 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include "PeriodicSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
index 33a6475b3a9e2e01a30c40f4cee60e304a600b40..db3c05d511872e082bf4b2bb3a01a1293f7468af 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -1,5 +1,7 @@
#include "SimplePeriodicSkill.h"
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
namespace armarx::skills
{
SimplePeriodicSkill::SimplePeriodicSkill(const SkillDescription& skillDescription,
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
index ce3c968f587a7984fb606e8485297aa56ffff04a..c0f8b7ebd23df0d41429b2e9c7ddd3a7b0f66e94 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -1,5 +1,6 @@
#pragma once
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include "PeriodicSkill.h"
#include "SimpleSpecializedSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 2fded9a0a50ede4cdc3e8d76861217fbc1127c58..fa3c43ce54eda2feacd0f88895ede30840c08e1f 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -3,47 +3,41 @@
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
-// Debug
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-namespace armarx
+namespace armarx::skills
{
- namespace skills
+ template <class AronT>
+ class SpecializedSkill : public Skill
{
- template <class AronT>
- class SpecializedSkill : public Skill
+ public:
+ using Base = Skill;
+ using ParamType = AronT;
+
+ using Skill::Skill;
+ virtual ~SpecializedSkill() = default;
+
+ void
+ setParameters(const AronT& d)
+ {
+ Base::setParameters(d.toAron());
+ }
+
+ /// Overwrite getter for parameters. Shadow Skill::getParameters()
+ AronT
+ getParameters() const
+ {
+ AronT d;
+ d.fromAron(this->parameters);
+ return d;
+ }
+
+ /// returns the accepted type of the skill
+ static armarx::aron::type::ObjectPtr
+ GetAcceptedType()
{
- public:
- using Base = Skill;
- using ParamType = AronT;
-
- using Skill::Skill;
- virtual ~SpecializedSkill() = default;
-
- void
- setParameters(const AronT& d)
- {
- Base::setParameters(d.toAron());
- }
-
- /// Overwrite getter for parameters. Shadow Skill::getParameters()
- AronT
- getParameters() const
- {
- AronT d;
- d.fromAron(this->parameters);
- return d;
- }
-
- /// returns the accepted type of the skill
- static armarx::aron::type::ObjectPtr
- GetAcceptedType()
- {
- return AronT::ToAronType();
- }
-
-
- protected:
- };
- } // namespace skills
-} // namespace armarx
+ return AronT::ToAronType();
+ }
+
+
+ protected:
+ };
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index a244ae6593ec82b802dcbd930af07990ce3d4503..0e1515343b7cf9bf84daa949eb70d2035f35e751 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -1,5 +1,6 @@
#include "SkillImplementationWrapper.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/aron/SkillErrorResult.aron.generated.h>