TransformWriter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2021
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #include "TransformWriter.h"
23 
24 #include <algorithm>
25 #include <iterator>
26 #include <numeric>
27 #include <optional>
28 
29 #include <Eigen/Geometry>
30 
31 #include <IceUtil/Time.h>
32 
37 
45 #include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
49 
51 {
53 
54  void
56  {
57  ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
58 
59  def->optional(properties.coreSegmentID.memoryName, propertyPrefix + "MemoryName");
60  def->optional(properties.coreSegmentID.coreSegmentName,
61  propertyPrefix + "LocalizationSegmentName",
62  "Name of the localization memory core segment to use.");
63  }
64 
65  void
67  {
68  // Wait for the memory to become available and add it as dependency.
69  ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '"
70  << properties.coreSegmentID.memoryName << "' ...";
71  try
72  {
73  memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
74  ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '"
75  << properties.coreSegmentID << "'";
76  }
78  {
79  ARMARX_ERROR << e.what();
80  return;
81  }
82  }
83 
84  bool
85  TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
86  {
87  std::lock_guard g{memoryWriterMutex};
88 
89  const MemoryID providerId =
90  properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
91  // const auto& timestamp = transform.header.timestamp;
92  const MemoryID entityID =
93  providerId.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
94  const Time timestamp = Time::Now(); // FIXME remove
95 
97  update.entityID = entityID;
98  update.referencedTime = timestamp;
99 
100  arondto::Transform aronTransform;
101  toAron(aronTransform, transform);
102  update.instancesData = {aronTransform.toAron()};
103 
104  ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
105  armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
106 
107  ARMARX_DEBUG << updateResult;
108 
109  if (not updateResult.success)
110  {
111  ARMARX_ERROR << updateResult.errorMessage;
112  }
113 
114  return updateResult.success;
115  }
116 
117 } // namespace armarx::armem::client::robot_state::localization
armarx::armem::client::robot_state::localization::TransformWriter::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def) override
to be called in Component::addPropertyDefinitions
Definition: TransformWriter.cpp:55
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::client::robot_state::localization::TransformWriter::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem) override
to be called in Component::onConnectComponent
Definition: TransformWriter.cpp:66
armarx::armem::client::robot_state::localization
Definition: interfaces.h:33
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::armem::EntityUpdateResult
Result of an EntityUpdate.
Definition: Commit.h:72
MemoryID.h
armarx::armem::toAron
void toAron(arondto::MemoryID &dto, const MemoryID &bo)
Definition: aron_conversions.cpp:19
armarx::armem::client::MemoryNameSystem::useWriter
Writer useWriter(const MemoryID &memoryID)
Use a memory server and get a writer for it.
Definition: MemoryNameSystem.cpp:276
query_fns.h
armarx::armem::client::Writer::commit
CommitResult commit(const Commit &commit)
Writes a Commit to the memory.
Definition: Writer.cpp:51
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
FramedPose.h
armarx::armem::client::robot_state::localization::TransformWriter::~TransformWriter
~TransformWriter() override
MemoryRemoteGui.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
error.h
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:27
aron_conversions.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
Factory.h
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
memory_definitions.h
CycleUtil.h
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::MemoryID::withEntityName
MemoryID withEntityName(const std::string &name) const
Definition: MemoryID.cpp:420
PropertyDefinitionContainer.h
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::transform
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...
Definition: algorithm.h:315
Builder.h
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
TransformWriter.h
Logging.h
ice_conversions.h
armarx::armem::client::robot_state::localization::TransformWriter::commitTransform
bool commitTransform(const ::armarx::armem::robot_state::Transform &transform) override
Definition: TransformWriter.cpp:85