diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index fac11b677edbf7fc26cd2538e453981601e94f13..9d9342ab21b51d11fd188a70c5e4cf6066afff24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -43,10 +43,18 @@
<Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
humanName="Platform Unit (obstacle avoiding)"
typeName="PlatformUnitInterfacePrx"
- memberName="obstacleAvoidingPlatformUnit"
- getterName="getObstacleAvoidingPlatformUnit"
- propertyName="ObstacleAvoidingPlatformUnitName"
+ memberName="platformUnit1"
+ getterName="getPlatformUnit1"
+ propertyName="PlatformUnitName1"
propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+ propertyIsOptional="true" />
+ <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+ humanName="Platform Unit (obstacle aware)"
+ typeName="PlatformUnitInterfacePrx"
+ memberName="platformUnit2"
+ getterName="getPlatformUnit2"
+ propertyName="PlatformUnitName2"
+ propertyDefaultValue="ObstacleAwarePlatformUnit"
propertyIsOptional="true" />
<Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
humanName="Platform Unit Observer"
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
index 0a88d089382eebb915104c24ee41339a18dd2f29..cf5e9899e943dd4076c2dd21ca9fe982b3c1a9bc 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -1,27 +1,21 @@
#include "Path.h"
+#include <iterator>
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+
namespace armarx::viz
{
Path& Path::clear()
{
data_->points.clear();
- return *this;
- }
-
- Path& Path::lineColor(Color color)
- {
- data_->lineColor = color;
return *this;
}
- Path& Path::lineColorGlasbeyLUT(std::size_t id, int alpha)
- {
- return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
- }
-
- Path& Path::lineWidth(float w)
+ Path& Path::width(float w)
{
data_->lineWidth = w;
@@ -33,17 +27,21 @@ namespace armarx::viz
auto& points = data_->points;
points.clear();
points.reserve(ps.size());
- for (auto& p : ps)
+
+ std::transform(ps.begin(),
+ ps.end(),
+ std::back_inserter(points),
+ [](const auto & e)
{
- points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
- }
+ return ToBasicVectorType(e);
+ });
return *this;
}
Path& Path::addPoint(Eigen::Vector3f p)
{
- data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ data_->points.emplace_back(ToBasicVectorType(p));
return *this;
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
index 50eec6abe454bbf56bc45053c4d35f5e41ca5bc6..7cfbecbd973a194cef0e0e350a5c7c3c1aedbc13 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -35,17 +35,7 @@ namespace armarx::viz
Path& clear();
- Path& lineColor(Color color);
-
- template<class...Ts>
- Path& lineColor(Ts&& ...ts)
- {
- return lineColor({std::forward<Ts>(ts)...});
- }
-
- Path& lineColorGlasbeyLUT(std::size_t id, int alpha = 255);
-
- Path& lineWidth(float w);
+ Path& width(float w);
Path& points(std::vector<Eigen::Vector3f> const& ps);
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
index 3018a3af51e25222b97c0c60ed06fea65a8c5822..a0190abd75a8438a2371c728a580837d68c3eecc 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -1,9 +1,12 @@
#include "VisualizationPath.h"
+#include <algorithm>
+
#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoPointSet.h>
namespace armarx::viz::coin
{
@@ -27,19 +30,36 @@ namespace armarx::viz::coin
node->addChild(coordinate3);
node->addChild(lineSep);
+
+ // points (use the following, if the points should be drawn in a different color)
+
+ // pclMat = new SoMaterial;
+
+ // SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ // pclMatBind->value = SoMaterialBinding::PER_PART;
+
+ pclCoords = new SoCoordinate3;
+ pclStyle = new SoDrawStyle;
+
+ // node->addChild(pclMat);
+ // node->addChild(pclMatBind);
+ node->addChild(pclCoords);
+ node->addChild(pclStyle);
+ node->addChild(new SoPointSet);
}
bool VisualizationPath::update(ElementType const& element)
{
// set position
- coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+ coordinate3->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
// set color
- const auto lineColor = element.lineColor;
+ const auto lineColor = element.color;
constexpr float toUnit = 1.0F / 255.0F;
- const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
lineMaterial->diffuseColor.setValue(color);
@@ -58,6 +78,20 @@ namespace armarx::viz::coin
const int pointSize = static_cast<int>(element.points.size());
lineSet->numVertices.set1Value(0, pointSize);
+ // points at each node
+ // const std::vector<SbVec3f> colorData(element.points.size(), color);
+
+ // pclMat->diffuseColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->ambientColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->transparency = transparency;
+
+ pclCoords->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
+
+ pclStyle->pointSize = element.lineWidth + 5;
+
return true;
}
} // namespace armarx::viz::coin
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
index e1e446bbe66bc49a6d0c4bdf1999c7f0615f06ec..487ee9df58baeb6b91e91de6fdb3be873511a0af 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -39,9 +39,17 @@ namespace armarx::viz::coin
bool update(ElementType const& element);
+ /// lines
SoCoordinate3* coordinate3;
SoDrawStyle* lineStyle;
SoLineSet* lineSet;
SoMaterial* lineMaterial;
+
+
+ /// points
+ // SoMaterial* pclMat;
+ SoCoordinate3* pclCoords;
+ SoDrawStyle* pclStyle;
+
};
} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e712e30ccb2978fdc7caa0210b919773b7dce46b..41fc3e85f7a6a7250c47fe58c0730973a16480be 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -22,6 +22,8 @@
#include "DynamicObstacleManager.h"
+#include <VirtualRobot/MathTools.h>
+
// STD/STL
#include <string>
#include <vector>
@@ -118,6 +120,7 @@ namespace armarx
void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
{
+ TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
const float length = difference_vec.norm();
const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
@@ -136,10 +139,10 @@ namespace armarx
return;
}
-
{
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ TIMING_START(add_decayable_line_segment_mutex);
// First check own obstacles
for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
{
@@ -155,13 +158,15 @@ namespace armarx
managed_obstacle->line_matches.push_back(line_start);
managed_obstacle->line_matches.push_back(line_end);
managed_obstacle->m_last_matched = IceUtil::Time::now();
+ TIMING_END(add_decayable_line_segment_mutex);
return;
}
}
+ TIMING_END(add_decayable_line_segment_mutex);
}
// Second check the obstacles from the obstacle avoidance
- for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
+ /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
{
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
{published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
@@ -172,7 +177,7 @@ namespace armarx
ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
return;
}
- }
+ }*/
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
@@ -198,6 +203,15 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
m_managed_obstacles.push_back(new_managed_obstacle);
}
+ TIMING_END(add_decayable_line_segment);
+ }
+
+ void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ {
+ for (const auto& line : lines)
+ {
+ add_decayable_line_segment(line.lineStart, line.lineEnd, c);
+ }
}
@@ -218,7 +232,7 @@ namespace armarx
std::lock_guard l(m_managed_obstacles_mutex);
ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
- for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
{
@@ -240,6 +254,36 @@ namespace armarx
}
+ float
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ {
+ std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+ const float sample_step = 5; // in [mm], sample step size towards goal.
+ const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+ float current_distance = safetyRadius;
+
+ while (current_distance < distance_to_goal)
+ {
+ for (const auto man_obstacle : m_managed_obstacles)
+ {
+ Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+ Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ {
+ return current_distance - safetyRadius;
+ }
+ }
+
+ current_distance += sample_step;
+ }
+
+ return std::numeric_limits<float>::infinity();
+ }
+
+
void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
@@ -283,7 +327,6 @@ namespace armarx
}
}
-
// Update obstacle avoidance
int checked_obstacles = 0;
bool updated = false;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 1629b7efe3102dccf85521ac0fac6379e09225cc..5f2d8065a307706ea16789930c12f5165d6044eb 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -69,10 +69,12 @@ namespace armarx
void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index fe21cc4e46558b3a70102592ee77ef56715421f8..f718a45434ec538fbd5d2541fb9413568f3e79f0 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -94,10 +94,25 @@ namespace armarx
float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
- const unsigned int SAMPLES = 40;
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
const float distance = difference_vec.norm();
+ const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
+
+ const Vector2f difference_start_origin = e_origin - line_seg_start;
+ const Vector2f difference_end_origin = e_origin - line_seg_end;
+
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+ if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+
const float step_size = distance / SAMPLES;
const Eigen::Vector2f dir = difference_vec_normed * step_size;
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index e43cbbc7c728405c745783a231484dd9cd154ebb..8c41ace39ea53283c22b3a191ae9cd9ebba2ce3c 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -70,9 +70,15 @@ namespace armarx::armem::server::robot_state
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ const std::string prefix = "mem.";
+
+ workingMemory.name() = "RobotState";
+ defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
const std::string robotUnitPrefix{sensorValuePrefix};
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
@@ -123,7 +129,7 @@ namespace armarx::armem::server::robot_state
ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
- workingMemory.name() = workingMemoryName;
+ // workingMemory.name() = workingMemoryName;
}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 51d1930b83753c195ff7558154092ea025466e4c..3fa53590170e0ff9d288f7f130adae9814ee7446 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..524b650bf5a3767a92a457ab97b26ffcdd903a40
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_add_component(
+ COMPONENT_NAME ObstacleAwarePlatformUnit
+ HEADERS ObstacleAwarePlatformUnit.h
+ SOURCES ObstacleAwarePlatformUnit.cpp
+ COMPONENT_LIBS ArmarXCoreComponentPlugins
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2acb4a23b423df17ca7f80ffc76251634bcb933f
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -0,0 +1,728 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <numeric>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+// ArmarX
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+namespace
+{
+
+ void
+ invalidate(float& v)
+ {
+ v = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector2f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector3f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ v.z() = std::numeric_limits<float>::infinity();
+ }
+
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
+
+ std::string
+ to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
+ {
+ switch (mode)
+ {
+ case armarx::ObstacleAwarePlatformUnit::control_mode::position:
+ return "position";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::velocity:
+ return "velocity";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::none:
+ default:
+ return "unset";
+ }
+ }
+
+}
+
+
+const std::string
+armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+
+
+armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
+
+
+armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ if (!hasRobot("robot"))
+ {
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+ }
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+ invalidate(m_viz.start);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAwarePlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveTo(
+ const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
+ m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
+ m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
+
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ schedule_high_level_control_loop(control_mode::velocity);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveRelative(
+ const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
+{
+ using namespace simox::math;
+
+ // Transform relative to global.
+ std::unique_lock lock{m_control_data.mutex};
+ synchronizeLocalClone(m_robot);
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
+ lock.unlock();
+
+ // Use moveTo.
+ moveTo(
+ agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
+ const float max_pos_vel,
+ const float max_rot_vel,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+ m_control_data.max_vel = max_pos_vel;
+ m_control_data.max_rot_vel = max_rot_vel;
+ m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
+{
+ schedule_high_level_control_loop(control_mode::none);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode mode)
+{
+ std::scoped_lock l{m_control_loop.mutex};
+
+ // If the control mode didn't change, there's nothing to do.
+ if (m_control_loop.mode == mode)
+ {
+ return;
+ }
+
+ // If a control loop is runnung, stop it and wait for termination.
+ if (m_control_loop.mode != mode and m_control_loop.task)
+ {
+ ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
+ const bool join = true;
+ m_control_loop.task->stop(join);
+ }
+
+ // If the new control mode is none, no new control loop needs to be created.
+ if (mode == control_mode::none)
+ {
+ ARMARX_VERBOSE << "Going into stand-by.";
+ return;
+ }
+
+ // Start next control loop.
+ ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
+ this,
+ &ObstacleAwarePlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
+{
+ const control_mode mode = m_control_loop.mode;
+ ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
+
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ const velocities vels = get_velocities();
+
+ // Debug.
+ StringVariantBaseMap m;
+ m["err_dist"] = new Variant{vels.err_dist};
+ m["err_angular_dist"] = new Variant{vels.err_angular_dist};
+
+ m["target_global_x"] = new Variant{vels.target_global.x()};
+ m["target_global_y"] = new Variant{vels.target_global.y()};
+ m["target_global_abs"] = new Variant{vels.target_global.norm()};
+
+ m["target_local_x"] = new Variant{vels.target_local.x()};
+ m["target_local_y"] = new Variant{vels.target_local.y()};
+ m["target_local_abs"] = new Variant(vels.target_local.norm());
+ m["target_rot"] = new Variant{vels.target_rot};
+
+ m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
+ m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
+ m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
+
+ m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
+ m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
+ m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
+
+ setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
+
+ m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
+ visualize(vels);
+ cu.waitForCycleDuration();
+ }
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Error occured while running control loop.\n"
+ << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unknown error occured while running control loop.";
+ }
+
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+ m_control_loop.mode = control_mode::none;
+
+ visualize();
+
+ ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
+}
+
+
+armarx::ObstacleAwarePlatformUnit::velocities
+armarx::ObstacleAwarePlatformUnit::get_velocities()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+ // Update agent and get target velocities.
+ update_agent_dependent_values();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
+ // Apply obstacle avoidance and apply post processing to get the modulated velocity.
+ const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
+ {
+ const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
+ ARMARX_DEBUG << "Circle approximation: " << circle.radius;
+ m_viz.boundingCircle = circle;
+ const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+
+ ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+ // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+ float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
+
+ const Eigen::Vector2f vel = modifier * target_vel;
+ return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
+ }();
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+
+ velocities vels;
+ vels.target_local = r * target_vel;
+ vels.target_global = target_vel;
+ vels.modulated_local = r * modulated_vel;
+ vels.modulated_global = modulated_vel;
+ vels.target_rot = target_rot_vel;
+ vels.err_dist = m_control_data.target_dist;
+ vels.err_angular_dist = m_control_data.target_angular_dist;
+
+ return vels;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAwarePlatformUnit::get_target_velocity()
+const
+{
+ using namespace simox::math;
+
+ Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
+
+ if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
+ {
+ uncapped_target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_vel = m_control_data.target_vel;
+ }
+
+ ARMARX_CHECK(uncapped_target_vel.allFinite());
+
+ return norm_max(uncapped_target_vel, m_control_data.max_vel);
+}
+
+
+float
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
+const
+{
+ using namespace simox::math;
+
+ float uncapped_target_rot_vel = 0;
+
+ if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
+ {
+ m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
+ uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_rot_vel = m_control_data.target_rot_vel;
+ }
+
+ ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
+
+ return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ uncapped_target_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
+{
+ using namespace simox::math;
+
+ synchronizeLocalClone(m_robot);
+ m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
+ m_control_data.agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // Update distances in position mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ m_control_data.target_dist =
+ (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist =
+ periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
+ -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
+
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+ }
+ // Otherwise invalidate them.
+ else
+ {
+ invalidate(m_control_data.target_dist);
+ invalidate(m_control_data.target_angular_dist);
+ }
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_position_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_orientation_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return target_position_reached() and target_orientation_reached();
+ }
+
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
+const noexcept
+{
+ if (m_control_data.target_dist < m_control_data.pos_near_threshold)
+ {
+ const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
+
+ const float sim = simox::math::cosine_similarity(target_direction, control_direction);
+
+ // if almost pointing into same direction
+ if (sim > cos(M_PI_4f32))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize()
+{
+ const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+ velocities vels;
+ vels.target_local = zero;
+ vels.target_global = zero;
+ vels.modulated_local = zero;
+ vels.modulated_global = zero;
+ vels.target_rot = 0;
+
+ visualize(vels);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
+{
+ using namespace armarx::viz;
+
+ if (not m_viz.enabled)
+ {
+ return;
+ }
+
+ Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ Layer l_prog = arviz.layer("progress");
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+
+ {
+ l_prog.add(Cylinder{"boundingCylinder"}
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
+ }
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = agent_pos;
+ }
+
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(agent_pos);
+ }
+
+ // Draw path.
+ if (not target_reached())
+ {
+ // Start.
+ l_prog.add(Sphere{"start"}
+ .position(m_viz.start)
+ .color(Color::cyan(255, 64))
+ .radius(40));
+
+ // Path.
+ for (unsigned i = 0; i < m_viz.path.size(); ++i)
+ {
+ l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
+ }
+
+ // Goal.
+ const Eigen::Vector3f target{m_control_data.target_pos.x(),
+ m_control_data.target_pos.y(),
+ 0};
+ l_prog.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+ }
+ else
+ {
+ m_viz.path.clear();
+ invalidate(m_viz.start);
+ }
+ }
+
+ // Velocities.
+ Layer l_vels = arviz.layer("velocities");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ const float min_velocity = 15;
+ const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+ const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
+ const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
+
+ if (original.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"original"}
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (modulated.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+ }
+
+ // Agent.
+ Layer l_agnt = arviz.layer("agent");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ l_agnt.add(Sphere{"agent"}
+ .position(agent_pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_control_data.agent_safety_margin > 0)
+ {
+ l_agnt.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
+ }
+ }
+
+ arviz.commit({l_prog, l_vels, l_agnt});
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+
+ def->component(m_platform, "Platform");
+ def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
+
+ // Settings.
+ def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ "the robot should at least set when near the target");
+ def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ "should at least set on general");
+ def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ "which the robot is considered to be near the target for min velocity, "
+ "smoothing, etc.");
+
+ // Control parameters.
+ def->optional(m_control_data.kp, "control.pos.kp");
+ def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
+ def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
+ def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..274b67ea153eec20473b67fa4f45b8425c7b174d
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -0,0 +1,259 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <tuple>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAwarePlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
+ {
+
+ public:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ private:
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAwarePlatformUnit>::pointer_type task;
+ IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
+ };
+
+ struct control_data
+ {
+ std::mutex mutex;
+ Eigen::Vector2f target_pos;
+ float target_ori;
+ Eigen::Vector2f target_vel;
+ float target_rot_vel;
+ float target_dist;
+ float target_angular_dist;
+ Eigen::Vector2f agent_pos;
+ float agent_ori;
+ double agent_safety_margin = 0;
+ float min_vel_near_target = 50;
+ float min_vel_general = 100;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_near_threshold = 250;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ VirtualRobot::Circle boundingCircle;
+ };
+
+ struct velocities
+ {
+ Eigen::Vector2f target_local;
+ Eigen::Vector2f modulated_local;
+ Eigen::Vector2f target_global;
+ Eigen::Vector2f modulated_global;
+ float target_rot;
+ float err_dist;
+ float err_angular_dist;
+ };
+
+ public:
+
+ ObstacleAwarePlatformUnit();
+
+ ~ObstacleAwarePlatformUnit()
+ override;
+
+ std::string
+ getDefaultName()
+ const override;
+
+ void
+ moveTo(
+ float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ moveRelative(
+ float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ void
+ onInitPlatformUnit()
+ override;
+
+ void
+ onStartPlatformUnit()
+ override;
+
+ void
+ onExitPlatformUnit()
+ override;
+
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ velocities
+ get_velocities();
+
+ void
+ update_agent_dependent_values();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
+
+ bool
+ target_position_reached()
+ const;
+
+ bool
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
+
+ void
+ visualize();
+
+ void
+ visualize(const velocities& vels);
+
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ VirtualRobot::RobotPtr m_robot;
+ DynamicObstacleManagerInterfacePrx m_obsman;
+
+ ObstacleAwarePlatformUnit::control_loop m_control_loop;
+ ObstacleAwarePlatformUnit::control_data m_control_data;
+
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f168208f6734ea228aa848dcf0c5d0976370bae7
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
@@ -0,0 +1,40 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAwarePlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index bc246422b98fbd89feac994a2ace52645532e22a..77f9c8d8b49f9f192eb63ab64fe9d5b308b9e22e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,6 +23,10 @@
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <cmath>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -34,7 +38,7 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
+ opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
@@ -78,24 +82,14 @@ namespace armarx
return;
}
- float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
- Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
-
- Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
- float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
- float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
- relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
-
- float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
- relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+ const float measuredOrientation = rtGetControlStruct().globalOrientation;
+ pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
- pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
- //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
+ const Eigen::Rotation2Df global_R_local(-measuredOrientation);
- Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
target->velocityX = velocities.x();
target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -112,7 +106,7 @@ namespace armarx
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
getWriterControlStruct().newTargetSet = true;
@@ -125,7 +119,7 @@ namespace armarx
// ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+ getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 1c00dd4e630208a79852ea3d18d96da8a501f7d0..bebbb3f414be2184f51fdb8518cdcd368da33ebc 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -104,7 +104,6 @@ module data
{
Vector3fSeq points;
- Color lineColor;
float lineWidth = 10.0f;
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index d21a3c55b7574a459f0be3e2577870cf7640f5e3..5ff2649fae12cb906f5821f73ba1fb1fd21647fe 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -30,6 +30,18 @@
module armarx
{
+ module dynamicobstaclemanager
+ {
+ struct LineSegment
+ {
+ Eigen::Vector2f lineStart;
+ Eigen::Vector2f lineEnd;
+ };
+
+ sequence<LineSegment> LineSegments;
+ }
+
+
interface DynamicObstacleManagerInterface
{
void
@@ -38,6 +50,9 @@ module armarx
void
add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void
+ add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+
void
remove_all_decayable_obstacles();
@@ -48,6 +63,8 @@ module armarx
remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
+
+ float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 5df6b028f0272c51a0538392375c27421d19dc06..02e5c638bfdfc145c271a995e016ce0c5f15ba01 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,7 +22,7 @@ add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
-add_subdirectory(armem_robot_mapping)
+add_subdirectory(armem_vision)
add_subdirectory(armem_skills)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 003f611949956c306ff73aac3c0602217c02a69c..ee5a16f2563ac08d997f1273035a16085ab1919c 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -85,6 +85,9 @@ set(LIB_FILES
client/Writer.cpp
client/WriterComponentPlugin.cpp
+ client/util/SimpleReaderBase.cpp
+ client/util/SimpleWriterBase.cpp
+
client/Query.cpp
client/query/Builder.cpp
client/query/selectors.cpp
@@ -196,6 +199,9 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
+ client/util/SimpleReaderBase.h
+ client/util/SimpleWriterBase.h
+
server.h
server/ComponentPlugin.h
server/MemoryToIceAdapter.h
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c3afb9a2adcc3f08495bc8c979d33ebc11742f2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -0,0 +1,55 @@
+#include "SimpleReaderBase.h"
+
+#include "RobotAPI/libraries/armem/client/ComponentPlugin.h"
+
+namespace armarx::armem::client::util
+{
+ SimpleReaderBase::SimpleReaderBase(ComponentPluginUser& memoryClient) :
+ component(memoryClient)
+ {
+ }
+
+ void SimpleReaderBase::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+ }
+
+ void SimpleReaderBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Writer: Waiting for memory '" << props.memoryName
+ << "' ...";
+ auto result = component.useMemory(props.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '"
+ << props.memoryName;
+
+ memoryReaderClient.setReadingMemory(result.proxy);
+ }
+
+ std::mutex& SimpleReaderBase::memoryReaderMutex()
+ {
+ return memoryMutex;
+ }
+
+ const armem::client::Reader& SimpleReaderBase::memoryReader() const
+ {
+ return memoryReaderClient;
+ }
+
+ const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+ {
+ return props;
+ }
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e2c8edf24d8b99362e81225b439f1ea7ed56522
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -0,0 +1,73 @@
+/*
+ * 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 <mutex>
+
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+
+namespace armarx::armem::client
+{
+ class ComponentPluginUser;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleReaderBase
+ {
+ public:
+ SimpleReaderBase(ComponentPluginUser& memoryClient);
+ virtual ~SimpleReaderBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ protected:
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ std::mutex& memoryReaderMutex();
+ const armem::client::Reader& memoryReader() const;
+
+ private:
+ Properties props;
+
+ armem::client::Reader memoryReaderClient;
+ std::mutex memoryMutex;
+
+ ComponentPluginUser& component;
+ };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7ac78e00496cf890b97f11523f200c5fc9c3ab3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -0,0 +1,61 @@
+#include "SimpleWriterBase.h"
+
+#include "RobotAPI/libraries/armem/client/ComponentPlugin.h"
+
+namespace armarx::armem::client::util
+{
+ SimpleWriterBase::SimpleWriterBase(ComponentPluginUser& component) :
+ component(component)
+ {
+ }
+
+ void
+ SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ props = defaultProperties();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+
+ def->required(props.providerName,
+ prefix + "Provider",
+ "Name of this provider");
+ }
+
+ void SimpleWriterBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
+ << props.memoryName << "' ...";
+ auto result = component.useMemory(props.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '"
+ << props.memoryName;
+
+ memoryWriterClient.setWritingMemory(result.proxy);
+ // memoryReader.setReadingMemory(result.proxy);
+ }
+
+ std::mutex& SimpleWriterBase::memoryWriterMutex()
+ {
+ return memoryMutex;
+ }
+
+ armem::client::Writer& SimpleWriterBase::memoryWriter()
+ {
+ return memoryWriterClient;
+ }
+ const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+ {
+ return props;
+ }
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..55829b2c7bbc3dc5e9758156ad7c4f7e14956db1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -0,0 +1,77 @@
+/*
+ * 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 <mutex>
+
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+
+
+namespace armarx::armem::client
+{
+ class ComponentPluginUser;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleWriterBase
+ {
+ public:
+ SimpleWriterBase(ComponentPluginUser& component);
+ virtual ~SimpleWriterBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ protected:
+ std::mutex& memoryWriterMutex();
+ armem::client::Writer& memoryWriter();
+
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ std::string providerName = ""; // required property
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ private:
+ Properties props;
+
+ armem::client::Writer memoryWriterClient;
+ std::mutex memoryMutex;
+
+ // armem::client::Reader memoryReader;
+ // std::mutex memoryReaderMutex;
+
+ ComponentPluginUser& component;
+ };
+
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
deleted file mode 100644
index 145ced819b68db659a92e232747c645c24037ce5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME armem_robot_mapping)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- # This package
- RobotAPICore
- RobotAPI::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./aron_conversions.h
- ./MappingDataWriter.h
- ./MappingDataReader.h
- SOURCES
- ./aron_conversions.cpp
- ./MappingDataWriter.cpp
- ./MappingDataReader.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/LaserScan.xml
-)
-
-
-add_library(RobotAPI::armem_robot_mapping ALIAS armem_robot_mapping)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
deleted file mode 100644
index 7ecffc237d480fd33c30226ef7f1cf02c09308de..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ /dev/null
@@ -1,70 +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 <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-namespace armarx
-{
-
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace aron
-
- // 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::datanavigator::NDArrayNavigatorPtr& navigator)
- {
- return aron::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::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
- {
- return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
- }
-
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 516769871608464d59aeb5318119dc5d5a1ab4f5..433694ec819859bdcda209dd57aad21d22512116 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -66,7 +66,7 @@ namespace armarx::armem::client::robot_state::localization
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index f69ab33f282c82599af7c66b43f7919fcba8114f..299c79b69222accb8290a7064229a9dfe59e8572 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -70,7 +70,7 @@ namespace armarx::armem::client::robot_state::localization
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1767c9cebd878b1d15cad6f8043a2bd7cf932814
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_vision)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ aroncommon
+ # System / External
+ Eigen3::Eigen
+ HEADERS
+ ./aron_conversions.h
+ ./client/laser_scans/Reader.h
+ ./client/laser_scans/Writer.h
+ ./client/occupancy_grid/Reader.h
+ ./client/occupancy_grid/Writer.h
+ SOURCES
+ ./aron_conversions.cpp
+ ./client/laser_scans/Reader.cpp
+ ./client/laser_scans/Writer.cpp
+ ./client/occupancy_grid/Reader.cpp
+ ./client/occupancy_grid/Writer.cpp
+ ./OccupancyGridHelper.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/LaserScan.xml
+ aron/OccupancyGrid.xml
+)
+
+add_library(
+ RobotAPI::armem_vision
+ ALIAS
+ armem_vision
+)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f88cc75454fd6f83545c3207c1e58642bedcc225
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
@@ -0,0 +1,39 @@
+#include "OccupancyGridHelper.h"
+
+#include "types.h"
+
+namespace armarx
+{
+ OccupancyGridHelper::OccupancyGridHelper(const OccupancyGrid& occupancyGrid,
+ const Params& params) :
+ occupancyGrid(occupancyGrid), params(params)
+ {
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::knownCells() const
+ {
+ return (occupancyGrid.grid > 0.F).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::freespace() const
+ {
+ // matrix1 = matrix1 .unaryExpr(std::ptr_fun(ReplaceNanWithValue<1>));
+ // return (occupancyGrid.grid ).cast<bool>();
+
+ const auto isFree = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p < params.freespaceThreshold and p > 0.F); };
+
+ // TODO(fabian.reister): which one to choose?
+ // return occupancyGrid.grid.unaryExpr(isFree).cast<bool>();
+ return occupancyGrid.grid.unaryViewExpr(isFree).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::obstacles() const
+ {
+ const auto isOccupied = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p > params.occupiedThreshold); };
+
+ return occupancyGrid.grid.unaryViewExpr(isOccupied).cast<bool>();
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::armem
+{
+ struct OccupancyGrid;
+}
+
+namespace armarx
+{
+ using armarx::armem::OccupancyGrid;
+
+ namespace detail
+ {
+ struct OccupancyGridHelperParams
+ {
+ float freespaceThreshold = 0.45F;
+ float occupiedThreshold = 0.55F;
+ };
+ }
+
+ class OccupancyGridHelper
+ {
+ public:
+ using Params = detail::OccupancyGridHelperParams;
+
+ OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+
+ using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
+
+ BinaryArray knownCells() const;
+ BinaryArray freespace() const;
+ BinaryArray obstacles() const;
+
+ private:
+ const OccupancyGrid& occupancyGrid;
+ const Params params;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
similarity index 83%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index 659df4a1536b0f81b81ba64f77e4049ba42fdd5c..f2d11c2e4111dfd0c24896ba807aef6435b7014b 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::arondto::SensorHeader">
+ <Object name="armarx::armem::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::arondto::SensorHeader />
+ <armarx::armem::arondto::SensorHeader />
</ObjectChild>
<!--
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0c508a4e2138b4b04c126287bf46d8826fb3da6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -0,0 +1,30 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::arondto::OccupancyGrid'>
+ <ObjectChild key='resolution'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+
+ <!--
+ <ObjectChild key='grid'>
+ <NdArray />
+ </ObjectChild> -->
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
similarity index 62%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 9b45357198639190d12b9a896705885f8046ea85..34d87dcf8473475b3a6b5e151f842b5d6004a62c 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,23 +5,22 @@
#include <iterator>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include "types.h"
-
-namespace armarx
+namespace armarx::armem
{
/************ fromAron ************/
SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
{
-
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
- .timestamp = aronSensorHeader.timestamp};
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
+ .timestamp = timeFromAron(aronSensorHeader.timestamp)};
}
void fromAron(const arondto::LaserScanStamped& aronLaserScan,
@@ -31,8 +30,10 @@ namespace armarx
// laserScan.data = fromAron(aronLaserScan.data);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
- std::int64_t& timestamp, std::string& frame,
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
std::string& agentName)
{
const auto header = fromAron(aronLaserScan.header);
@@ -40,14 +41,12 @@ namespace armarx
// laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp.toMicroSeconds();
- frame = header.frame;
+ frame = header.frame;
agentName = header.agent;
}
-
/************ toAron ************/
-
// auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
// {
// aronLaserScan.scan = toAron(laserScan);
@@ -62,9 +61,9 @@ namespace armarx
{
arondto::SensorHeader aronSensorHeader;
- aronSensorHeader.agent = sensorHeader.agent;
- aronSensorHeader.frame = sensorHeader.frame;
- aronSensorHeader.timestamp = sensorHeader.timestamp;
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.timestamp = toAron(sensorHeader.timestamp);
return aronSensorHeader;
}
@@ -86,9 +85,26 @@ namespace armarx
{
.agent = agentName, .frame = frame, .timestamp = timestamp};
- const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+ const LaserScanStamped laserScanStamped{.header = header,
+ .data = laserScan};
toAron(laserScanStamped, aronLaserScanStamped);
}
-} // namespace armarx
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.pose, bo.pose);
+ aron::toAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.pose, bo.pose);
+ aron::fromAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..db97ad5076457805d953100201821f912a046291
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -0,0 +1,82 @@
+/*
+ * 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 "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+namespace armarx::armem
+{
+
+ 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::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::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::datanavigator::NDArrayNavigatorPtr
+ toAron(const LaserScan& laserScan)
+ {
+ using aron::converter::AronVectorConverter;
+ return AronVectorConverter::ConvertFromVector(laserScan);
+ }
+
+ // OccupancyGrid
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr
+ toAron(const OccupancyGrid::Grid& grid)
+ {
+ return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+ }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
similarity index 70%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index 3c3e0e8c3303728b233eb76bf62a0a505b30f0e1..9c46b0c4729dfe6ca038c1f31ef81255f56d4968 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -1,31 +1,32 @@
-#include "MappingDataReader.h"
+#include "Reader.h"
// STD / STL
-#include <cstring>
-#include <vector>
#include <algorithm>
+#include <cstring>
#include <map>
#include <optional>
#include <ostream>
-#include <type_traits>
#include <utility>
+#include <vector>
+
+#include <type_traits>
// ICE
-#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
// ArmarXCore
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#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/aron/core/Exception.h>
@@ -40,41 +41,40 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataReader::MappingDataReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
{
}
- MappingDataReader::~MappingDataReader() = default;
+ Reader::~Reader() = default;
+
- void MappingDataReader::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataReader::connect()
+ void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
@@ -91,19 +91,19 @@ namespace armarx::armem
}
}
- armem::client::query::Builder
- MappingDataReader::buildQuery(const Query& query) const
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
{
- armem::client::query::Builder qb;
+ armarx::armem::client::query::Builder qb;
ARMARX_INFO << "Query for agent: " << query.agent
- << " memory name: " << properties.mappingMemoryName;
+ << " memory name: " << properties.memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().all()
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -113,7 +113,7 @@ namespace armarx::armem
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -121,10 +121,10 @@ namespace armarx::armem
}
return qb;
-
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
+ std::vector<LaserScanStamped>
+ asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -133,25 +133,27 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert =
+ [](const arondto::LaserScanStamped & aronLaserScanStamped,
+ const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
- const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+ const auto ndArrayNavigator =
+ aron::datanavigator::NDArrayNavigator::DynamicCast(
+ ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
ARMARX_IMPORTANT << "4";
-
return laserScanStamped;
-
};
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
if (entity.empty())
{
@@ -160,11 +162,12 @@ namespace armarx::armem
ARMARX_DEBUG << "History size: " << entity.size();
- for (const auto &[ss, entitySnapshot] : entity)
+ for (const auto& [ss, entitySnapshot] : entity)
{
for (const auto& entityInstance : entitySnapshot.instances())
{
- const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+ const auto o =
+ tryCast<arondto::LaserScanStamped>(entityInstance);
if (o)
{
@@ -177,8 +180,7 @@ namespace armarx::armem
return outV;
}
- MappingDataReader::Result
- MappingDataReader::queryData(const Query& query) const
+ Reader::Result Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
@@ -193,25 +195,25 @@ namespace armarx::armem
{
ARMARX_WARNING << "Failed to query data from memory: "
<< qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
// now create result from memory
const auto& entities =
- qResult.memory.getCoreSegment(properties.mappingMemoryName)
+ qResult.memory.getCoreSegment(properties.memoryName)
.getProviderSegment(query.agent)
.entities();
const auto laserScans = asLaserScans(entities);
- const auto sensors = simox::alg::get_keys(entities);
+ const auto sensors = simox::alg::get_keys(entities);
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index 2479b9413aebe634257eadb2cf00f0765f4ba20c..138717c42257e02275a15785940783330126ee4d 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -21,7 +21,7 @@
#pragma once
-#include <stdint.h>
+#include <cstdint>
#include <string>
#include <vector>
@@ -31,8 +31,7 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx
@@ -40,7 +39,7 @@ namespace armarx
class ManagedIceObject;
}
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
struct TimeRange
@@ -60,16 +59,15 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader
+ class Reader
{
public:
- MappingDataReader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~MappingDataReader();
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
void connect();
-
struct Query
{
std::string agent;
@@ -78,7 +76,6 @@ namespace armarx::armem
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
-
};
struct Result
@@ -95,16 +92,14 @@ namespace armarx::armem
} status;
std::string errorMessage;
-
};
Result queryData(const Query& query) const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
- client::query::Builder buildQuery(const Query& query) const ;
-
+ armarx::armem::client::query::Builder
+ buildQuery(const Query& query) const;
private:
@@ -114,12 +109,12 @@ namespace armarx::armem
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
};
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
similarity index 61%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
index fcf303e7bdfeb2bc35ce415da34e315671bb5ec9..7358e504ffecf3a2a710bd77b1631437e3818a42 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -1,36 +1,37 @@
-#include "MappingDataWriter.h"
+#include "Writer.h"
-#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataWriter::MappingDataWriter(armem::client::MemoryNameSystem& memoryNameSystem)
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem)
: memoryNameSystem(memoryNameSystem) {}
+ Writer::~Writer() = default;
- MappingDataWriter::~MappingDataWriter() = default;
- void MappingDataWriter::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
- ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataWriter::connect()
+ void Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
+ << properties.memoryName << "' ...";
try
{
memoryWriter = memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
@@ -41,18 +42,20 @@ namespace armarx::armem
ARMARX_ERROR << e.what();
return;
}
- }
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
+ << properties.memoryName;
+ }
- bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
- const std::string& frame,
- const std::string& agentName,
- const std::int64_t& timestamp)
+ bool Writer::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
{
std::lock_guard g{memoryWriterMutex};
const auto result =
- memoryWriter.addSegment(properties.mappingMemoryName, agentName);
+ memoryWriter.addSegment(properties.memoryName, agentName);
if (not result.success)
{
@@ -79,7 +82,7 @@ namespace armarx::armem
dict->addElement("scan", toAron(laserScan));
update.instancesData = {dict};
- update.timeCreated = iceTimestamp;
+ update.timeCreated = iceTimestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -94,4 +97,4 @@ namespace armarx::armem
return updateResult.success;
}
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
similarity index 72%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index ab4187ada2cf1476f1841c869112e5cce1b3d78c..afc383634b87a31fc84a841a5658f83ec845ee27 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -27,12 +27,11 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
/**
@@ -46,12 +45,13 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter
+ class Writer
{
public:
- MappingDataWriter(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~MappingDataWriter();
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
void connect();
@@ -60,9 +60,13 @@ namespace armarx::armem
// void connect() override;
/// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+ void registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def) /*override*/;
- bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+ bool storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
private:
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -71,15 +75,14 @@ namespace armarx::armem
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
std::mutex memoryWriterMutex;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
};
-
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba7edf547d1d45873609d723b92934b18d7f5c88
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <utility>
+#include <vector>
+
+#include <type_traits>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#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/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/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/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().memoryName)
+ .providerSegments().withName(query.providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+ OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+ {
+ ARMARX_CHECK(not entities.empty()) << "No entities";
+ ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+
+ const wm::Entity& entity = entities.begin()->second;
+ ARMARX_CHECK(not entity.empty()) << "No snapshots";
+
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ const auto& entityInstance = entitySnapshot.instances().front();
+
+ const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+ ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+
+ OccupancyGrid occupancyGrid;
+ fromAron(*aronDto, occupancyGrid);
+
+ // direct access to grid data
+ const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+ entityInstance.data()->getElement("grid"));
+ ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
+
+ return occupancyGrid;
+ }
+
+ Reader::Result Reader::query(const Query& query) const
+ {
+ const auto qb = buildQuery(query);
+
+ ARMARX_IMPORTANT << "[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 {.occupancyGrid = std::nullopt,
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
+ .getProviderSegment(query.providerName)
+ .entities();
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities.";
+ return {.occupancyGrid = std::nullopt,
+ .status = Result::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ const auto occupancyGrid = asOccupancyGrid(entities);
+ return Result{.occupancyGrid = occupancyGrid,
+ .status = Result::Status::Success};
+ }
+ catch (...)
+ {
+ return Result{.status = Result::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..a43c2c7c1151223358d5b1ab608f824fd3cdf66f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -0,0 +1,74 @@
+/*
+ * 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 <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ };
+
+ struct Result
+ {
+ std::optional<OccupancyGrid> occupancyGrid = std::nullopt;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ Result query(const Query& query) const;
+
+ ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2930f14f479d9ae68ee222f2643edf327392a43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -0,0 +1,70 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+ Writer::~Writer() = default;
+
+ bool Writer::store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::OccupancyGrid aronGrid;
+ // currently only sets the header
+ toAron(aronGrid, grid);
+
+ auto dict = aronGrid.toAron();
+ dict->addElement("grid", toAron(grid.grid));
+
+ update.instancesData = {dict};
+ update.timeCreated = iceTimestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string Writer::propertyPrefix() const
+ {
+ return "mem.vision.occupancy_grid.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+
+ return SimpleWriterBase::Properties{.memoryName = "Vision",
+ .coreSegmentName = "OccupancyGrid"};
+ }
+} // namespace armarx::armem::vision::occupancy_grid::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf1268444c333d10bb3c2f9efb68da11fe697cc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -0,0 +1,62 @@
+/*
+ * 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 "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+namespace armarx::armem::vision::occupancy_grid::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 : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_vision/types.h
similarity index 73%
rename from source/RobotAPI/libraries/armem_robot_mapping/types.h
rename to source/RobotAPI/libraries/armem_vision/types.h
index d822597e103177b0833014d25f0530e25f2b2075..dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -21,10 +21,12 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <vector>
+
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx
+namespace armarx::armem
{
struct SensorHeader
@@ -40,4 +42,20 @@ namespace armarx
LaserScan data;
};
-} // namespace armarx
+ // template<typename _ValueT = float>
+ struct OccupancyGrid
+ {
+ float resolution;
+
+ std::string frame;
+ Eigen::Affine3f pose;
+
+ // using ValueType = _ValueT;
+ using CellType = float;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+
+ Grid grid;
+ };
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 787ae30a23f097821989fcd9658a3fa31d6b4761..4a5a02e56edc8fae399b2a216b4ee77a6f4536d8 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -28,6 +28,7 @@
// Eigen
#include <Eigen/Geometry>
#include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -103,9 +104,26 @@ namespace armarx::aron::converter
return ConvertToMatrix<T, Rows, Cols>(*nav);
}
- template<typename T, int Rows, int Cols>
+ template<typename T>
+ static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+ MatrixT ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
{
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicMatrix<T>(nav);
+ }
+
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
auto dims = nav.getDimensions();
@@ -114,6 +132,61 @@ namespace armarx::aron::converter
return ret;
}
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+ // Eigen::Array
+
+ template<typename T>
+ static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+ ArrayT ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+ static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicArray<T>(nav);
+ }
+
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ auto dims = nav.getDimensions();
+
+ Eigen::Array<T, Rows, Cols> ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ return ret;
+ }
+
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+
private:
/**
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 91f15876447e836e6a7e5f883747ae613765a0e5..65db64e872112548b72048542cfdecb7835cd81f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -90,7 +90,7 @@ namespace armarx
return _root;
}
- bool RemoteRobot::hasRobotNode(const std::string& robotNodeName)
+ bool RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
{
if (_cachedNodes.find(name) == _cachedNodes.end())
{
@@ -103,7 +103,7 @@ namespace armarx
}
- bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode)
+ bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
{
return this->hasRobotNode(robotNode->getName());
@@ -159,7 +159,7 @@ namespace armarx
}
}
- bool RemoteRobot::hasRobotNodeSet(const std::string& name)
+ bool RemoteRobot::hasRobotNodeSet(const std::string& name) const
{
return _robot->hasRobotNodeSet(name);
}
@@ -539,17 +539,17 @@ namespace armarx
// Private (unused methods)
- bool RemoteRobot::hasEndEffector(const std::string& endEffectorName)
+ bool RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
{
return false;
}
- EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName)
+ EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName) const
{
return EndEffectorPtr();
}
- void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) {}
+ void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const {}
void RemoteRobot::setRootNode(RobotNodePtr node) {}
void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index b687f0e0fc8cc1eac7b967b10408ec8b63440f56..734db7407354682df6fa45c866acdb0e9c8c0a6f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -145,13 +145,13 @@ namespace armarx
VirtualRobot::RobotNodePtr getRootNode() const override;
- bool hasRobotNode(const std::string& robotNodeName) override;
- bool hasRobotNode(VirtualRobot::RobotNodePtr) override;
+ bool hasRobotNode(const std::string& robotNodeName) const override;
+ bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
- bool hasRobotNodeSet(const std::string& name) override;
+ bool hasRobotNodeSet(const std::string& name) const override;
VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
@@ -223,11 +223,11 @@ namespace armarx
protected:
/// Not implemented yet
- bool hasEndEffector(const std::string& endEffectorName) override;
+ bool hasEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) override;
+ VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) override;
+ void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
/// Not implemented yet
void setRootNode(VirtualRobot::RobotNodePtr node) override;