From f3159e8c60629ed451c89fdde36ddde019393870 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:28:07 +0200
Subject: [PATCH] dynamic_scene_provider component: tested on ARMAR-6; minor
fixes
---
.../dynamic_scene_provider/CMakeLists.txt | 2 +-
.../dynamic_scene_provider/Component.cpp | 24 ++++++++++++++++---
.../dynamic_scene_provider/Component.h | 5 ++++
3 files changed, 27 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 39d89c1a..e061ea23 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -35,7 +35,7 @@ armarx_add_component(dynamic_scene_provider
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
- DEPENDENCIES_PRIVATE
+ # DEPENDENCIES_PRIVATE
range-v3::range-v3
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index e8abd88f..4a1cd5e8 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -53,6 +53,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(virtualRobotReaderPlugin);
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
+ addPlugin(humanWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -86,6 +87,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
def->optional(properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
def->optional(properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
+
+ def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
return def;
}
@@ -141,12 +144,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
task = new PeriodicTask<Component>(
this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+ task->start();
}
void
Component::onDisconnectComponent()
{
+ task->stop();
}
@@ -189,12 +194,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Human
//
- const armem::human::client::Reader::Query humanPoseQuery{.providerName = "", // all
+ ARMARX_INFO << "Querying humans";
+
+ const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
.timestamp = timestamp};
const armem::human::client::Reader::Result humanPoseResult =
humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success);
+ ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success) << humanPoseResult.errorMessage;
ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
@@ -202,6 +209,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
+ ARMARX_INFO << "Querying laser scanner features";
+
const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
@@ -219,6 +228,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Objects in the scene (both static and dynamic)
//
+ ARMARX_INFO << "Querying object poses";
+
const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
// remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
@@ -243,8 +254,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Costmaps
//
+ ARMARX_INFO << "Querying costmap";
+
const memory::client::costmap::Reader::Query costmapQuery{.providerName =
- "navigator", // TODO check
+ "distance_to_obstacle_costmap_provider", // TODO check
.name = "distance_to_obstacles",
.timestamp = timestamp};
@@ -319,8 +332,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
// here ends: data fetching
+ ARMARX_INFO << "Running human tracker";
+
humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
.humanPoses = humanPoseResult.humanPoses});
+
+
+ humanWriterPlugin->get().store(humanTracker.getTrackedHumans(),getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 171b8638..bcc3ad7d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -49,6 +49,7 @@
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
@@ -151,6 +152,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
float freespaceThreshold = 0.45F;
float occupiedThreshold = 0.55;
} occupancyGrid;
+
+ std::string humanPoseProvider = "OpenNIPointCloudProvider";
};
Properties properties;
/* Use a mutex if you access variables from different threads
@@ -199,6 +202,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
occupancyGridReaderPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
HumanTracker humanTracker;
};
--
GitLab