diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt index 1767c9cebd878b1d15cad6f8043a2bd7cf932814..cc81788baa40cf3089de08f3699ac8e9cf026c36 100644 --- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt @@ -19,6 +19,7 @@ armarx_add_library( ./client/laser_scans/Writer.h ./client/occupancy_grid/Reader.h ./client/occupancy_grid/Writer.h + ./OccupancyGridHelper.h SOURCES ./aron_conversions.cpp ./client/laser_scans/Reader.cpp diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h index 17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3..4b936b6e5bbc7de8141ea380efdac63f3e8d14ad 100644 --- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h +++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h @@ -25,7 +25,7 @@ namespace armarx public: using Params = detail::OccupancyGridHelperParams; - OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params); + OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params = Params()); using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>; diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp index 717a2ec98e5283515a5454bfa31eb7c00636b01c..fe59e94773e8f1a82396705e43be5e51c57c08b0 100644 --- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp +++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp @@ -48,6 +48,7 @@ namespace armarx::armem::vision::occupancy_grid::client { + Reader::~Reader() = default; armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const { @@ -55,7 +56,7 @@ namespace armarx::armem::vision::occupancy_grid::client // clang-format off qb - .coreSegments().withName(properties().memoryName) + .coreSegments().withName(properties().coreSegmentName) .providerSegments().withName(query.providerName) .entities().all() .snapshots().beforeOrAtTime(query.timestamp); @@ -63,6 +64,20 @@ namespace armarx::armem::vision::occupancy_grid::client return qb; } + + std::string Reader::propertyPrefix() const + { + return "mem.vision.occupancy_grid."; + } + + armarx::armem::client::util::SimpleReaderBase::Properties Reader::defaultProperties() const + { + return + { + .memoryName = "Vision", + .coreSegmentName = "OccupancyGrid" + }; + } OccupancyGrid asOccupancyGrid(const wm::ProviderSegment& providerSegment) { @@ -116,9 +131,19 @@ namespace armarx::armem::vision::occupancy_grid::client .errorMessage = qResult.errorMessage}; } - // now create result from memory - const wm::ProviderSegment& providerSegment = qResult.memory.getCoreSegment(properties().memoryName) - .getProviderSegment(query.providerName); + const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName); + + if(not coreSegment.hasProviderSegment(query.providerName)) + { + ARMARX_WARNING << "Provider segment `" << query.providerName << "` does not exist (yet)."; + return + { + .occupancyGrid = std::nullopt, + .status = Result::Status::NoData + }; + } + + const wm::ProviderSegment& providerSegment = coreSegment.getProviderSegment(query.providerName); if (providerSegment.empty()) { diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp index ce5883afb6eeb72c7ebb9337d0b4980eee7e462e..3275d888db9c8841e1dd7a8baf45582d27ab03ed 100644 --- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp +++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp @@ -10,12 +10,12 @@ namespace armarx::armem::vision::occupancy_grid::client bool Writer::store(const OccupancyGrid& grid, const std::string& frame, - const std::string& agentName, + const std::string& providerName, const std::int64_t& timestamp) { std::lock_guard g{memoryWriterMutex()}; - const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName); + const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName); if (not result.success) { diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h index b2a900321224d2c4617cab541c7b1cf77f41d1f1..df5e02ceaa5b2034e0028e7625c616924af8a2bf 100644 --- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h +++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h @@ -49,7 +49,7 @@ namespace armarx::armem::vision::occupancy_grid::client bool store(const OccupancyGrid& grid, const std::string& frame, - const std::string& agentName, + const std::string& providerName, const std::int64_t& timestamp); protected: diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h index dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85..00fb545a104afb19c38c9d7b01fd864ed6ba1fe6 100644 --- a/source/RobotAPI/libraries/armem_vision/types.h +++ b/source/RobotAPI/libraries/armem_vision/types.h @@ -45,7 +45,7 @@ namespace armarx::armem // template<typename _ValueT = float> struct OccupancyGrid { - float resolution; + float resolution; // [mm] std::string frame; Eigen::Affine3f pose;