Skip to content
Snippets Groups Projects
Commit 4e5b313c authored by Patrick Hegemann's avatar Patrick Hegemann
Browse files

Refactor, fix SimObjPoseProvider, add property to provide everything

parent 84dbc4d4
No related branches found
No related tags found
1 merge request!49Fix simulation object pose provider
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 1
− 0
View file @ 4e5b313c
......@@ -19,6 +19,7 @@ depends_on_armarx_package(RobotComponents)
depends_on_armarx_package(VisionX "OPTIONAL")
#depends_on_armarx_package(MujocoX "OPTIONAL")
find_package(range-v3 QUIET)
find_package("Simox" 2.3.45 REQUIRED)
if (Simox_FOUND AND SimDynamics_USE_BULLET)
......
source/ArmarXSimulation/components/SimulationObjectPoseProvider/CMakeLists.txt
+ 2
− 0
View file @ 4e5b313c
......@@ -12,6 +12,8 @@ armarx_add_component(
RobotAPICore
RobotAPIInterfaces
ArmarXSimulationInterfaces
range-v3::range-v3
)
armarx_generate_and_add_component_executable(
......
source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp
+ 134
− 60
View file @ 4e5b313c
......@@ -20,27 +20,32 @@
* GNU General Public License
*/
#include "SimulationObjectPoseProvider.h"
#include <algorithm>
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <experimental/map>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
namespace armarx
{
const std::string SimulationObjectPoseProvider::defaultName = "SimulationObjectPoseProvider";
armarx::PropertyDefinitionsPtr
SimulationObjectPoseProvider::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
const armarx::PropertyDefinitionsPtr def =
new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
......@@ -55,13 +60,18 @@ namespace armarx
"UpdateFrequency",
"Frequency at which to update the objects in the ObjectMemory.");
def->optional(properties.initiallyRequestedEntities, "InitiallyRequestedEntities",
"All entities (comma separated) that should be requested from the beginning. If you want an entity to be localized for n seconds append ':n'.");
def->optional(properties.requestAllEntities,
"p.requestAllEntites",
"True if all entities should be requested all the time.");
def->optional(properties.initiallyRequestedEntities,
"p.initiallyRequestedEntities",
"All entities (comma separated) that should be requested from the beginning."
" If you want an entity to be localized for n seconds append ':n'.");
return def;
}
void
SimulationObjectPoseProvider::onInitComponent()
{
......@@ -74,18 +84,24 @@ namespace armarx
// setup the active requests
for (const auto& entity : simox::alg::split(properties.initiallyRequestedEntities, ","))
{
auto x = simox::alg::split(entity, ":");
auto until = armarx::core::time::DateTime::Invalid();
if (x.size() > 1)
{
ARMARX_WARNING << "Could not use multiple durations for localization request '" << x[0] << "'. Continue with the first duration which is " << x[1] << " seconds.";
until = armarx::core::time::DateTime::Now() + armarx::core::time::Duration::Seconds(std::stoi(x[1]));
}
activeRequests.insert({entity, until});
const auto tokens = simox::alg::split(entity, ":");
ARMARX_CHECK(not tokens.empty());
ARMARX_CHECK(tokens.size() <= 2)
<< "Could not use multiple durations for localization request '" << entity << "'";
const auto& objectName = tokens[0];
using DateTime = armarx::core::time::DateTime;
using Duration = core::time::Duration;
const auto until = tokens.size() == 1
? DateTime::Invalid()
: DateTime::Now() + Duration::Seconds(std::stoi(tokens.at(1)));
activeRequests.insert({objectName, until});
}
}
void
SimulationObjectPoseProvider::onConnectComponent()
{
......@@ -95,19 +111,16 @@ namespace armarx
poseProviderTask->start();
}
void
SimulationObjectPoseProvider::onDisconnectComponent()
{
}
void
SimulationObjectPoseProvider::onExitComponent()
{
}
std::string
SimulationObjectPoseProvider::getDefaultName() const
{
......@@ -122,34 +135,47 @@ namespace armarx
}
objpose::provider::RequestObjectsOutput
SimulationObjectPoseProvider::requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
SimulationObjectPoseProvider::requestObjects(
const objpose::provider::RequestObjectsInput& input,
const Ice::Current& /*unused*/)
{
std::lock_guard l(knownObjectsMutex);
std::lock_guard l2(activeRequestsMutex);
if (properties.requestAllEntities)
{
ARMARX_WARNING << "All entities are requested by default. "
"Requesting an object does not have an effect.";
// The method will still finish to generate a return object.
}
const std::lock_guard l(knownObjectsMutex);
const std::lock_guard l2(activeRequestsMutex);
auto now = armarx::core::time::DateTime::Now();
objpose::provider::RequestObjectsOutput output;
for (const auto& id : input.objectIDs)
{
std::string entityId = id.dataset + "/" + id.className + "/" + id.instanceName;
if (knownObjects.count(entityId) > 0)
const std::string entityId = id.dataset + "/" + id.className + "/" + id.instanceName;
if (knownObjects.find(entityId) != knownObjects.end())
{
if (activeRequests.find(entityId) != activeRequests.end())
{
ARMARX_INFO << "Object '" << entityId << "' is already requested. Updating the request time.";
ARMARX_INFO << "Object '" << entityId
<< "' is already requested. Updating the request time.";
}
activeRequests[entityId] = now + armarx::core::time::Duration::MilliSeconds(input.relativeTimeoutMS);
activeRequests[entityId] =
now + armarx::core::time::Duration::MilliSeconds(input.relativeTimeoutMS);
output.results[id].success = true;
}
else
{
std::vector<std::string> allNames;
allNames.reserve(knownObjects.size());
for (const auto& [name, _] : knownObjects)
{
allNames.push_back(name);
}
ARMARX_WARNING << "Could not query unknown object '" << entityId << "'. All known objects are: " << allNames;
ARMARX_WARNING << "Could not query unknown object '" << entityId
<< "'. All known objects are: " << allNames;
output.results[id].success = false;
}
}
......@@ -157,7 +183,7 @@ namespace armarx
}
objpose::ProviderInfo
SimulationObjectPoseProvider::getProviderInfo(const Ice::Current&)
SimulationObjectPoseProvider::getProviderInfo(const Ice::Current& /*unused*/)
{
objpose::ProviderInfo info;
info.objectType = objpose::KnownObject;
......@@ -166,7 +192,6 @@ namespace armarx
return info;
}
void
SimulationObjectPoseProvider::poseProviderTaskRun()
{
......@@ -189,40 +214,26 @@ namespace armarx
continue;
}
std::vector<objpose::ProvidedObjectPose> providedObjects;
{
std::scoped_lock l(knownObjectsMutex);
knownObjects.clear();
for (const auto& object : sceneData.objects)
{
knownObjects.insert({object.name, object});
}
std::scoped_lock l2(activeRequestsMutex);
for (auto req_it = activeRequests.begin(); req_it != activeRequests.end(); /* no increment */)
{
// Remove invalid and outdated requests
if (not(req_it->second.isInvalid()) and now > req_it->second)
{
req_it = activeRequests.erase(req_it);
continue;
}
// Add object poses for requested objects
if (const auto& object_it = knownObjects.find(req_it->first); object_it != knownObjects.end())
{
objpose::ProvidedObjectPose& pose = providedObjects.emplace_back();
objectPoseFromVisuData(pose, object_it->second, now);
}
req_it++;
}
if (properties.requestAllEntities)
{
// Convert all poses directly
providedObjects = getAllPoses(sceneData, now);
}
else
{
// Update known objects
updateKnownObjects(sceneData);
removeExpiredRequests(now);
providedObjects = getRequestedPoses(now);
}
// Report the poses
try
{
ARMARX_IMPORTANT << deactivateSpam(5) << "reporting " << providedObjects.size() << " object poses";
ARMARX_IMPORTANT << deactivateSpam(5) << "reporting " << providedObjects.size()
<< " object poses";
objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(providedObjects));
}
catch (const Ice::LocalException& e)
......@@ -233,11 +244,72 @@ namespace armarx
}
}
std::vector<objpose::ProvidedObjectPose>
SimulationObjectPoseProvider::getAllPoses(const armarx::SceneVisuData& sceneData,
const DateTime& time) const
{
auto convertPoses = [this, time](const auto& simObject)
{ return objectPoseFromVisuData(simObject, time); };
return sceneData.objects | ranges::views::transform(convertPoses) | ranges::to_vector;
}
void
SimulationObjectPoseProvider::objectPoseFromVisuData(objpose::ProvidedObjectPose& pose,
const armarx::ObjectVisuData& visuData,
const DateTime& time)
SimulationObjectPoseProvider::updateKnownObjects(const armarx::SceneVisuData& sceneData)
{
const std::scoped_lock l(knownObjectsMutex);
knownObjects.clear();
for (const auto& object : sceneData.objects)
{
knownObjects.insert({object.name, object});
}
}
void
SimulationObjectPoseProvider::removeExpiredRequests(const DateTime& time)
{
const std::scoped_lock l2(activeRequestsMutex);
auto isTimeout = [time](const auto& request)
{
const auto& provideUntilTime = request.second;
return (not provideUntilTime.isInvalid()) and time > provideUntilTime;
};
std::experimental::erase_if(activeRequests, isTimeout);
}
std::vector<objpose::ProvidedObjectPose>
SimulationObjectPoseProvider::getRequestedPoses(const DateTime& time) const
{
const std::scoped_lock l(knownObjectsMutex);
const std::scoped_lock l2(activeRequestsMutex);
auto isRequestedObjectKnown = [this](const auto& request)
{
const auto& objectName = request.first;
ARMARX_INFO << "isRequestedObjectKnown (" << objectName << ")";
return this->knownObjects.find(objectName) != this->knownObjects.end();
};
auto getPoseFromRequest = [this, time](const auto& request)
{
const auto& object = this->knownObjects.at(request.first);
ARMARX_INFO << "getPoseFromRequest (" << request.first << ")";
return objectPoseFromVisuData(object, time);
};
return activeRequests | ranges::views::filter(isRequestedObjectKnown) |
ranges::views::transform(getPoseFromRequest) | ranges::to_vector;
}
objpose::ProvidedObjectPose
SimulationObjectPoseProvider::objectPoseFromVisuData(const armarx::ObjectVisuData& visuData,
const DateTime& time) const
{
objpose::ProvidedObjectPose pose;
pose.providerName = getName();
pose.objectType = objpose::ObjectType::KnownObject;
pose.isStatic = visuData.staticObject;
......@@ -249,5 +321,7 @@ namespace armarx
pose.confidence = 1;
pose.timestamp = time;
return pose;
}
} // namespace armarx
source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.h
+ 17
− 21
View file @ 4e5b313c
......@@ -20,26 +20,24 @@
* GNU General Public License
*/
#pragma once
#include <cstdint>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
namespace armarx
{
class SimulationObjectPoseProvider :
virtual public armarx::Component
, virtual public armarx::ObjectPoseProviderPluginUser
virtual public armarx::Component,
virtual public armarx::ObjectPoseProviderPluginUser
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
......@@ -50,10 +48,11 @@ namespace armarx
// ObjectPoseProvider interface
public:
objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
objpose::ProviderInfo getProviderInfo(const Ice::Current&) override;
objpose::provider::RequestObjectsOutput
requestObjects(const objpose::provider::RequestObjectsInput& input,
const Ice::Current& /* unused */) override;
objpose::ProviderInfo getProviderInfo(const Ice::Current& /* unused */) override;
protected:
/// @see PropertyUser::createPropertyDefinitions()
......@@ -71,19 +70,19 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
private:
// Private methods go here.
void poseProviderTaskRun();
void objectPoseFromVisuData(objpose::ProvidedObjectPose& pose,
const armarx::ObjectVisuData& visuData,
const DateTime& time);
std::vector<objpose::ProvidedObjectPose> getAllPoses(const armarx::SceneVisuData& sceneData,
const DateTime& time) const;
// Forward declare `Properties` if you used it before its defined.
// struct Properties;
void updateKnownObjects(const armarx::SceneVisuData& sceneData);
void removeExpiredRequests(const DateTime& time);
std::vector<objpose::ProvidedObjectPose> getRequestedPoses(const DateTime& time) const;
objpose::ProvidedObjectPose objectPoseFromVisuData(const armarx::ObjectVisuData& visuData,
const DateTime& time) const;
private:
static const std::string defaultName;
......@@ -94,21 +93,18 @@ namespace armarx
mutable std::mutex activeRequestsMutex;
std::map<std::string, armarx::core::time::DateTime> activeRequests;
// Private member variables go here.
SimulatorInterfacePrx simulatorPrx;
armarx::SimpleRunningTask<>::pointer_type poseProviderTask;
/// Properties shown in the Scenario GUI.
struct Properties
{
float updateFrequency = 10;
std::int64_t updateFrequency = 10;
// Comma separated list of object entities and durations if desired.
bool requestAllEntities = false;
std::string initiallyRequestedEntities = "";
};
Properties properties;
};
} // namespace armarx
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