SceneProvider.cpp
Go to the documentation of this file.
1 #include "SceneProvider.h"
2 
3 #include <VirtualRobot/SceneObjectSet.h>
4 
9 
12 
20 
22 {
23 
25  srv(srv), config(config)
26  {
27  }
28 
29  const core::Scene&
31  {
32  return scn;
33  }
34 
35  bool
37  {
38  scn.timestamp = timestamp;
39 
40  // Only load structure. This includes the primitive models (no mesh files though).
41  scn.robot = srv.virtualRobotReader->getRobotWaiting(
42  config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
43  scn.robot->setPrimitiveApproximationModel(config.primitiveApproximationModels);
44 
46 
47  scn.staticScene.emplace(getStaticScene(timestamp));
48  scn.dynamicScene.emplace(getDynamicScene(timestamp));
49  scn.graph = getSceneGraph(timestamp);
50 
51  return true; // TODO(fabian.reister): return false if sync fails
52  }
53 
54  bool
55  SceneProvider::synchronize(const DateTime& timestamp, const bool fullUpdate)
56  {
57  scn.timestamp = timestamp;
58 
61 
62  if (fullUpdate)
63  {
64  scn.staticScene.emplace(getStaticScene(timestamp));
65  }
66  scn.dynamicScene.emplace(getDynamicScene(timestamp));
67  // scn.graph = getSceneGraph(timestamp);
68 
69  const auto platformState =
70  srv.virtualRobotReader->queryPlatformState(scn.robot->getName(), timestamp);
71  ARMARX_CHECK(platformState.has_value())
72  << "Could not get a platform state for ts: " << VAROUT(timestamp);
73 
74  scn.platformVelocity = core::Twist{.linear = platformState->twist.linear,
75  .angular = platformState->twist.angular};
76 
77  return true; // TODO(fabian.reister): return false if sync fails
78  }
79 
81  SceneProvider::getStaticScene(const DateTime& timestamp) const
82  {
84 
86 
87  objpose::ObjectPoseMap objectMap;
88  for (const objpose::ObjectPose& object : objectPoses)
89  {
90  objectMap.emplace(object.objectID, object);
91  }
92 
93  std::vector<ObjectInfo> objectInfo = srv.objectPoseClient.objectFinder.findAllObjects();
94 
95  // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
96  const auto objectPosesStatic =
97  armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
98 
99  const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
100 
101  ARMARX_CHECK_NOT_NULL(objects);
102  ARMARX_INFO << objects->getSize() << " objects in the scene";
103 
104  ARMARX_INFO << "Retrieving costmap in memory";
105 
106  ARMARX_TRACE;
107 
108  const algorithms::Costmap costmap = [&]()
109  {
110  // waiting for static costmap to become available
111  while (true)
112  {
115  .name = config.staticCostmapName,
116  .timestamp = armarx::Clock::Now()};
117 
118 
119  if (const memory::client::costmap::Reader::Result costmap =
120  srv.costmapReader->query(query))
121  {
122  ARMARX_CHECK(costmap.costmap.has_value());
123  ARMARX_INFO << "Static costmap available.";
124  return costmap.costmap.value();
125  }
126 
127  ARMARX_INFO << deactivateSpam(5) << "Static costmap `" << query.name
128  << "` from provider " << query.providerName << " not available yet.";
129  }
130  }();
131 
132  const auto locations = srv.graphReader->locations();
133 
134  ARMARX_INFO << "Retrieved static costmap";
135 
136  return {.objects = objects,
137  .objectMap = std::move(objectMap),
138  .objectInfo = std::move(objectInfo),
139  .distanceToObstaclesCostmap = costmap,
140  .locations = std::move(locations)};
141  }
142 
143  core::DynamicScene
144  SceneProvider::getDynamicScene(const DateTime& timestamp) const
145  {
146  const memory::client::human::Reader::Query queryHumans{
147  .providerName = config.humanProviderName,
148  .timestamp = timestamp,
149  .maxAge = Duration::MilliSeconds(500)};
150 
151  const memory::client::laser_scanner_features::Reader::Query queryFeatures{
152  .providerName = config.laserScannerFeaturesProviderName,
153  .name = {},
154  .timestamp = timestamp};
155 
156  return {.humans = srv.humanReader->queryHumans(queryHumans).humans,
157  .laserScannerFeatures =
158  srv.laserScannerFeaturesReader->queryData(queryFeatures).features};
159  }
160 
161  core::SceneGraph
162  SceneProvider::getSceneGraph(const DateTime& /*timestamp*/) const
163  {
165  return {.subgraphs = srv.graphReader->graphs()};
166  }
167 
168 } // namespace armarx::navigation::server::scene_provider
armarx::navigation::server::scene_provider::SceneProvider::Config::staticCostmapProviderName
std::string staticCostmapProviderName
Definition: SceneProvider.h:90
armarx::navigation::core::Scene::graph
std::optional< core::SceneGraph > graph
Definition: types.h:81
armarx::navigation::core::Scene::robot
VirtualRobot::RobotPtr robot
Definition: types.h:78
armarx::objpose::ObjectPoseClient::fetchObjectPoses
ObjectPoseSeq fetchObjectPoses() const
Fetch all known object poses.
Definition: ObjectPoseClient.cpp:39
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::objectPoseClient
objpose::ObjectPoseClient objectPoseClient
Definition: SceneProvider.h:83
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::graphReader
memory::client::graph::Reader * graphReader
Definition: SceneProvider.h:71
armarx::armem::robot_state::RobotReader::queryPlatformState
std::optional< PlatformState > queryPlatformState(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:336
armarx::navigation::core::Scene::staticScene
std::optional< core::StaticScene > staticScene
Definition: types.h:75
armarx::objpose::ObjectPoseSeq
std::vector< ObjectPose > ObjectPoseSeq
Definition: forward_declarations.h:20
armarx::ObjectFinder::findAllObjects
std::vector< ObjectInfo > findAllObjects(bool checkPaths=true) const
Definition: ObjectFinder.cpp:144
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::virtualRobotReader
armem::robot_state::VirtualRobotReader * virtualRobotReader
Definition: SceneProvider.h:77
CostmapBuilder.h
ARMARX_CHECK_NOT_NULL
#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...
Definition: ExpressionException.h:206
armarx::navigation::server::scene_provider::SceneProvider::Config::primitiveApproximationModels
std::vector< std::string > primitiveApproximationModels
Definition: SceneProvider.h:96
armarx::navigation::memory::client::laser_scanner_features::Reader::queryData
Result queryData(const Query &query) const
Definition: Reader.cpp:130
armarx::objpose::ObjectPoseClient::objectFinder
ObjectFinder objectFinder
Definition: ObjectPoseClient.h:119
Duration.h
Reader.h
armarx::armem::robot_state::VirtualRobotReader::synchronizeRobot
bool synchronizeRobot(VirtualRobot::Robot &robot, const armem::Time &timestamp) const
Definition: VirtualRobotReader.cpp:20
armarx::navigation::server::scene_provider::SceneProvider::Config::robotName
std::string robotName
Definition: SceneProvider.h:88
armarx::navigation::memory::client::costmap::Reader::query
Result query(const Query &query) const
Definition: Reader.cpp:77
armarx::navigation::server::scene_provider::SceneProvider::SceneProvider
SceneProvider(const InjectedServices &srv, const Config &config)
Definition: SceneProvider.cpp:24
armarx::navigation::server::scene_provider
Definition: Navigator.h:57
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::laserScannerFeaturesReader
memory::client::laser_scanner_features::Reader * laserScannerFeaturesReader
Definition: SceneProvider.h:81
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::navigation::memory::client::human::Reader::queryHumans
HumanResult queryHumans(const Query &query) const
Definition: Reader.cpp:200
Reader.h
Clock.h
armarx::navigation::core::Twist
Definition: basic_types.h:53
armarx::navigation::memory::client::costmap::Reader::Query::providerName
std::string providerName
Definition: Reader.h:42
armarx::navigation::core::Scene::platformVelocity
core::Twist platformVelocity
Definition: types.h:79
armarx::navigation::server::scene_provider::SceneProvider::Config::staticCostmapName
std::string staticCostmapName
Definition: SceneProvider.h:91
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::navigation::core::Scene::timestamp
DateTime timestamp
Definition: types.h:73
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::navigation::server::scene_provider::SceneProvider::Config
Definition: SceneProvider.h:86
util.h
ObjectPose.h
armarx::navigation::algorithms::Costmap::value
std::optional< float > value(const Index &index) const
Definition: Costmap.cpp:404
armarx::navigation::util::asSceneObjects
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq &objectPoses)
Definition: util.cpp:146
armarx::navigation::memory::client::costmap::Reader::Query
Definition: Reader.h:40
types.h
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::humanReader
memory::client::human::Reader * humanReader
Definition: SceneProvider.h:79
armarx::navigation::core::Scene
Definition: types.h:71
Reader.h
armarx::navigation::server::scene_provider::SceneProvider::synchronize
bool synchronize(const DateTime &timestamp, bool fullUpdate) override
Definition: SceneProvider.cpp:55
armarx::navigation::server::scene_provider::SceneProvider::scene
const core::Scene & scene() const override
Definition: SceneProvider.cpp:30
armarx::navigation::core::Scene::dynamicScene
std::optional< core::DynamicScene > dynamicScene
Definition: types.h:76
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::navigation::core::Twist::linear
LinearVelocity linear
Definition: basic_types.h:55
armarx::navigation::memory::client::graph::Reader::locations
std::map< std::string, core::Location > locations()
Definition: Reader.cpp:39
armarx::navigation::memory::client::graph::Reader::graphs
std::vector< armarx::navigation::core::Graph > graphs()
Definition: Reader.cpp:61
armarx::navigation::memory::client::human::Reader::HumanResult::humans
armarx::navigation::human::Humans humans
Definition: Reader.h:51
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::navigation::memory::client::laser_scanner_features::Reader::Result::features
std::vector< LaserScannerFeatures > features
Definition: Reader.h:69
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:97
armarx::navigation::server::scene_provider::SceneProvider::Config::laserScannerFeaturesProviderName
std::string laserScannerFeaturesProviderName
Definition: SceneProvider.h:94
armarx::navigation::server::scene_provider::SceneProvider::Config::humanProviderName
std::string humanProviderName
Definition: SceneProvider.h:93
armarx::navigation::server::scene_provider::SceneProvider::initialize
bool initialize(const DateTime &timestamp) override
Definition: SceneProvider.cpp:36
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices::costmapReader
memory::client::costmap::Reader * costmapReader
Definition: SceneProvider.h:73
Logging.h
armarx::navigation::core::StaticScene
Definition: StaticScene.h:40
types.h
Reader.h
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:36
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:55
armarx::navigation::server::scene_provider::SceneProvider::InjectedServices
Definition: SceneProvider.h:68
armarx::navigation::util::filterObjects
objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects, const std::vector< std::string > &datasetDisableList)
Definition: util.cpp:50
SceneProvider.h
armarx::objpose::ObjectPoseMap
std::map< ObjectID, ObjectPose > ObjectPoseMap
Definition: forward_declarations.h:21