Segment.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <filesystem>
4 #include <map>
5 #include <optional>
6 #include <string>
7 
8 #include <SimoxUtility/caching/CacheMap.h>
9 #include <SimoxUtility/shapes/OrientedBox.h>
10 
12 
15 #include <RobotAPI/interface/core/RobotState.h>
16 #include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
24 
25 #include "ArticulatedObjectVisu.h"
26 #include "Decay.h"
27 
28 namespace armarx::armem::arondto
29 {
30  class ObjectInstance;
31 }
32 
34 {
35 
37  {
38  public:
39  struct CommitStats
40  {
41  int numUpdated = 0;
42  };
43 
46  using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
47 
48  struct Calibration
49  {
50  std::string robotName = "";
51  std::string robotNode = "Neck_2_Pitch";
52  float offset = 0.0f;
53 
55  const std::string& prefix = "calibration.");
56  };
57 
58 
59  public:
61  virtual ~Segment() override;
62 
63 
65  const std::string& prefix = "") override;
66  void init() override;
67  void connect(viz::Client arviz);
68 
69 
70  CommitStats commitObjectPoses(const std::string& providerName,
71  const objpose::data::ProvidedObjectPoseSeq& providedPoses,
72  const Calibration& calibration,
73  std::optional<Time> discardUpdatesUntil = std::nullopt);
74  void commitObjectPoses(const ObjectPoseSeq& objectPoses,
75  const std::string& providerName = "");
76 
77 
79  objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName,
80  const DateTime& now);
81 
82  wm::Entity* findObjectEntity(const ObjectID& objectID,
83  const std::string& providerName = "");
84  std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
85 
86  objpose::ProviderInfo getProviderInfo(const std::string& providerName);
87 
88  objpose::AttachObjectToRobotNodeOutput
89  attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
90  objpose::DetachObjectFromRobotNodeOutput
91  detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
92  objpose::DetachAllObjectsFromRobotNodesOutput
93  detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
94 
95 
96  /**
97  * @brief If the object is attached to a robot node, update it according to the current robot state.
98  *
99  * If there is no attachement info in `objectPose` itself, the internal data
100  * structure `attachments` is queried. If an attachment is found there,
101  * it is written into the given `objectPose` (thus, it is "cached" in the
102  * info `objectPose`).
103  *
104  * @param synchronized Indicates whether the agent is already synchronized to the current time.
105  */
106  void updateAttachement(ObjectPose& objectPose,
108  bool& synchronized) const;
109 
110 
111  static ObjectPoseMap getLatestObjectPoses(const wm::CoreSegment& coreSeg);
113  static ObjectPose getLatestObjectPose(const wm::Entity& entity);
114 
115  static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out);
116  static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
117  static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
118 
119  static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
120 
122 
123  static std::map<DateTime, ObjectPose>
124  getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
125 
126  private:
128 
129  void updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now);
130  void updateObjectPoses(ObjectPoseMap& objectPoses,
131  const DateTime& now,
133  bool& agentSynchronized) const;
134  void updateObjectPose(ObjectPose& objectPose,
135  const DateTime& now,
137  bool& agentSynchronized) const;
138 
139 
140  ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
141 
142 
143  void storeDetachedSnapshot(wm::Entity& entity,
144  const arondto::ObjectInstance& data,
145  Time now,
146  bool commitAttachedPose);
147 
148 
149  std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
150 
151  friend struct DetachVisitor;
152 
153 
154  private:
155  void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
156  std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
157  std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
158  std::optional<std::filesystem::path> resolveSceneFilepath(const std::string& filename);
159 
160  armarx::objects::Scene getSceneSnapshot() const;
161  void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
162  void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
163 
164 
165  public:
166  /// Loaded robot models identified by the robot name.
167  struct RobotsCache : public armarx::Logging
168  {
170  std::string fallbackName;
171 
172  std::map<std::string, VirtualRobot::RobotPtr> loaded;
173 
174  VirtualRobot::RobotPtr get(const std::string& robotName,
175  const std::string& providerName = "");
176  };
177 
179 
180 
181  objpose::ProviderInfoMap providers;
182 
183 
185 
186  /// Decay model.
188 
189 
190  private:
191  struct Properties
192  {
193  bool discardSnapshotsWhileAttached = true;
194 
195  /// Package containing the scene snapshots
196  std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
197  std::string sceneSnapshotsDirectory = "scenes";
198  std::string sceneSnapshotsToLoad = "";
199 
200  bool autoReloadSceneSnapshotsOnFileChange = false;
201 
202  std::vector<std::string> getSceneSnapshotsToLoad() const;
203  };
204 
205  Properties p;
206 
207 
208  /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
209  simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
210 
211  /// Class name -> dataset name.
212  simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
213 
214  std::unique_ptr<CMakePackageFinder> finder;
215 
216  static const std::string timestampPlaceholder;
217 
218  public:
219  struct RemoteGui
220  {
222 
226 
230 
233 
234  void setup(const Segment& data);
235  void update(Segment& data);
236  };
237 
238 
239  private:
240  std::unique_ptr<ArticulatedObjectVisu> visu;
241 
242  SimpleRunningTask<>::pointer_type fileWatcherTask;
243  };
244 
245 } // namespace armarx::armem::server::obj::instance
armarx::armem::server::obj::instance::Segment::getProviderInfo
objpose::ProviderInfo getProviderInfo(const std::string &providerName)
Definition: Segment.cpp:800
Client.h
SpecializedSegment.h
armarx::armem::server::obj::instance::Segment::getObjectPoses
objpose::ObjectPoseMap getObjectPoses(const DateTime &now)
Definition: Segment.cpp:486
armarx::armem::robot_state::VirtualRobotReader
The VirtualRobotReader class.
Definition: VirtualRobotReader.h:39
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:10
armarx::ObjectFinder::DefaultObjectsPackageName
static const std::string DefaultObjectsPackageName
Definition: ObjectFinder.h:26
armarx::armem::server::obj::instance::Segment::getObjectOOBB
std::optional< simox::OrientedBoxf > getObjectOOBB(const ObjectID &id)
Definition: Segment.cpp:794
armarx::armem::server::obj::instance::Segment::providers
objpose::ProviderInfoMap providers
Definition: Segment.h:181
armarx::armem::server::segment::SpecializedCoreSegment
A base class for core segments.
Definition: SpecializedCoreSegment.h:19
armarx::armem::server::obj::instance::Segment::detachAllObjectsFromRobotNodes
objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput &input)
Definition: Segment.cpp:973
armarx::objpose::ObjectPoseSeq
std::vector< ObjectPose > ObjectPoseSeq
Definition: forward_declarations.h:20
armarx::armem::server::obj::instance::Segment::RemoteGui::loadButton
armarx::RemoteGui::Client::Button loadButton
Definition: Segment.h:225
armarx::armem::server::obj::instance::Segment::Calibration
Definition: Segment.h:48
armarx::armem::server::MemoryToIceAdapter
Helps connecting a Memory server to the Ice interface.
Definition: MemoryToIceAdapter.h:19
armarx::armem::server::obj::instance::Decay
Models decay of object localizations by decreasing the confidence the longer the object was not local...
Definition: Decay.h:18
armarx::armem::server::obj::instance::Segment::RobotsCache
Loaded robot models identified by the robot name.
Definition: Segment.h:167
Prediction.h
armarx::armem::server::obj::instance::Segment::RemoteGui::storeLoadLine
armarx::RemoteGui::Client::LineEdit storeLoadLine
Definition: Segment.h:223
armarx::armem::server::obj::instance::Segment::Calibration::offset
float offset
Definition: Segment.h:52
armarx::armem::server::obj::instance::Segment::objectFinder
ObjectFinder objectFinder
Definition: Segment.h:184
armarx::armem::server::obj::instance::Segment::init
void init() override
Definition: Segment.cpp:163
armarx::armem::server::obj::instance::Segment::RobotsCache::loaded
std::map< std::string, VirtualRobot::RobotPtr > loaded
Definition: Segment.h:172
armarx::armem::server::obj::instance::Segment::Calibration::robotName
std::string robotName
Definition: Segment.h:50
armarx::armem::server::wm::Entity
Definition: memory_definitions.h:27
armarx::armem::server::obj::instance::Segment::Calibration::robotNode
std::string robotNode
Definition: Segment.h:51
armarx::armem::server::obj::instance::Segment::attachObjectToRobotNode
objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput &input)
Definition: Segment.cpp:820
armarx::armem::arondto
Definition: aron_conversions.h:17
armarx::armem::server::obj::instance
Definition: ArticulatedObjectVisu.cpp:16
memory_definitions.h
armarx::armem::server::obj::instance::Segment::connect
void connect(viz::Client arviz)
Definition: Segment.cpp:259
armarx::ObjectFinder
Used to find objects in the ArmarX objects repository [1] (formerly [2]).
Definition: ObjectFinder.h:22
armarx::armem::server::obj::instance::Segment::detachObjectFromRobotNode
objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput &input)
Definition: Segment.cpp:925
Decay.h
armarx::armem::server::obj::instance::Segment::updateAttachement
void updateAttachement(ObjectPose &objectPose, VirtualRobot::RobotPtr agent, bool &synchronized) const
If the object is attached to a robot node, update it according to the current robot state.
Definition: Segment.cpp:592
armarx::armem::server::obj::instance::Segment::RobotsCache::fallbackName
std::string fallbackName
Definition: Segment.h:170
armarx::armem::server::obj::instance::Segment::RobotsCache::get
VirtualRobot::RobotPtr get(const std::string &robotName, const std::string &providerName="")
Definition: Segment.cpp:1416
armarx::armem::server::obj::instance::Segment::ObjectPoseMap
std::map< ObjectID, ObjectPose > ObjectPoseMap
Definition: Segment.h:46
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
ObjectID.h
armarx::armem::server::obj::instance::Segment::RemoteGui::group
armarx::RemoteGui::Client::GroupBox group
Definition: Segment.h:221
armarx::RemoteGui::Client::GroupBox
Definition: Widgets.h:193
armarx::RemoteGui::Client::LineEdit
Definition: Widgets.h:40
armarx::armem::server::obj::instance::Segment::Calibration::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="calibration.")
Definition: Segment.cpp:52
filename
std::string filename
Definition: VisualizationRobot.cpp:86
armarx::armem::server::obj::instance::Segment::getArticulatedObjects
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects()
Definition: Segment.cpp:712
ObjectPose.h
armarx::armem::server::obj::instance::Segment::RemoteGui::discardSnapshotsWhileAttached
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached
Definition: Segment.h:229
VirtualRobotReader.h
armarx::armem::server::obj::instance::Segment::Segment
Segment(server::MemoryToIceAdapter &iceMemory)
Definition: Segment.cpp:62
armarx::armem::server::obj::instance::Segment::decay
Decay decay
Decay model.
Definition: Segment.h:187
TaskUtil.h
armarx::armem::server::obj::instance::Segment::CommitStats
Definition: Segment.h:39
armarx::armem::server::obj::instance::Segment::getLatestObjectPose
static ObjectPose getLatestObjectPose(const wm::Entity &entity)
Definition: Segment.cpp:646
armarx::armem::server::wm::ProviderSegment
Definition: memory_definitions.h:52
armarx::armem::server::obj::instance::Segment::RobotsCache::reader
robot_state::VirtualRobotReader * reader
Definition: Segment.h:169
armarx::armem::server::obj::instance::Segment::RemoteGui::update
void update(Segment &data)
Definition: Segment.cpp:1367
armarx::armem::server::obj::instance::Segment::RemoteGui::detachAllObjectsCommitAttachedPoseCheckBox
armarx::RemoteGui::Client::CheckBox detachAllObjectsCommitAttachedPoseCheckBox
Definition: Segment.h:232
armarx::objpose::ProvidedObjectPoseSeq
std::vector< ProvidedObjectPose > ProvidedObjectPoseSeq
Definition: forward_declarations.h:25
armarx::objects::Scene
Definition: Scene.h:55
armarx::armem::server::obj::instance::Segment::getObjectPosesByProvider
objpose::ObjectPoseMap getObjectPosesByProvider(const std::string &providerName, const DateTime &now)
Definition: Segment.cpp:494
armarx::armem::server::wm::CoreSegment
base::CoreSegmentBase
Definition: memory_definitions.h:75
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
forward_declarations.h
armarx::armem::articulated_object::ArticulatedObjects
armarx::armem::robot_state::Robots ArticulatedObjects
Definition: types.h:141
armarx::Logging
Base Class for all Logging classes.
Definition: Logging.h:239
armarx::armem::server::obj::instance::Segment::RemoteGui::infiniteHistory
armarx::RemoteGui::Client::CheckBox infiniteHistory
Definition: Segment.h:228
armarx::armem::server::obj::instance::Segment::getLatestObjectPoses
static ObjectPoseMap getLatestObjectPoses(const wm::CoreSegment &coreSeg)
Definition: Segment.cpp:630
armarx::armem::server::segment::detail::SegmentBase< server::wm::CoreSegment >::iceMemory
MemoryToIceAdapter & iceMemory
Definition: SpecializedSegment.h:58
armarx::armem::server::obj::instance::Segment::commitObjectPoses
CommitStats commitObjectPoses(const std::string &providerName, const objpose::data::ProvidedObjectPoseSeq &providedPoses, const Calibration &calibration, std::optional< Time > discardUpdatesUntil=std::nullopt)
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::server::obj::instance::Segment::RemoteGui::setup
void setup(const Segment &data)
Definition: Segment.cpp:1307
armarx::armem::server::obj::instance::Segment::~Segment
virtual ~Segment() override
Definition: Segment.cpp:101
armarx::armem::server::obj::instance::Segment::RemoteGui
Definition: Segment.h:219
armarx::RemoteGui::Client::IntSpinBox
Definition: Widgets.h:69
armarx::armem::server::obj::instance::Segment::RemoteGui::detachAllObjectsButton
armarx::RemoteGui::Client::Button detachAllObjectsButton
Definition: Segment.h:231
armarx::RemoteGui::Client::CheckBox
Definition: Widgets.h:129
armarx::armem::server::obj::instance::Segment::ObjectPoseSeq
objpose::ObjectPoseSeq ObjectPoseSeq
Definition: Segment.h:45
armarx::RemoteGui::Client::Button
Definition: Widgets.h:120
armarx::armem::server::obj::instance::Segment
Definition: Segment.h:36
armarx::armem::server::obj::instance::Segment::RemoteGui::maxHistorySize
armarx::RemoteGui::Client::IntSpinBox maxHistorySize
Definition: Segment.h:227
armarx::armem::server::obj::instance::Segment::RemoteGui::storeButton
armarx::RemoteGui::Client::Button storeButton
Definition: Segment.h:224
armarx::viz::Client
Definition: Client.h:117
armarx::armem::server::obj::instance::Segment::getLatestInstanceData
static arondto::ObjectInstance getLatestInstanceData(const wm::Entity &entity)
Definition: Segment.cpp:697
armarx::armem::server::obj::instance::Segment::findObjectEntity
wm::Entity * findObjectEntity(const ObjectID &objectID, const std::string &providerName="")
Definition: Segment.cpp:504
armarx::armem::server::obj::instance::Segment::robots
RobotsCache robots
Definition: Segment.h:178
ArticulatedObjectVisu.h
armarx::armem::server::obj::instance::Segment::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
Definition: Segment.cpp:106
armarx::armem::server::obj::instance::Segment::CommitStats::numUpdated
int numUpdated
Definition: Segment.h:41
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:33
armarx::armem::server::obj::instance::Segment::DetachVisitor
friend struct DetachVisitor
Definition: Segment.h:151
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
ObjectFinder.h
armarx::armem::server::obj::instance::Segment::getObjectPosesInRange
static std::map< DateTime, ObjectPose > getObjectPosesInRange(const wm::Entity &entity, const DateTime &start, const DateTime &end)
Definition: Segment.cpp:766
armarx::objpose::ObjectPoseMap
std::map< ObjectID, ObjectPose > ObjectPoseMap
Definition: forward_declarations.h:21