From c8017d983d9056ab5563bee1fe5c064208e931e7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 22:56:22 +0200
Subject: [PATCH 1/7] human reader plugin
---
source/armarx/navigation/components/navigator/Component.cpp | 1 +
source/armarx/navigation/components/navigator/Component.h | 6 +++++-
2 files changed, 6 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 40f41ce2..e8612e91 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -123,6 +123,7 @@ namespace armarx::navigation::components::navigator
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
+ addPlugin(humanReaderPlugin);
addPlugin(virtualRobotReaderPlugin);
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index 8b2b3467..7462c3d7 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>
@@ -194,6 +196,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 +221,4 @@ namespace armarx::navigation::components::navigator
Parameters params;
};
-} // namespace armarx::navigation::components::navigator
+} // namespace armarx::navigation::components::navigator
--
GitLab
From 7961d63d548e201b21c71e6f869ba857738d8d6c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 22:56:46 +0200
Subject: [PATCH 2/7] navigator: remotegui as optional
---
source/armarx/navigation/components/navigator/Component.cpp | 2 +-
source/armarx/navigation/components/navigator/Component.h | 4 +---
2 files changed, 2 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index e8612e91..b5dff818 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -191,7 +191,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 7462c3d7..316e4da5 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -176,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;
--
GitLab
From 6f5e92d3c30d35cb4722d7996eb0b92de33cfc71 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 23:15:53 +0200
Subject: [PATCH 3/7] scene provider: adding human reader
---
.../server/scene_provider/SceneProvider.cpp | 20 ++++++++++------
.../server/scene_provider/SceneProvider.h | 24 +++++++++++++++++--
2 files changed, 35 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 6ab93b8f..26d9cd41 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 a71a958f..1f21e85e 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);
--
GitLab
From c126f89b65f8d6055880e0601ebfa0aafa133c78 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 23:16:03 +0200
Subject: [PATCH 4/7] dynamic scene: adding humans
---
source/armarx/navigation/core/DynamicScene.h | 9 +++------
1 file changed, 3 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 05f211a9..172957d0 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
--
GitLab
From 9ba0352267389370bf3d08b92bb9f17dc78be2ef Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 23:16:37 +0200
Subject: [PATCH 5/7] navigator: human readerwriter plugin
---
source/armarx/navigation/components/navigator/Component.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index b5dff818..25dbba4c 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -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>
@@ -165,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()
--
GitLab
From cbd3b908e5f27595793ce9c6c1c2aacca0594734 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 23:17:29 +0200
Subject: [PATCH 6/7] timedelasticbands: activated
obstManager.addHumanObstacle
---
.../local_planning/TimedElasticBands.cpp | 15 ++++++++-------
1 file changed, 8 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 54c9700c..b9a9d0b3 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);
}
--
GitLab
From 6f52f9814edab484754bb4a67da0cb007a03aa03 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 18 Aug 2022 23:17:38 +0200
Subject: [PATCH 7/7] navigator: minor
---
.../armarx/navigation/components/navigator/Component.cpp | 2 +-
.../navigation/local_planning/TebObstacleManager.cpp | 8 ++++----
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 25dbba4c..4cf28549 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>
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 6adb5614..1e395628 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);
--
GitLab