Go to the documentation of this file.
25 #include <qnamespace.h>
27 #include <SimoxUtility/algorithm/apply.hpp>
28 #include <VirtualRobot/VirtualRobot.h>
34 #include <RobotAPI/interface/armem/commit.h>
41 #include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
66 profileSegment(iceAdapter()),
67 personInstanceSegment(iceAdapter()),
68 poseSegment(iceAdapter()),
69 faceRecognitionSegment(iceAdapter()),
70 identificationSegment(iceAdapter()),
85 profileSegment.
init();
86 personInstanceSegment.
init();
88 faceRecognitionSegment.
init();
89 identificationSegment.
init();
119 if (robots.count(robotName) == 0)
121 auto newRobot = virtualRobotReaderPlugin->get().getRobot(robotName);
123 <<
"Failed to obtain robot with name `" << robotName <<
"`.";
125 robots[robotName] = newRobot;
128 return robots.at(robotName);
132 ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
139 const ::armarx::aron::data::dto::AronDictSeq& dictSeq,
140 const ::std::string& providerName,
141 const ::armarx::core::time::dto::DateTime& iceTimestamp,
142 const ::Ice::Current& )
151 fromIce(iceTimestamp, timestamp);
155 {
return armarx::human::arondto::HumanPose::FromAron(dict); };
157 std::vector<armarx::human::arondto::HumanPose> humanPoses =
158 simox::alg::apply(dictSeq, toHumanPose);
161 const std::string robotName =
162 humanPoses.front().keypoints.begin()->second.positionCamera.header.agent;
169 for (armarx::human::arondto::HumanPose& humanPose : humanPoses)
171 for (
auto& [_, keyPoint] : humanPose.keypoints)
173 const auto& cameraFrame = keyPoint.positionCamera.header.frame;
174 const auto& agent = keyPoint.positionCamera.header.agent;
177 keyPoint.positionCamera.position, cameraFrame, agent);
194 robot->getRootNode()->getName(),
199 if (keyPoint.orientationCamera.has_value())
202 keyPoint.positionCamera.header.frame);
205 keyPoint.orientationCamera->orientation.toRotationMatrix(),
206 keyPoint.positionCamera.position,
212 const Eigen::Isometry3f global_T_human(
218 keyPoint.orientationGlobal.value(),
226 const Eigen::Isometry3f robot_root_T_human(
235 robot->getRootNode()->getName(),
249 const std::vector<::armarx::human::arondto::HumanPose>& humanPoses,
250 const std::string& providerName,
257 const auto entityID = providerId.withEntityName(
"human_poses").withTimestamp(timestamp);
260 update.entityID = entityID;
261 update.sentTime = timestamp;
262 update.referencedTime = timestamp;
267 std::back_inserter(
update.instancesData),
269 { return humanPose.toAron(); });
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
void connect(const viz::Client &arviz, DebugObserverInterfacePrx debugObserver=nullptr)
A bundle of updates to be sent to the memory.
static const std::string CORE_SEGMENT_NAME
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
const std::string GlobalFrame
std::string getDefaultName() const override
Retrieve default name of component.
void setMemoryName(const std::string &memoryName)
PluginT * addPlugin(const std::string prefix="", ParamsT &&...params)
const DebugObserverInterfacePrx & getDebugObserver() const
Eigen::Vector3f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
server::wm::Memory & workingMemory()
Eigen::Vector3f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
void onExitComponent() override
Hook for subclass.
Eigen::Matrix4f toGlobalEigen(const SharedRobotInterfacePrx &referenceRobot) const
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
virtual void init() override
The FramedPosition class.
An update of an entity for a specific point in time.
CoreSegmentT & addCoreSegment(const std::string &name, aron::type::ObjectPtr coreSegmentType=nullptr, const std::vector< PredictionEngine > &predictionEngines={})
Add an empty core segment with the given name, type and prediction engines.
MemoryToIceAdapter & iceAdapter()
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
The FramedOrientation class.
void onInitComponent() override
Pure virtual hook for the subclass.
void fromIce(const data::MemoryID &ice, MemoryID &id)
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
const VirtualRobot::RobotPtr & getSynchronizedRobot(const std::string &robotName, const armarx::DateTime ×tamp)
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx &referenceRobot) const
virtual data::CommitResult commit(const data::Commit &commit, const Ice::Current &=Ice::emptyCurrent) override
Represents a point in time.
std::shared_ptr< Dict > DictPtr
void onDisconnectComponent() override
Hook for subclass.
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
MatrixXX< 3, 3, float > Matrix3f
const VariantTypeId FramedOrientation
Default component property definition container.
armarx::viz::Client arviz
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
const VariantTypeId FramedPosition
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
void onConnectComponent() override
Pure virtual hook for the subclass.
data::CommitResult commitLocking(const data::Commit &commitIce, Time timeArrived)
std::shared_ptr< class Robot > RobotPtr
void commitHumanPoses(const std::vector<::armarx::human::arondto::HumanPose > &humanPoses, const std::string &providerName, const armarx::DateTime ×tamp)
void commitHumanPosesInCameraFrame(const ::armarx::aron::data::dto::AronDictSeq &dictSeq, const ::std::string &providerName, const ::armarx::core::time::dto::DateTime ×tamp, const ::Ice::Current &=::Ice::emptyCurrent) override
const armem::MemoryID MemoryID