diff --git a/source/RobotAPI/drivers/GamepadUnit/README.md b/source/RobotAPI/drivers/GamepadUnit/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..f987b05e9a80bc59741e77b07502516f7de3d842
--- /dev/null
+++ b/source/RobotAPI/drivers/GamepadUnit/README.md
@@ -0,0 +1,17 @@
+# GamepadUnit
+
+This component is used to control the robot with a gamepad.
+
+You have to set two device names:
+ - `GamepadDeviceName`
+ - Used to read the input from the device.
+ You can find the proper name by checking various names from `/dev/input/js*` with the command line util `jstest`.
+ - `GamepadForceFeedbackName`
+ - Used to give force feedback on the controller.
+ You can find the proper name by checking various names from `/dev/input/event*` with the command line util `fftest` or `evtest`.
+
+Alternatively you can find the proper name through `/dev/input/by-id`.
+
+E.g. for a Logitech F710 gamepad the proper names are: \
+`GamepadDeviceName=/dev/input/by-id/usb-Logitech_Wireless_Gamepad_F710_582F9A6B-joystick` \
+`GamepadForceFeedbackName=/dev/input/by-id/usb-Logitech_Wireless_Gamepad_F710_582F9A6B-event-joystick`
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index eed9e12ee2b2d5870ef6f0262bd2f23bda92fb9e..74a2ece78b8db2462731d6460836387d143cf514 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,16 +19,12 @@ armarx_add_library(
./aron_conversions.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
- ./client/laser_scanner_features/Reader.h
- ./client/laser_scanner_features/Writer.h
./OccupancyGridHelper.h
constants.h
SOURCES
./aron_conversions.cpp
./client/occupancy_grid/Reader.cpp
./client/occupancy_grid/Writer.cpp
- ./client/laser_scanner_features/Reader.cpp
- ./client/laser_scanner_features/Writer.cpp
./OccupancyGridHelper.cpp
)
@@ -36,7 +32,6 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
- aron/LaserScannerFeatures.xml
aron/OccupancyGrid.xml
)
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
deleted file mode 100644
index 32600d7d19eef8f3969bde99c4a66d60635cf4c6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
+++ /dev/null
@@ -1,67 +0,0 @@
-<!--Some fancy comment -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- </AronIncludes>
-
- <GenerateTypes>
-
- <Object name='armarx::armem::vision::arondto::Ellipsoid'>
- <ObjectChild key='globalPose'>
- <Pose />
- </ObjectChild>
- <ObjectChild key='radii'>
- <Matrix rows="2" cols="1" type="float32" />
- </ObjectChild>
- </Object>
-
- <Object name='armarx::armem::vision::arondto::Circle'>
- <ObjectChild key='center'>
- <Matrix rows="2" cols="1" type="float32" />
- </ObjectChild>
- <ObjectChild key='radius'>
- <float />
- </ObjectChild>
- </Object>
-
- <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
- <ObjectChild key="convexHull">
- <List>
- <Matrix rows="2" cols="1" type="float32" />
- </List>
- </ObjectChild>
- <ObjectChild key="circle">
- <armarx::armem::vision::arondto::Circle/>
- </ObjectChild>
- <ObjectChild key="ellipsoid">
- <armarx::armem::vision::arondto::Ellipsoid/>
- </ObjectChild>
- <!-- <ObjectChild key="chain">
- <armarx::armem::vision::arondto::Chain/>
- </ObjectChild> -->
- <ObjectChild key="points">
- <List>
- <Matrix rows="2" cols="1" type="float32" />
- </List>
- </ObjectChild>
- </Object>
-
- <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
- <ObjectChild key="frame">
- <String/>
- </ObjectChild>
- <ObjectChild key="frameGlobalPose">
- <Pose/>
- </ObjectChild>
- <ObjectChild key="features">
- <List>
- <armarx::armem::vision::arondto::LaserScannerFeature />
- </List>
- </ObjectChild>
- </Object>
-
-
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 86a6c62c0e047f528a5b4eb649056659886d18d8..7051c186ac290a3e71fe3cbb435d1b9ab09b997d 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -4,34 +4,18 @@
#include <cstdint>
#include <iterator>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
#include "types.h"
-
namespace armarx::armem::vision
{
/************ toAron ************/
- // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
- // {
- // aronLaserScan.scan = toAron(laserScan);
- // }
-
- int64_t
- toAron(const armem::Time& timestamp)
- {
- return timestamp.toMicroSecondsSinceEpoch();
- }
-
-
void
toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
{
@@ -50,70 +34,4 @@ namespace armarx::armem::vision
// bo.grid is NdArray -> need special handling.
}
- // LaserScannerFeatures
-
-
- void
- toAron(arondto::Circle& dto, const Circle& bo)
- {
- dto.center = bo.center;
- dto.radius = bo.radius;
- }
-
- void
- toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
- {
- dto.globalPose = bo.pose.matrix();
- dto.radii = bo.radii;
- }
-
- void
- toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
- {
- toAron(dto.circle, bo.circle);
- dto.convexHull = bo.convexHull;
- toAron(dto.ellipsoid, bo.ellipsoid);
- dto.points = bo.points;
- }
-
- void
- toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
- {
- aron::toAron(dto.frame, bo.frame);
- aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
- aron::toAron(dto.features, bo.features);
- }
-
- void
- fromAron(const arondto::Circle& dto, Circle& bo)
- {
- bo.center = dto.center;
- bo.radius = dto.radius;
- }
- void
- fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
- {
- bo.pose = dto.globalPose;
- bo.radii = dto.radii;
- }
-
- void
- fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
- {
- bo.convexHull = dto.convexHull;
- fromAron(dto.circle, bo.circle);
- fromAron(dto.ellipsoid, bo.ellipsoid);
-
- // bo.chain = dto.chain;
- bo.points = dto.points;
- }
-
- void
- fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
- {
- aron::fromAron(dto.frame, bo.frame);
- aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
- aron::fromAron(dto.features, bo.features);
- }
-
} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 4d70758f166b66666b9feb22b2dd2a754082bb76..f6f1a53225698f2617559073db3f19c3351b1dd1 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -21,9 +21,7 @@
#pragma once
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
@@ -33,42 +31,6 @@
namespace armarx::armem::vision
{
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace arondto
-
- // struct LaserScan;
- struct LaserScanStamped;
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName);
-
- template <typename T>
- auto
- fromAron(const aron::data::NDArrayPtr& navigator)
- {
- return aron::data::converter::AronVectorConverter::ConvertToVector<T>(navigator);
- }
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
- void toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScan);
-
- inline aron::data::NDArrayPtr
- toAron(const LaserScan& laserScan)
- {
- using aron::data::converter::AronVectorConverter;
- return AronVectorConverter::ConvertFromVector(laserScan);
- }
-
// OccupancyGrid
void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
@@ -79,8 +41,4 @@ namespace armarx::armem::vision
return aron::data::converter::AronEigenConverter::ConvertFromArray(grid);
}
- // LaserScannerFeatures
- void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
- void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
-
} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
deleted file mode 100644
index 216088b410231671dd49a22a95f9fa2b6ae9362b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-#include "Reader.h"
-
-// STD / STL
-#include <algorithm>
-#include <cstring>
-#include <map>
-#include <optional>
-#include <ostream>
-#include <type_traits>
-#include <utility>
-#include <vector>
-
-// ICE
-#include <IceUtil/Handle.h>
-#include <IceUtil/Time.h>
-
-// Simox
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-// ArmarXCore
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/LogSender.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-// RobotAPI Interfaces
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
-// RobotAPI Aron
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-
-// RobotAPI Armem
-#include <RobotAPI/libraries/armem/client/Query.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
- Reader::~Reader() = default;
-
-
- void
- Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- // registerPropertyDefinitions(def);
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void
- Reader::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
- << "' ...";
- try
- {
- memoryReader =
- memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
- }
-
- armarx::armem::client::query::Builder
- Reader::buildQuery(const Query& query) const
- {
- armarx::armem::client::query::Builder qb;
-
- qb.coreSegments()
- .withName(properties.coreSegmentName)
- .providerSegments()
- .withName(query.providerName)
- .entities()
- .all()
- .snapshots()
- .beforeOrAtTime(query.timestamp);
-
- // auto entitySel = [&]()
- // {
- // if (query.name.empty())
- // {
- // ARMARX_INFO << "querying all entities";
- // return sel.entities().all();
- // }
- // return sel.entities().withName(query.name);
- // }();
-
- // entitySel.snapshots().beforeOrAtTime(query.timestamp);
-
- return qb;
- }
-
- std::vector<LaserScannerFeatures>
- asFeaturesList(const wm::ProviderSegment& providerSegment)
- {
- if (providerSegment.empty())
- {
- ARMARX_WARNING << "No entities!";
- return {};
- }
-
- // const auto convert = [](const auto& aronLaserScanStamped,
- // const wm::EntityInstance& ei) -> LaserScanStamped
- // {
- // LaserScanStamped laserScanStamped;
- // fromAron(aronLaserScanStamped, laserScanStamped);
-
- // const auto ndArrayNavigator =
- // aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
-
- // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
-
- // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
- // ARMARX_IMPORTANT << "4";
-
- // return laserScanStamped;
- // };
-
- std::vector<LaserScannerFeatures> laserScannerFeatures;
-
- // loop over all entities and their snapshots
- providerSegment.forEachEntity(
- [&](const wm::Entity& entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
- {
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance(
- [&](const wm::EntityInstance& entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
- {
- LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
- fromAron(o.value(), f);
- }
- return true;
- });
- return true;
- });
-
- return laserScannerFeatures;
- }
-
- Reader::Result
- Reader::queryData(const Query& query) const
- {
- const auto qb = buildQuery(query);
-
- ARMARX_DEBUG << "[MappingDataReader] query ... ";
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
-
- if (not qResult.success)
- {
- ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
- return {.features = {},
- .status = Result::Status::Error,
- .errorMessage = qResult.errorMessage};
- }
-
- // now create result from memory
- const wm::ProviderSegment& providerSegment =
- qResult.memory.getCoreSegment(properties.coreSegmentName)
- .getProviderSegment(query.providerName);
-
- const auto features = asFeaturesList(providerSegment);
-
- // const auto laserScans = asLaserScans(providerSegment);
- // std::vector<std::string> sensors;
- // providerSegment.forEachEntity(
- // [&sensors](const wm::Entity& entity)
- // {
- // sensors.push_back(entity.name());
- // return true;
- // });
-
- return {.features = features,
- // .sensors = sensors,
- .status = Result::Status::Success,
- .errorMessage = ""};
- }
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
deleted file mode 100644
index 4752ca85cf3df286b3d1378c2a0526f5a89f1a31..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/time/DateTime.h>
-
-#include <RobotAPI/libraries/armem/client.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
-
-
-namespace armarx
-{
- class ManagedIceObject;
-}
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
-
- /**
- * @defgroup Component-ExampleClient ExampleClient
- * @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
- *
- * @class ExampleClient
- * @ingroup Component-ExampleClient
- * @brief Brief description of class ExampleClient.
- *
- * Detailed description of class ExampleClient.
- */
- class Reader
- {
- public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Reader();
-
- void connect();
-
- struct Query
- {
- std::string providerName;
-
- std::string name; // sensor name
-
- armarx::core::time::DateTime timestamp;
-
- // std::vector<std::string> sensorList;
- };
-
- struct Result
- {
- std::vector<LaserScannerFeatures> features;
-
- // std::vector<std::string> sensors;
-
- enum Status
- {
- Error,
- Success
- } status;
-
- std::string errorMessage;
- };
-
- Result queryData(const Query& query) const;
-
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
-
- protected:
- armarx::armem::client::query::Builder buildQuery(const Query& query) const;
-
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Reader memoryReader;
-
- // Properties
- struct Properties
- {
- std::string memoryName = "Vision";
- std::string coreSegmentName = "LaserScannerFeatures";
- } properties;
-
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
- };
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
deleted file mode 100644
index 8ae7ba4379288420d3578d7f30ae9bc9c58c559d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-#include "Writer.h"
-
-#include "RobotAPI/libraries/armem_vision/constants.h"
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
- Writer::~Writer() = default;
-
-
- void
- Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- // def->optional(properties.coreSegmentName,
- // prefix + "CoreSegment",
- // "Name of the mapping memory core segment to use.");
-
- // def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void
- Writer::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
- << "' ...";
- try
- {
- memoryWriter =
- memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
-
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
- }
-
- bool
- Writer::store(const LaserScannerFeatures& features,
- const std::string& providerName,
- const Time& timestamp)
- {
- std::lock_guard g{memoryWriterMutex};
-
- // const auto result = memoryWriter.addSegment(constants::memoryName,
- // constants::laserScannerFeaturesCoreSegment);
-
- // if (not result.success)
- // {
- // ARMARX_ERROR << result.errorMessage;
-
- // // TODO(fabian.reister): message
- // return false;
- // }
-
- const auto entityID = armem::MemoryID()
- .withMemoryName(constants::memoryName)
- .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
- .withProviderSegmentName(providerName)
- .withEntityName(features.frame)
- .withTimestamp(timestamp);
-
- ARMARX_VERBOSE << "Memory id is " << entityID.str();
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- ARMARX_TRACE;
-
- arondto::LaserScannerFeatures dto;
- toAron(dto, features);
-
- ARMARX_TRACE;
-
- update.instancesData = {dto.toAron()};
- update.referencedTime = timestamp;
-
- ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
-
- ARMARX_TRACE;
- armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
-
- ARMARX_DEBUG << updateResult;
-
- if (not updateResult.success)
- {
- ARMARX_ERROR << updateResult.errorMessage;
- }
-
- return updateResult.success;
- }
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
deleted file mode 100644
index ca58005772268d70f7eedf06189c02250c75fef6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include "RobotAPI/libraries/armem_vision/types.h"
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- /**
- * @defgroup Component-ExampleClient ExampleClient
- * @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
- *
- * @class ExampleClient
- * @ingroup Component-ExampleClient
- * @brief Brief description of class ExampleClient.
- *
- * Detailed description of class ExampleClient.
- */
- class Writer
- {
- public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Writer();
-
-
- void connect();
-
- // MappingDataWriterInterface
- /// to be called in Component::onConnectComponent
- // void connect() override;
-
- /// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
-
- bool store(const LaserScannerFeatures& features,
- const std::string& providerName,
- const Time& timestamp);
-
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Writer memoryWriter;
-
- // Properties
- struct Properties
- {
- // std::string memoryName = "Vision";
- // std::string coreSegmentName = "LaserScannerFeatures";
- } properties;
-
- std::mutex memoryWriterMutex;
-
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
- };
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/constants.h b/source/RobotAPI/libraries/armem_vision/constants.h
index 653d6fda6fc9e787d00f570af5440a6c81c6108b..3474d925b7b548068fc9059d82e5ec469b0f24b3 100644
--- a/source/RobotAPI/libraries/armem_vision/constants.h
+++ b/source/RobotAPI/libraries/armem_vision/constants.h
@@ -28,6 +28,5 @@ namespace armarx::armem::vision::constants
const inline std::string memoryName = "Vision";
// core segments
- const inline std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index fe584e3dc235a19333fd9b174cfab390c1a5d435..1a6b6b7d69eb52cc7857744dfb774564ac984720 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -47,45 +47,5 @@ namespace armarx::armem::vision
Grid grid;
};
- struct Ellipsoid
- {
- Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
-
- Eigen::Vector2f radii = Eigen::Vector2f::Zero();
- };
-
- struct Circle
- {
- Eigen::Vector2f center = Eigen::Vector2f::Zero();
- float radius = 0.F;
- };
-
- struct LaserScannerFeature
- {
- using Points = std::vector<Eigen::Vector2f>;
- using Chain = Points;
-
- Points convexHull;
-
- Circle circle;
- Ellipsoid ellipsoid;
-
- Chain chain;
-
- Points points;
- };
-
- struct LaserScannerFeatures
- {
- // TODO(fabian.reister): framed pose
- std::string frame;
- Eigen::Isometry3f frameGlobalPose;
-
- std::vector<LaserScannerFeature> features;
-
-
- // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
- };
-
} // namespace armarx::armem::vision