ObjectPoseProviderExample.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  * @package RobotAPI::ArmarXObjects::ObjectPoseProviderExample
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <SimoxUtility/algorithm/string/string_tools.h>
26 #include <SimoxUtility/math/pose/pose.h>
27 
29 
34 
35 
36 namespace armarx
37 {
38 
40  {
42 
43  defs->topic(debugObserver);
44 
45  defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
46  .map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
47 
48  defs->optional(singleShot, "SingleShot", "If true, publishes only one snapshot.");
49 
50  return defs;
51  }
52 
54  {
55  return "ObjectPoseProviderExample";
56  }
57 
59  {
60  for (const auto& initial : initialObjectIDs)
61  {
62  // -1 -> infinitely
63  requestedObjects.requestObjects({ObjectID(initial)}, -1);
64  }
65 
66  {
67  providerInfo.objectType = objpose::ObjectType::KnownObject;
68  std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
69  for (const auto& obj : objects)
70  {
71  providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
72  }
73  }
74  }
75 
77  {
78  poseEstimationTask = new SimpleRunningTask<>([this]()
79  {
80  this->poseEstimationTaskRun();
81  });
82  poseEstimationTask->start();
83  }
84 
86  {
87  }
88 
90  {
91  }
92 
93  objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
94  {
95  return providerInfo;
96  }
97 
98  objpose::provider::RequestObjectsOutput ObjectPoseProviderExample::requestObjects(
99  const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
100  {
101  ARMARX_INFO << "Requested object IDs for " << input.relativeTimeoutMS << " ms: "
102  << input.objectIDs;
103  {
104  std::scoped_lock lock(requestedObjectsMutex);
105  requestedObjects.requestObjects(input.objectIDs, input.relativeTimeoutMS);
106  }
107 
108  objpose::provider::RequestObjectsOutput output;
109  // All requests are successful.
110  for (const auto& id : input.objectIDs)
111  {
112  output.results[id].success = true;
113  }
114  return output;
115  }
116 
117  void ObjectPoseProviderExample::poseEstimationTaskRun()
118  {
119  CycleUtil cycle(50);
121 
122  std::map<ObjectID, ObjectInfo> objectInfos;
123 
124  while (poseEstimationTask and not poseEstimationTask->isStopped())
125  {
127  float t = float((now - start).toSecondsDouble());
128 
130  {
131  std::scoped_lock lock(requestedObjectsMutex);
132  update = requestedObjects.updateRequestedObjects(now);
133  }
134 
135  if (update.added.size() > 0 || update.removed.size() > 0)
136  {
137  ARMARX_INFO << "Added: " << update.added
138  << "Removed: " << update.removed;
139  }
140 
141  int i = 0;
143  for (const ObjectID& id : update.current)
144  {
145  if (objectInfos.count(id) == 0)
146  {
147  if (std::optional<ObjectInfo> info = objectFinder.findObject(id))
148  {
149  objectInfos.emplace(id, *info);
150  }
151  else
152  {
153  ARMARX_WARNING << "Could not find object class '" << id << "'.";
154  }
155  }
156  const ObjectInfo& info = objectInfos.at(id);
157 
158  armarx::objpose::ProvidedObjectPose& pose = poses.emplace_back();
159  pose.providerName = getName();
160  pose.objectType = objpose::ObjectType::KnownObject;
161 
162  pose.objectID = info.id();
163 
164  Eigen::Vector3f pos = 200 * Eigen::Vector3f(std::sin(t - i), std::cos(t - i), 1 + i);
165  Eigen::AngleAxisf ori((t - i) / M_PI_2, Eigen::Vector3f::UnitZ());
166  pose.objectPose = simox::math::pose(pos, ori);
167  // pose.objectPoseFrame = armarx::GlobalFrame;
168  pose.objectPoseFrame = "DepthCamera";
169 
170  pose.objectPoseGaussian = objpose::PoseManifoldGaussian();
171  pose.objectPoseGaussian->mean = pose.objectPose;
172  pose.objectPoseGaussian->covariance = Eigen::Matrix6f::Identity();
173  // Translational (co)variance.
174  const float posVar = 10 + 10 * std::sin(t - i);
175  pose.objectPoseGaussian->covariance.diagonal()(0) = 1.0 * posVar;
176  pose.objectPoseGaussian->covariance.diagonal()(1) = 3.0 * posVar;
177  pose.objectPoseGaussian->covariance.diagonal()(2) = 2.0 * posVar;
178  if (i % 2 == 1)
179  {
180  Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
181  pose.objectPoseGaussian->positionCovariance()
182  = rot * pose.objectPoseGaussian->positionCovariance() * rot.transpose();
183  }
184 
185  // Rotational (co)variance.
186  const float oriVar = (M_PI_4) + (M_PI_4) * std::sin(t - i);
187  pose.objectPoseGaussian->covariance.diagonal()(3) = 1.0 * oriVar;
188  pose.objectPoseGaussian->covariance.diagonal()(4) = 4.0 * oriVar;
189  pose.objectPoseGaussian->covariance.diagonal()(5) = 2.0 * oriVar;
190  if (i % 2 == 1)
191  {
192  Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
193  pose.objectPoseGaussian->orientationCovariance()
194  = rot * pose.objectPoseGaussian->orientationCovariance() * rot.transpose();
195  }
196 
197  pose.confidence = 0.75 + 0.25 * std::sin(t - i);
198  pose.timestamp = DateTime::Now();
199 
200  i++;
201  }
202 
203  ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
204  objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
205 
206  if (singleShot)
207  {
208  break;
209  }
210 
211  cycle.waitForCycleDuration();
212  }
213  }
214 }
armarx::ObjectPoseProviderExample::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ObjectPoseProviderExample.cpp:39
armarx::SimpleRunningTask
Usage:
Definition: TaskUtil.h:70
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::objpose::RequestedObjects::requestObjects
void requestObjects(const std::vector< armarx::data::ObjectID > &objectIDs, long relativeTimeOutMS)
Definition: RequestedObjects.cpp:14
ice_conversions.h
armarx::objpose::ProvidedObjectPose::objectID
armarx::ObjectID objectID
The object ID, i.e. dataset, class name and instance name.
Definition: ProvidedObjectPose.h:47
armarx::ObjectPoseProviderExample::requestObjects
objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput &input, const Ice::Current &) override
Definition: ObjectPoseProviderExample.cpp:98
armarx::armem::attachment::ObjectID
armem::MemoryID ObjectID
Definition: types.h:79
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
Pose.h
armarx::ObjectPoseProviderExample::onExitComponent
void onExitComponent() override
Definition: ObjectPoseProviderExample.cpp:89
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::objpose::ProvidedObjectPose::objectPoseFrame
std::string objectPoseFrame
Definition: ProvidedObjectPose.h:50
armarx::ObjectPoseProviderPluginUser::objectPoseTopic
objpose::ObjectPoseTopicPrx objectPoseTopic
Definition: ObjectPoseProviderPlugin.h:56
armarx::objpose::RequestedObjects::updateRequestedObjects
Update updateRequestedObjects()
Definition: RequestedObjects.cpp:43
armarx::toIce
void toIce(std::map< IceKeyT, IceValueT > &iceMap, const boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:15
ObjectPoseProviderExample.h
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::ObjectFinder::findAllObjectsOfDataset
std::vector< ObjectInfo > findAllObjectsOfDataset(const std::string &dataset, bool checkPaths=true) const
Definition: ObjectFinder.cpp:201
FramedPose.h
armarx::objpose::ProvidedObjectPose::objectPoseGaussian
std::optional< PoseManifoldGaussian > objectPoseGaussian
Definition: ProvidedObjectPose.h:51
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
armarx::ObjectPoseProviderExample::onConnectComponent
void onConnectComponent() override
Definition: ObjectPoseProviderExample.cpp:76
armarx::objpose::ProvidedObjectPose::providerName
std::string providerName
Name of the providing component.
Definition: ProvidedObjectPose.h:40
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
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
armarx::objpose::ProvidedObjectPoseSeq
std::vector< ProvidedObjectPose > ProvidedObjectPoseSeq
Definition: forward_declarations.h:25
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
CycleUtil.h
armarx::objpose::ProvidedObjectPose
An object pose provided by an ObjectPoseProvider.
Definition: ProvidedObjectPose.h:25
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::objpose::toIce
objpose::AABB toIce(const simox::AxisAlignedBoundingBox &aabb)
Definition: ice_conversions.cpp:94
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
armarx::objpose::ProvidedObjectPose::objectType
ObjectType objectType
Known or unknown object.
Definition: ProvidedObjectPose.h:42
ProvidedObjectPose.h
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::ObjectPoseProviderExample::getDefaultName
std::string getDefaultName() const override
Definition: ObjectPoseProviderExample.cpp:53
armarx::ObjectFinder::findObject
std::optional< ObjectInfo > findObject(const std::string &dataset, const std::string &name) const
Definition: ObjectFinder.cpp:65
armarx::objpose::RequestedObjects::Update
Definition: RequestedObjects.h:27
armarx::ObjectPoseProviderExample::onDisconnectComponent
void onDisconnectComponent() override
Definition: ObjectPoseProviderExample.cpp:85
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::objpose::ProvidedObjectPose::confidence
float confidence
Confidence in [0, 1] (1 = full, 0 = none).
Definition: ProvidedObjectPose.h:58
armarx::objpose::ProvidedObjectPose::timestamp
DateTime timestamp
Source timestamp.
Definition: ProvidedObjectPose.h:60
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::objpose::ProvidedObjectPose::objectPose
Eigen::Matrix4f objectPose
Definition: ProvidedObjectPose.h:49
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::ObjectPoseProviderExample::getProviderInfo
objpose::ProviderInfo getProviderInfo(const Ice::Current &=Ice::emptyCurrent) override
Definition: ObjectPoseProviderExample.cpp:93
armarx::ObjectPoseProviderExample::onInitComponent
void onInitComponent() override
Definition: ObjectPoseProviderExample.cpp:58