From 6074af232e5f8baa4df8d3ef9367fd6a3322e093 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 21 Jul 2021 14:34:19 +0200
Subject: [PATCH] Handle missing kinematic unit
---
.../server/description/Segment.cpp | 48 +++++++++----------
.../server/description/Segment.h | 2 +-
.../server/proprioception/Segment.cpp | 23 +++++----
3 files changed, 36 insertions(+), 37 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 8c8d1fcac..03488c3d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -5,11 +5,12 @@
#include <IceUtil/Time.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -56,9 +57,7 @@ namespace armarx::armem::server::robot_state::description
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::RobotDescription::toAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::RobotDescription::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
@@ -79,7 +78,7 @@ namespace armarx::armem::server::robot_state::description
}
- void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+ void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
@@ -99,11 +98,7 @@ namespace armarx::armem::server::robot_state::description
Commit commit;
commit.updates.push_back(update);
-
- {
- // std::lock_guard g{memoryMutex};
- iceMemory.commit(commit);
- }
+ iceMemory.commitLocking(commit);
}
@@ -111,23 +106,29 @@ namespace armarx::armem::server::robot_state::description
{
ARMARX_CHECK_NOT_NULL(robotUnit);
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
- ARMARX_CHECK_NOT_NULL(kinematicUnit);
+ if (kinematicUnit)
+ {
+ const std::string robotName = kinematicUnit->getRobotName();
+ const std::string robotFilename = kinematicUnit->getRobotFilename();
- const std::string robotName = kinematicUnit->getRobotName();
- const std::string robotFilename = kinematicUnit->getRobotFilename();
+ const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
- const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
- ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+ const robot::RobotDescription robotDescription
+ {
+ .name = kinematicUnit->getRobotName(),
+ .xml = {package, kinematicUnit->getRobotFilename()}
+ }; // FIXME
- const robot::RobotDescription robotDescription
+ commitRobotDescription(robotDescription);
+ }
+ else
{
- .name = kinematicUnit->getRobotName(),
- .xml = {package, kinematicUnit->getRobotFilename()}
- }; // FIXME
-
- storeRobotDescription(robotDescription);
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Cannot commit robot description.";
+ }
}
@@ -159,8 +160,7 @@ namespace armarx::armem::server::robot_state::description
}
else
{
- ARMARX_WARNING << "Could not convert entity instance to "
- "'RobotDescription'";
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
}
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 2411180a3..2a51250f7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -72,7 +72,7 @@ namespace armarx::armem::server::robot_state::description
private:
- void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void commitRobotDescription(const robot::RobotDescription& robotDescription);
void updateRobotDescription();
server::MemoryToIceAdapter& iceMemory;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 8c5a777bd..c84460fc7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -5,6 +5,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -59,20 +60,18 @@ namespace armarx::armem::server::robot_state::proprioception
{
this->robotUnit = robotUnitPrx;
+ std::string providerSegmentName = "Robot";
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
- const std::string providerSegmentName = kinematicUnit->getRobotName();
+ if (kinematicUnit)
+ {
+ providerSegmentName = kinematicUnit->getRobotName();
+ }
+ else
+ {
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
+ }
this->robotUnitProviderID = coreSegment->id().withProviderSegmentName(providerSegmentName);
-
-#if 0
- // TODO what is the purpose?
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
-#endif
}
--
GitLab