diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 40f41ce28de7d2e0dcbb2d1eee057fae55c3ad2a..4cf2854911ac53fbcf20c1cae6854f9419072c6a 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -56,7 +56,7 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
@@ -66,8 +66,9 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include "armarx/navigation/server/execution/ExecutorInterface.h"
-#include "armarx/navigation/server/scene_provider/SceneProvider.h"
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
@@ -123,6 +124,7 @@ namespace armarx::navigation::components::navigator
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
+ addPlugin(humanReaderPlugin);
addPlugin(virtualRobotReaderPlugin);
@@ -164,6 +166,7 @@ namespace armarx::navigation::components::navigator
.graphReader = &graphReaderPlugin->get(),
.costmapReader = &costmapReaderPlugin->get(),
.virtualRobotReader = &virtualRobotReaderPlugin->get(),
+ .humanReader = &humanReaderPlugin->get(),
.objectPoseClient = ObjectPoseClientPluginUser::getClient()};
const std::string robotName = getControlComponentPlugin()
@@ -190,7 +193,7 @@ namespace armarx::navigation::components::navigator
// memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
- navRemoteGui = std::make_unique<navigator::RemoteGui>(remoteGui, *this);
+ navRemoteGui.emplace(remoteGui, *this);
navRemoteGui->enable();
initialized = true;
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index 8b2b34677076c560e6d37b889a0d8d6ee61122ff..316e4da539d2c35021bcc9e1f325f0af72a1de22 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -50,6 +50,8 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <armarx/control/client/ComponentPlugin.h>
+
+#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
@@ -174,9 +176,7 @@ namespace armarx::navigation::components::navigator
std::mutex propertiesMutex;
- // TODO maybe as optional, but requires some effort
- std::unique_ptr<armarx::navigation::components::navigator::RemoteGui> navRemoteGui;
-
+ std::optional<armarx::navigation::components::navigator::RemoteGui> navRemoteGui;
// unique_ptr to avoid dangling refs
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
@@ -194,6 +194,8 @@ namespace armarx::navigation::components::navigator
graphReaderPlugin = nullptr;
armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
costmapReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::human::Reader>*
+ humanReaderPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
@@ -217,4 +219,4 @@ namespace armarx::navigation::components::navigator
Parameters params;
};
-} // namespace armarx::navigation::components::navigator
+} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 05f211a9a6a4d40e243ab560a437814d3eb02b94..172957d00e1adb5b42a1bb47ca9fea7f80083b17 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -23,17 +23,14 @@
#pragma once
+#include <armarx/navigation/human/types.h>
+
namespace armarx::navigation::core
{
struct DynamicScene
{
-
- // TODO(SALt): Implement
-
- public:
- protected:
- private:
+ human::Humans humans;
};
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 6adb56142ff59a87a2110066df1ab519f1440ed4..1e395628f619f218d3c5721e5d71fb68e9462658 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -39,7 +39,7 @@ namespace armarx::navigation::local_planning
obst->finalizePolygon();
container.push_back(obst);
- if (visLayer)
+ if (visLayer != nullptr)
{
const Eigen::Vector3f min3d = conv::fromRos(min);
const Eigen::Vector3f max3d = conv::fromRos(max);
@@ -58,7 +58,7 @@ namespace armarx::navigation::local_planning
{
auto proxemicZones = proxemics.createProxemicZones(human);
- for (auto& proxemicZone : proxemicZones)
+ for (const auto& proxemicZone : proxemicZones)
{
auto pose = conv::toRos(proxemicZone.pose);
auto shape = conv::toRos(proxemicZone.shape);
@@ -67,7 +67,7 @@ namespace armarx::navigation::local_planning
auto obst = boost::make_shared<teb_local_planner::extension::EllipseObstacle>(
pose.position(), pose.theta(), shape.a, shape.b);
- auto& penalty = proxemicZone.penalty;
+ const auto& penalty = proxemicZone.penalty;
obst->setPenaltyModel(boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
teb_local_planner::LinearPenaltyModel(penalty.minDistance, penalty.epsilon),
@@ -83,7 +83,7 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
- if (visLayer)
+ if (visLayer != nullptr)
{
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
const core::Pose pose3d = conv::to3D(human.pose);
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 54c9700c29599ad4698eef95134986e1e435af80..b9a9d0b3abefe7ec13ac6b185feb2da0495c2627 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -145,7 +145,7 @@ namespace armarx::navigation::local_planning
viz::Layer* visPtr = nullptr;
viz::Layer visLayer;
- if (arviz)
+ if (arviz != nullptr)
{
visLayer = arviz->layer("local_planner_obstacles");
visPtr = &visLayer;
@@ -154,6 +154,7 @@ namespace armarx::navigation::local_planning
if (scene.staticScene)
{
+ ARMARX_CHECK(scene.staticScene.has_value());
for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
{
obstManager.addBoxObstacle(obst->getGlobalBoundingBox(), visPtr);
@@ -161,14 +162,14 @@ namespace armarx::navigation::local_planning
}
if (scene.dynamicScene)
{
- //TODO include humans in dynamic scene
- //for (const auto& obst : scene.dynamicScene.value().humans)
- //{
- // obstManager.addHumanObstacle(obst, visPtr);
- //}
+ ARMARX_CHECK(scene.dynamicScene.has_value());
+ for (const auto& obst : scene.dynamicScene.value().humans)
+ {
+ obstManager.addHumanObstacle(obst, visPtr);
+ }
}
- if (arviz)
+ if (arviz != nullptr)
{
arviz->commit(visLayer);
}
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 6ab93b8fa535db0a94e98160f6b6d3c1dcd4d5ba..26d9cd41fbe72fc72082b7d9ffc5d08e06e78d02 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -2,17 +2,20 @@
#include <VirtualRobot/SceneObjectSet.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_robot/types.h>
-#include "armarx/navigation/core/types.h"
-#include "armarx/navigation/memory/client/costmap/Reader.h"
#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/util/util.h>
+
namespace armarx::navigation::server::scene_provider
{
@@ -113,7 +116,10 @@ namespace armarx::navigation::server::scene_provider
core::DynamicScene
SceneProvider::getDynamicScene(const DateTime& timestamp) const
{
- return {}; // FIXME implement
+ const memory::client::human::Reader::Query query{.providerName = config.humanProviderName,
+ .timestamp = timestamp};
+
+ return {.humans = srv.humanReader->queryHumans(query).humans};
}
core::SceneGraph
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
index a71a958fd5f3e1997ee8defc2a95142e9ed281be..1f21e85ead9be31ca561a925e2bcceff0be53567 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.h
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -31,12 +31,28 @@
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
-#include <armarx/navigation/memory/client/costmap/Reader.h>
-#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+namespace armarx::navigation::memory::client
+{
+ namespace graph
+ {
+ class Reader;
+ }
+ namespace costmap
+ {
+ class Reader;
+ }
+ namespace human
+ {
+ class Reader;
+ }
+} // namespace armarx::navigation::memory::client
+
+
namespace armarx::navigation::server::scene_provider
{
@@ -54,6 +70,8 @@ namespace armarx::navigation::server::scene_provider
// `robot_state` memory reader and writer
armem::robot_state::VirtualRobotReader* virtualRobotReader;
+ memory::client::human::Reader* humanReader;
+
objpose::ObjectPoseClient objectPoseClient;
};
@@ -63,6 +81,8 @@ namespace armarx::navigation::server::scene_provider
std::string staticCostmapProviderName = "distance_to_obstacle_costmap_provider";
std::string staticCostmapName = "distance_to_obstacles";
+
+ std::string humanProviderName = "dynamic_scene_provider";
};
SceneProvider(const InjectedServices& srv, const Config& config);