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

Merge branch 'eurobin_nancy' into 'master'

euROBIN

See merge request !497
parents c5312132 e5544d4d
No related branches found
No related tags found
1 merge request!497euROBIN
Pipeline #21875 failed
Showing
with 65 additions and 48 deletions
......@@ -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);
......
#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())
......
......@@ -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;
}
......
......@@ -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
......@@ -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
......@@ -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;
}
......
......@@ -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
......
......@@ -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;
......
......@@ -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
......@@ -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);
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment