Skip to content
Snippets Groups Projects
Commit 163a3d1a authored by Your Name's avatar Your Name
Browse files

tested on ARMAR-DE

parent bf6c6b6b
No related branches found
No related tags found
No related merge requests found
......@@ -89,6 +89,8 @@ namespace armarx::armem::server::robot_state::proprioception
// Proprioception + Exteroception
armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
ARMARX_INFO << VAROUT(update.exteroception.updates.size());
armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
......@@ -151,24 +153,27 @@ namespace armarx::armem::server::robot_state::proprioception
// Send batch to memory
Update update;
{
if(data.proprioception){
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
up.timeCreated = data.timestamp;
up.instancesData = {data.proprioception};
}
// Exteroception
{
if(data.exteroception){
armem::EntityUpdate& up = update.exteroception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
up.timeCreated = data.timestamp;
up.instancesData = {data.exteroception};
}
// Extract odometry data.
ARMARX_CHECK_NOT_NULL(data.proprioception);
const std::string platformKey = "platform";
if (data.proprioception->hasElement(platformKey))
{
......
......@@ -24,15 +24,23 @@ namespace armarx::armem::server::robot_state::proprioception
const std::string& robotTypeName)
{
{
converterProprioception = converterRegistry.get(robotTypeName);
converterProprioception = proprioceptionConverterRegistry.get(robotTypeName);
ARMARX_CHECK_NOT_NULL(converterProprioception)
<< "No converter for robot type '" << robotTypeName << "' available. \n"
<< "Known are: " << converterRegistry.getKeys();
<< "Known are: " << proprioceptionConverterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
receiver = robotUnitPlugin.startDataStreaming(config);
description = receiver->getDataDescription();
}
{
converterExteroception = exteroceptionConverterRegistry.get(robotTypeName);
ARMARX_CHECK_NOT_NULL(converterProprioception)
<< "No converter for robot type '" << robotTypeName << "' available. \n"
<< "Known are: " << exteroceptionConverterRegistry.getKeys();
}
{
// Thread-local copy of debug observer helper.
debugObserver = DebugObserverHelper(
......
......@@ -72,7 +72,8 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitDataStreamingReceiverPtr receiver;
RobotUnitDataStreaming::DataStreamingDescription description;
ConverterRegistry converterRegistry;
ConverterRegistry proprioceptionConverterRegistry;
exteroception::ConverterRegistry exteroceptionConverterRegistry;
proprioception::ConverterInterface* converterProprioception = nullptr;
exteroception::ConverterInterface* converterExteroception = nullptr;
......
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