diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 8af364a4a4213d9210698826a3540b553343260d..9c60131a806481066122ca5a6a9fbf99d57f6681 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -53,3 +53,4 @@ add_subdirectory(StatechartExecutorExample)
add_subdirectory(NaturalIKTest)
+add_subdirectory(DynamicObstacleManager)
diff --git a/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e8cd2eda8e4ab5fab7909b9b1a2febf0ccbe4bf4
--- /dev/null
+++ b/source/RobotAPI/applications/DynamicObstacleManager/CMakeLists.txt
@@ -0,0 +1,7 @@
+armarx_component_set_name("DynamicObstacleManager")
+
+set(COMPONENT_LIBS
+ DynamicObstacleManager
+)
+
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/applications/DynamicObstacleManager/main.cpp b/source/RobotAPI/applications/DynamicObstacleManager/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b115f14589702625322d2b461a5adf38aa98643
--- /dev/null
+++ b/source/RobotAPI/applications/DynamicObstacleManager/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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian Peller-Konrad <fabian.peller-konrad@kit.edu>
+ * @date 2020
+ * @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>
+
+// Component
+#include <RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h>
+
+using namespace armarx;
+
+int main(int argc, char* argv[])
+{
+ const std::string name = DynamicObstacleManager::default_name;
+ return runSimpleComponentApp<DynamicObstacleManager>(argc, argv, name);
+}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index e3adea9f26ea18835bac052d9badc959d2bbd8d3..8841868b70021ca022dc06c8d5940a9ffd10e123 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -6,6 +6,7 @@ add_subdirectory(DebugDrawer)
add_subdirectory(DebugDrawerToArViz)
add_subdirectory(DSObstacleAvoidance)
add_subdirectory(DummyTextToSpeech)
+add_subdirectory(DynamicObstacleManager)
add_subdirectory(EarlyVisionGraph)
add_subdirectory(FrameTracking)
add_subdirectory(GamepadControlUnit)
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
index ff53d0508dd334b6095daa28a7e7b00438fa3ff8..4eb4b16a3d8df576ee0a2a870aaced1f69a78816 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
+++ b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
@@ -9,6 +9,12 @@ if(Eigen3_FOUND)
include_directories(${Eigen3_INCLUDE_DIR})
endif()
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+if(Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIR})
+endif()
+
find_package(PythonLibs 3.6 QUIET)
armarx_build_if(PYTHONLIBS_FOUND "Python libs not available")
if(PYTHONLIBS_FOUND)
diff --git a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d32b8830630d33ae0a1666477e77e62297506c0a
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
@@ -0,0 +1,53 @@
+armarx_component_set_name("DynamicObstacleManager")
+
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+if(Eigen3_FOUND)
+ include_directories(${Eigen3_INCLUDE_DIR})
+endif()
+
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+armarx_build_if(Simox_FOUND "Simox ${ArmarX_Simox_VERSION} or later not available")
+if (Simox_FOUND)
+ include_directories(${Simox_INCLUDE_DIRS})
+endif()
+
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+if(OpenCV_FOUND)
+ include_directories(${OpenCV_INCLUDE_DIRS})
+endif()
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotAPIInterfaces
+ ${OpenCV_LIBS}
+ ${Simox_LIBS}
+)
+
+set(SOURCES
+ ./DynamicObstacleManager.cpp
+ ./ManagedObstacle.cpp
+)
+set(HEADERS
+ ./DynamicObstacleManager.h
+ ./ManagedObstacle.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(DynamicObstacleManager PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e39c54aecc85964c9c5f5b4ec0fe5d6ea37a20c3
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -0,0 +1,430 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DynamicObstacleManager.h"
+
+// STD/STL
+#include <string>
+#include <vector>
+#include <cmath>
+#include <limits>
+
+// Ice
+#include <Ice/Current.h>
+using namespace Ice;
+
+// Eigen
+#include <Eigen/Core>
+using namespace Eigen;
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+// ArmarX
+#include <ArmarXCore/util/time.h>
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+using namespace armarx;
+
+
+const std::string
+armarx::DynamicObstacleManager::default_name = "DynamicObstacleManager";
+
+namespace armarx
+{
+
+ DynamicObstacleManager::DynamicObstacleManager() noexcept :
+ m_obstacle_index(0),
+ m_decay_after_ms(5000),
+ m_periodic_task_interval(500),
+ m_min_value_for_accepting(1000),
+ m_min_coverage_of_obstacles(0.7f),
+ m_min_coverage_of_line_samples_in_obstacle(0.9f),
+ m_min_size_of_obstacles(100),
+ m_min_length_of_lines(50),
+ m_max_size_of_obstacles(600),
+ m_max_length_of_lines(10000),
+ m_thickness_of_lines(200),
+ m_security_margin_for_obstacles(500),
+ m_security_margin_for_lines(400),
+ m_remove_enabled(false),
+ m_only_visualize(false),
+ m_allow_spwan_inside(false)
+ {
+
+ }
+
+
+ std::string DynamicObstacleManager::getDefaultName() const
+ {
+ return DynamicObstacleManager::default_name;
+ }
+
+
+ void DynamicObstacleManager::onInitComponent()
+ {
+
+ }
+
+
+ void DynamicObstacleManager::onConnectComponent()
+ {
+ m_decay_factor = m_periodic_task_interval;
+ m_access_bonus = m_periodic_task_interval;
+
+ m_update_obstacles = new PeriodicTask<DynamicObstacleManager>(this,
+ &DynamicObstacleManager::update_decayable_obstacles, m_periodic_task_interval);
+ m_update_obstacles->start();
+ }
+
+ void DynamicObstacleManager::onDisconnectComponent()
+ {
+ m_update_obstacles->stop();
+ }
+
+ void DynamicObstacleManager::onExitComponent()
+ {
+
+ }
+
+ void DynamicObstacleManager::add_decayable_obstacle(const Eigen::Vector2f& e_origin, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
+ {
+ // TODO
+ }
+
+
+ void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
+ {
+ 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;
+ const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
+ const float dot_product = difference_vec_normed(0);
+ const float cross_product = difference_vec_normed(1);
+ float yaw = acos(dot_product);
+
+ if (cross_product < 0)
+ {
+ yaw = 2 * M_PI - yaw;
+ }
+
+ if (length < m_min_length_of_lines)
+ {
+ return;
+ }
+
+
+ {
+ std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+
+ // First check own obstacles
+ for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ {
+ std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
+ float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
+ {managed_obstacle->m_obstacle.posX, managed_obstacle->m_obstacle.posY}, managed_obstacle->m_obstacle.axisLengthX, managed_obstacle->m_obstacle.axisLengthY, managed_obstacle->m_obstacle.yaw,
+ line_start, line_end);
+ //ARMARX_DEBUG << "The coverage of the new line to obstacle " << managed_obstacle.obstacle.name << "(" << ManagedObstacle::calculateObstacleArea(managed_obstacle.obstacle) << ") is " << coverage;
+ if (coverage >= m_min_coverage_of_line_samples_in_obstacle)
+ {
+ // Obstacle already in list. Increase counter and update current obstacle.
+ managed_obstacle->line_matches.push_back(line_start);
+ managed_obstacle->line_matches.push_back(line_end);
+ managed_obstacle->m_last_matched = IceUtil::Time::now();
+ return;
+ }
+ }
+ }
+
+ // Second check the obstacles from the obstacle avoidance
+ 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,
+ line_start, line_end);
+ if (coverage >= m_min_coverage_of_line_samples_in_obstacle)
+ {
+ // If own managed obstacle we already updatet its value. If its a static obstacle from the obstacle avoidance, then we can't update the position anyway...
+ 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());
+ new_managed_obstacle->m_obstacle.name = "managed_obstacle_" + std::to_string(m_obstacle_index++);
+ new_managed_obstacle->m_obstacle.posX = center_vec(0);
+ new_managed_obstacle->m_obstacle.posY = center_vec(1);
+ new_managed_obstacle->m_obstacle.refPosX = center_vec(0);
+ new_managed_obstacle->m_obstacle.refPosY = center_vec(1);
+ new_managed_obstacle->m_obstacle.yaw = yaw;
+ new_managed_obstacle->m_obstacle.axisLengthX = std::clamp(length * 0.5f, static_cast<float>(m_min_length_of_lines), static_cast<float>(m_max_length_of_lines));
+ new_managed_obstacle->m_obstacle.axisLengthY = m_thickness_of_lines;
+ new_managed_obstacle->m_obstacle.safetyMarginX = m_security_margin_for_lines;
+ new_managed_obstacle->m_obstacle.safetyMarginY = m_security_margin_for_lines;
+ new_managed_obstacle->m_last_matched = IceUtil::Time::now();
+ new_managed_obstacle->m_published = false;
+ new_managed_obstacle->m_value = m_min_value_for_accepting * 0.5;
+ new_managed_obstacle->position_buffer_index = 0;
+ new_managed_obstacle->position_buffer_fillctr = 0;
+ new_managed_obstacle->line_matches.push_back(line_start);
+ new_managed_obstacle->line_matches.push_back(line_end);
+
+ {
+ std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
+ m_managed_obstacles.push_back(new_managed_obstacle);
+ }
+ }
+
+
+ void DynamicObstacleManager::remove_all_decayable_obstacles(const Ice::Current&)
+ {
+ std::lock_guard l(m_managed_obstacles_mutex);
+
+ ARMARX_DEBUG << "Remove all obstacles from obstacle map by setting their value to -inf";
+ for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ {
+ managed_obstacle->m_value = std::numeric_limits<double>::min();
+ managed_obstacle->m_updated = true;
+ }
+ }
+
+ void DynamicObstacleManager::remove_obstacle(const std::string& name, const Ice::Current&)
+ {
+ 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)
+ {
+ if(managed_obstacle->m_obstacle.name == name)
+ {
+ managed_obstacle->m_value = std::numeric_limits<double>::min();
+ managed_obstacle->m_updated = true;
+ managed_obstacle->m_published = false;
+ m_obstacle_detection->removeObstacle(name);
+ m_obstacle_detection->updateEnvironment();
+ return; // We assume unique names
+ }
+ }
+ }
+
+ 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;
+ new_unmanaged_obstacle.name = name;
+ new_unmanaged_obstacle.posX = obs_pos(0);
+ new_unmanaged_obstacle.posY = obs_pos(1);
+ new_unmanaged_obstacle.refPosX = obs_pos(0);
+ new_unmanaged_obstacle.refPosY = obs_pos(1);
+ new_unmanaged_obstacle.yaw = e_yaw;
+ new_unmanaged_obstacle.axisLengthX = std::clamp(e_rx, static_cast<float>(m_min_size_of_obstacles), static_cast<float>(m_max_size_of_obstacles));
+ new_unmanaged_obstacle.axisLengthY = std::clamp(e_ry, static_cast<float>(m_min_size_of_obstacles), static_cast<float>(m_max_size_of_obstacles));
+ new_unmanaged_obstacle.safetyMarginX = m_security_margin_for_obstacles;
+ new_unmanaged_obstacle.safetyMarginY = m_security_margin_for_obstacles;
+ m_obstacle_detection->updateObstacle(new_unmanaged_obstacle);
+ m_obstacle_detection->updateEnvironment();
+ }
+
+
+ void DynamicObstacleManager::update_decayable_obstacles()
+ {
+ IceUtil::Time started = IceUtil::Time::now();
+ std::vector<std::string> remove_obstacles;
+
+ // Some debug definitions
+ viz::Layer obstacleLayer = arviz.layer(m_obstacle_manager_layer_name);
+
+ // Update positions
+ {
+ std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ if (!m_managed_obstacles.size())
+ {
+ return;
+ }
+
+ for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ {
+ std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
+ managed_obstacle->update_position(m_thickness_of_lines);
+ }
+ }
+
+
+ // Update obstacle avoidance
+ int checked_obstacles = 0;
+ bool updated = false;
+ int published_obstacles = 0;
+ int updated_obstacles = 0;
+ {
+ std::lock_guard l(m_managed_obstacles_mutex);
+ std::sort(m_managed_obstacles.begin(), m_managed_obstacles.end(), ManagedObstacle::ComparatorDESCPrt);
+
+ checked_obstacles = m_managed_obstacles.size();
+
+ for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ {
+ std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
+ if (!managed_obstacle->m_updated)
+ {
+ managed_obstacle->m_value = std::max(-1.0f, managed_obstacle->m_value - m_decay_factor);
+ }
+ else
+ {
+ managed_obstacle->m_value = std::min(1.0f * m_decay_after_ms, managed_obstacle->m_value + m_access_bonus);
+ }
+
+ if (managed_obstacle->m_published)
+ {
+ if (managed_obstacle->m_value < m_min_value_for_accepting)
+ {
+ updated_obstacles++;
+ managed_obstacle->m_published = false;
+ if (!m_only_visualize)
+ {
+ m_obstacle_detection->removeObstacle(managed_obstacle->m_obstacle.name);
+ updated = true;
+ }
+ }
+ else
+ {
+ published_obstacles++;
+ if (managed_obstacle->m_updated)
+ {
+ updated_obstacles++;
+ if (!m_only_visualize)
+ {
+ m_obstacle_detection->updateObstacle(managed_obstacle->m_obstacle);
+ updated = true;
+ }
+ //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
+ }
+ else
+ {
+ //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 0, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
+ }
+ }
+ }
+ else // if (!managed_obstacle.published)
+ {
+ if (managed_obstacle->m_value >= m_min_value_for_accepting)
+ {
+ published_obstacles++;
+ updated_obstacles++;
+ managed_obstacle->m_published = true;
+ if (!m_only_visualize)
+ {
+ m_obstacle_detection->updateObstacle(managed_obstacle->m_obstacle);
+ updated = true;
+ }
+ //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 0, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
+ }
+ else if (managed_obstacle->m_value < 0 && m_remove_enabled) // Completely forget the obstacle
+ {
+ remove_obstacles.push_back(managed_obstacle->m_obstacle.name);
+ }
+ else if (managed_obstacle->m_updated)
+ {
+ visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{1, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_min_value_for_accepting)}, 40, true);
+ }
+ else
+ {
+ visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{1, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_min_value_for_accepting)}, 40, true);
+ }
+ }
+ managed_obstacle->m_updated = false;
+ }
+ }
+
+ if (updated)
+ {
+ m_obstacle_detection->updateEnvironment();
+ }
+ arviz.commit({obstacleLayer});
+
+ if (!remove_obstacles.empty())
+ {
+ std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
+ for (const auto& name : remove_obstacles)
+ {
+ // TODO schöner machen ohne loop. Iteratoren in main loop
+ m_managed_obstacles.erase(
+ std::remove_if(
+ m_managed_obstacles.begin(),
+ m_managed_obstacles.end(),
+ [&](const ManagedObstaclePtr & oc)
+ {
+ return oc->m_obstacle.name == name;
+ }
+ ),
+ m_managed_obstacles.end()
+ );
+ }
+ }
+
+ ARMARX_DEBUG << "Finished updating the " << checked_obstacles << " managed obstacles (removed: " << remove_obstacles.size() << ", updated: " << updated_obstacles << "). " << published_obstacles << " of them should be published. The whole operation took " << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
+ }
+
+ void DynamicObstacleManager::visualize_obstacle(viz::Layer& layer, const ManagedObstaclePtr& o, const armarx::DrawColor& color, double pos_z, bool text)
+ {
+ // Visualize ellipses.m_obstacle_manager_layer_name
+ Eigen::Vector3f dim(o->m_obstacle.axisLengthX * 2,
+ o->m_obstacle.axisLengthY * 2,
+ o->m_obstacle.axisLengthZ * 2);
+
+ const std::string name = o->m_obstacle.name;
+
+ layer.add(viz::Box(name).position(Eigen::Vector3f(o->m_obstacle.posX, o->m_obstacle.posY, pos_z))
+ .orientation(Eigen::AngleAxisf(o->m_obstacle.yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix())
+ .size(dim).color(color.r, color.g, color.b, color.a));
+
+ }
+
+
+ armarx::PropertyDefinitionsPtr DynamicObstacleManager::createPropertyDefinitions()
+ {
+ PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
+
+ defs->component(m_obstacle_detection, "PlatformObstacleAvoidance", "ObstacleAvoidanceName", "The name of the used obstacle avoidance interface");
+
+ defs->optional(m_min_coverage_of_obstacles, "MinSampleRatioPerEllipsis", "Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle");
+ defs->optional(m_min_coverage_of_line_samples_in_obstacle, "MinSampleRatioPerLineSegment", "Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle");
+ defs->optional(m_min_size_of_obstacles, "MinObstacleSize", "The minimal obstacle size in mm.");
+ defs->optional(m_max_size_of_obstacles, "MaxObstacleSize", "The maximal obstacle size in mm.");
+ defs->optional(m_min_length_of_lines, "MinLengthOfLines", "Minimum length of lines in mm");
+ defs->optional(m_max_length_of_lines, "MaxLengthOfLines", "Maximum length of lines in mm");
+ defs->optional(m_decay_after_ms, "MaxObstacleValue", "Maximum value for the obstacles");
+ defs->optional(m_min_value_for_accepting, "MinObstacleValueForAccepting", "Minimum value for the obstacles to get accepted");
+ defs->optional(m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
+ defs->optional(m_thickness_of_lines, "LineThickness", "The thickness of line obstacles");
+ defs->optional(m_security_margin_for_obstacles, "ObstacleSecurityMargin", "Security margin of ellipsis obstacles");
+ defs->optional(m_security_margin_for_lines, "LineSecurityMargin", "Security margin of line obstacles");
+ defs->optional(m_remove_enabled, "EnableRemove", "Delete Obstacles when value < 0");
+ defs->optional(m_only_visualize, "OnlyVisualizeObstacles", "Connection to obstacle avoidance");
+ defs->optional(m_allow_spwan_inside, "AllowInRobotSpawning", "Allow obstacles to spawn inside the robot");
+
+ return defs;
+ }
+}
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..371ccbc3e9099e5baed3ae8165cea453ba2059f9
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -0,0 +1,123 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <array>
+#include <string>
+#include <tuple>
+#include <vector>
+#include <shared_mutex>
+
+// Eigen
+#include <SimoxUtility/EigenStdVector.h>
+#include <Eigen/Geometry>
+
+// Ice
+#include <Ice/Current.h>
+
+// ArmarX
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+// Managed Objects
+#include <RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h>
+
+// ObstacleAvoidance
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
+
+// ArViz
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+namespace armarx
+{
+
+ class DynamicObstacleManager :
+ virtual public Component,
+ virtual public DynamicObstacleManagerInterface,
+ virtual public ArVizComponentPluginUser
+ {
+ public:
+
+ DynamicObstacleManager() noexcept;
+
+ std::string getDefaultName() const override;
+
+ 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 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;
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ private:
+ void update_decayable_obstacles();
+
+ void visualize_obstacle(viz::Layer& l, const ManagedObstaclePtr& o, const armarx::DrawColor& color, double z_pos, bool);
+
+ public:
+ static const std::string default_name;
+
+ private:
+ const std::string m_obstacle_manager_layer_name = "DynamicObstacleManagerObstacles";
+
+ unsigned long m_obstacle_index;
+
+ std::vector<ManagedObstaclePtr> m_managed_obstacles;
+ std::shared_mutex m_managed_obstacles_mutex;
+
+ PeriodicTask<DynamicObstacleManager>::pointer_type m_update_obstacles;
+ unsigned int m_decay_after_ms;
+ unsigned int m_periodic_task_interval;
+ unsigned int m_decay_factor;
+ unsigned int m_access_bonus;
+ unsigned int m_min_value_for_accepting;
+
+ float m_min_coverage_of_obstacles;
+ float m_min_coverage_of_line_samples_in_obstacle;
+ unsigned int m_min_size_of_obstacles;
+ unsigned int m_min_length_of_lines;
+ unsigned int m_max_size_of_obstacles;
+ unsigned int m_max_length_of_lines;
+ unsigned int m_thickness_of_lines;
+ unsigned int m_security_margin_for_obstacles;
+ unsigned int m_security_margin_for_lines;
+
+ bool m_remove_enabled;
+ bool m_only_visualize;
+ bool m_allow_spwan_inside;
+
+ ObstacleDetectionInterface::ProxyType m_obstacle_detection;
+
+ };
+}
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d33d61f70cf01d5a54889c2f5c487719cb3fafc
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -0,0 +1,313 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ManagedObstacle.h"
+
+// STD/STL
+#include <string>
+#include <vector>
+
+// Ice
+#include <Ice/Current.h>
+using namespace Ice;
+
+// OpenCV
+#include <opencv2/core.hpp>
+
+// Eigen
+#include <Eigen/Core>
+
+// ArmarX
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+using namespace Eigen;
+using namespace armarx;
+
+namespace armarx
+{
+
+ double ManagedObstacle::calculateObstacleArea(const obstacledetection::Obstacle& o)
+ {
+ return M_PI * std::abs(o.axisLengthX) * std::abs(o.axisLengthY);
+ }
+
+ bool ManagedObstacle::point_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f point)
+ {
+ // See https://stackoverflow.com/questions/7946187/point-and-ellipse-rotated-position-test-algorithm
+ const float sin = std::sin(e_yaw);
+ const float cos = std::cos(e_yaw);
+
+ const float a = cos * (point(0) - e_origin(0)) + sin * (point(1) - e_origin(1));
+ const float b = sin * (point(0) - e_origin(0)) - cos * (point(1) - e_origin(1));
+
+ const float e_rx_sq = e_rx * e_rx;
+ const float e_ry_sq = e_ry * e_ry;
+
+ return a * a * e_ry_sq + b * b * e_rx_sq <= e_rx_sq * e_ry_sq;
+ }
+
+ float ManagedObstacle::ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin, const float e1_rx, const float e1_ry, const float e1_yaw, const Eigen::Vector2f e2_origin, const float e2_rx, const float e2_ry, const float e2_yaw)
+ {
+ // We use a very simple approach here: We sample points in one ellipsis and check whether it lies in the other (https://www.quora.com/Is-it-possible-to-generate-random-points-within-a-given-rotated-ellipse-without-using-rejection-sampling)
+ // For a real approach to intersect two ellipsis see https://arxiv.org/pdf/1106.3787.pdf
+
+ unsigned int SAMPLES = 100;
+ unsigned int matches = 0;
+ for (unsigned int i = 0; i < SAMPLES; ++i)
+ {
+ float theta = static_cast<float>(rand()) / static_cast<float>(RAND_MAX / 2 * M_PI);
+ float k = sqrt(static_cast<float>(rand()) / static_cast<float>(RAND_MAX));
+ Eigen::Vector2f sample(k * e2_rx * cos(theta), k * e2_ry * sin(theta));
+ //sample *= Eigen::Rotation2D(e2_yaw);
+ sample += e1_origin;
+
+ if (ManagedObstacle::point_ellipsis_coverage(e1_origin, e1_rx, e1_ry, e1_yaw, sample))
+ {
+ matches++;
+ }
+ }
+
+ return (1.0f * matches) / SAMPLES;
+ }
+
+ 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 float step_size = distance / SAMPLES;
+ const Eigen::Vector2f dir = difference_vec_normed * step_size;
+
+ unsigned int samples_in_ellipsis = 0;
+
+ // Sample over line segment with a fixed size
+ for (unsigned int i = 0; i < SAMPLES * 0.5; ++i)
+ {
+ Eigen::Vector2f start_sample = line_seg_start + i * dir;
+ Eigen::Vector2f end_sample = line_seg_end - i * dir;
+ unsigned int samples_in_ellipsis_this_iteration = 0;
+
+ for (const auto& point :
+ {
+ start_sample, end_sample
+ })
+ {
+ if (ManagedObstacle::point_ellipsis_coverage(e_origin, e_rx, e_ry, e_yaw, point))
+ {
+ ++samples_in_ellipsis;
+ ++samples_in_ellipsis_this_iteration;
+ }
+ }
+
+ if (samples_in_ellipsis_this_iteration == 2)
+ {
+ // Last two points (from the outside) were inside this ellipsis, so the
+ // remaining ones are as well simce ellipsis are concave. Consider this line
+ // segment as explained by the obstacle and bail early.
+ const unsigned int remaining_samples = SAMPLES - 2 * (i + 1);
+ return (1.0f * samples_in_ellipsis + remaining_samples) / SAMPLES;
+ }
+ }
+ return (1.0f * samples_in_ellipsis) / SAMPLES; // only if one point or no point in ellipsis
+ }
+
+ void ManagedObstacle::update_position(const unsigned int thickness)
+ {
+ if (line_matches.size() < 2)
+ {
+ return;
+ }
+
+ ARMARX_INFO << "Obstacle " << m_obstacle.name << " has " << line_matches.size() << " matching line segments since last update";
+
+ std::vector<float> x_pos;
+ std::vector<float> y_pos;
+ for (Eigen::Vector2f& match : line_matches)
+ {
+ ARMARX_INFO << "Match: " << match;
+ x_pos.push_back(match(0));
+ y_pos.push_back(match(1));
+ }
+
+ cv::Mat x(x_pos.size(), 1, CV_32F, x_pos.data());
+ cv::Mat y(y_pos.size(), 1, CV_32F, y_pos.data());
+
+ ARMARX_INFO << "X: " << x;
+ ARMARX_INFO << "Y: " << y;
+
+ cv::Mat data(x_pos.size(), 2, CV_32F);
+ x.col(0).copyTo(data.col(0));
+ y.col(0).copyTo(data.col(1));
+
+ ARMARX_INFO << "Data: " << data;
+
+ cv::PCA pca(data, cv::Mat(), CV_PCA_DATA_AS_ROW, 2);
+
+ cv::Mat mean = pca.mean;
+ ARMARX_INFO << "Mean: " << mean;
+ cv::Mat eigenvalues = pca.eigenvalues;
+ ARMARX_INFO << "Eigenvalues: " << eigenvalues;
+ cv::Mat eigenvectors = pca.eigenvectors;
+ ARMARX_INFO << "Eigenvectors: " << eigenvectors;
+
+ Eigen::Vector2f pca_center(mean.at<float>(0, 0), mean.at<float>(0, 1));
+ Eigen::Vector2f pca1_direction(eigenvectors.at<float>(0, 0), eigenvectors.at<float>(0, 1));
+ Eigen::Vector2f pca2_direction(eigenvectors.at<float>(1, 0), eigenvectors.at<float>(1, 1));
+ double pca1_eigenvalue = eigenvalues.at<float>(0, 0);
+ double pca2_eigenvalue = eigenvalues.at<float>(1, 0);
+
+ ARMARX_INFO << "PCA: Mean: " << pca_center;
+ ARMARX_INFO << "PCA1: Eigenvector: " << pca1_direction << ", Eigenvalue: " << pca1_eigenvalue;
+ ARMARX_INFO << "PCA2: Eigenvector: " << pca2_direction << ", Eigenvalue: " << pca2_eigenvalue;
+
+ // calculate yaw of best line (should be close to current yaw)
+ const Eigen::Vector2f difference_vec = pca1_direction;
+ const Eigen::Vector2f center_vec = pca_center;
+ //const Eigen::Vector2f difference_vec1_normed = difference_vec.normalized();
+
+ const float yaw1 = getXAxisAngle(difference_vec);
+ const float yaw2 = getXAxisAngle(-1 * difference_vec);
+
+ const float diff_origin_yaw1 = std::abs(yaw1 - m_obstacle.yaw);
+ const float diff_origin_yaw2 = std::abs(yaw2 - m_obstacle.yaw);
+ const float diff_origin_yaw1_opposite = std::abs(2 * M_PI - diff_origin_yaw1);
+ const float diff_origin_yaw2_opposite = std::abs(2 * M_PI - diff_origin_yaw2);
+
+
+ float yaw = yaw2;
+ if ((diff_origin_yaw1 < diff_origin_yaw2 && diff_origin_yaw1 < diff_origin_yaw2_opposite) || (diff_origin_yaw1_opposite < diff_origin_yaw2 && diff_origin_yaw1_opposite < diff_origin_yaw2_opposite))
+ {
+ yaw = yaw1;
+ }
+
+ // Print matches to debug drawer
+ //const std::string debug_line_name = "line_segment_input_" + std::to_string(m_input_index++);
+ //const armarx::Vector3BasePtr start_vec_3d{new Vector3(best_start(0), best_start(1), 50)};
+ //const armarx::Vector3BasePtr end_vec_3d{new Vector3(best_end(0), best_end(1), 50)};
+ //const armarx::Vector3BasePtr center_vec_3d{new Vector3(center_vec(0), center_vec(1), 50)};
+ //const armarx::Vector3BasePtr difference_vec_normed_3d{new Vector3(difference_vec1_normed(0), difference_vec1_normed(1), 0)};
+ //debug_drawer->setCylinderVisu(layer_name, debug_line_name, center_vec_3d, difference_vec_normed_3d, max_length, 15.f, armarx::DrawColor{1, 0, 0, 0.8});
+ //debug_drawer->setSphereVisu(layer_name, debug_line_name + "_ref", start_vec_3d, armarx::DrawColor{1, 1, 0, 0.8}, 35);
+ //debug_drawer->setSphereVisu(layer_name, debug_line_name + "_ref2", end_vec_3d, armarx::DrawColor{0, 1, 0, 0.8}, 35);
+
+ // Udate position buffer with new best line
+ position_buffer.at(position_buffer_index++) = std::make_tuple(center_vec(0), center_vec(1), yaw, sqrt(pca1_eigenvalue) * 1.3, std::max(1.0f * thickness, static_cast<float>(sqrt(pca2_eigenvalue)) * 1.3f)); // add 30% security margin to length of pca
+ position_buffer_index %= position_buffer.size();
+
+ position_buffer_fillctr++;
+ position_buffer_fillctr = std::min(position_buffer_fillctr, static_cast<unsigned int>(position_buffer.size()));
+
+ // Calculate means from position buffer
+ double sum_x_pos = 0;
+ double sum_y_pos = 0;
+ double sum_yaw = 0;
+ double sum_sin_yaw = 0;
+ double sum_cos_yaw = 0;
+ double sum_axisX = 0;
+ double sum_axisY = 0;
+
+ std::string yaws;
+ for (unsigned int i = 0; i < position_buffer_fillctr; ++i)
+ {
+ double current_x_pos = std::get<0>(position_buffer[i]);
+ double current_y_pos = std::get<1>(position_buffer[i]);
+ double current_yaw = std::get<2>(position_buffer[i]);
+ double current_axisX = std::get<3>(position_buffer[i]);
+ double current_axisY = std::get<4>(position_buffer[i]);
+ sum_x_pos += current_x_pos;
+ sum_y_pos += current_y_pos;
+ sum_yaw += current_yaw;
+ sum_sin_yaw += sin(current_yaw);
+ sum_cos_yaw += cos(current_yaw);
+ sum_axisX += current_axisX;
+ sum_axisY += current_axisY;
+
+ yaws += std::to_string(current_yaw) + ", ";
+ }
+
+ double mean_x_pos = sum_x_pos / position_buffer_fillctr;
+ double mean_y_pos = sum_y_pos / position_buffer_fillctr;
+ double mean_yaw = atan2((1.0 / position_buffer_fillctr) * sum_sin_yaw, (1.0 / position_buffer_fillctr) * sum_cos_yaw); //sum_yaw / position_buffer_fillctr; //std::fmod(sum_yaw, 2.0 * M_PI);
+ double mean_axisX = sum_axisX / position_buffer_fillctr;
+ double mean_axisY = sum_axisY / position_buffer_fillctr;
+ mean_yaw = normalizeYaw(mean_yaw);
+
+ // Update position and size of obstacle
+ m_obstacle.posX = mean_x_pos;
+ m_obstacle.posY = mean_y_pos;
+ m_obstacle.refPosX = mean_x_pos;
+ m_obstacle.refPosY = mean_y_pos;
+ m_obstacle.yaw = mean_yaw;
+ //if (mean_axisX > m_obstacle.axisLengthX)
+ //{
+ m_obstacle.axisLengthX = mean_axisX;
+ //}
+ //if (mean_axisY > m_obstacle.axisLengthY)
+ //{
+ m_obstacle.axisLengthY = mean_axisY;
+ //}
+
+ line_matches.clear();
+ m_updated = true;
+ }
+
+ float ManagedObstacle::normalizeYaw(float yaw) const
+ {
+ float pi2 = 2.0 * M_PI;
+ while (yaw < 0)
+ {
+ yaw += pi2;
+ }
+ while (yaw > pi2)
+ {
+ yaw -= pi2;
+ }
+ return yaw;
+ }
+
+ float ManagedObstacle::getAngleBetweenVectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2) const
+ {
+ // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
+ const Eigen::Vector2f v1_vec_normed = v1.normalized();
+ const Eigen::Vector2f v2_vec_normed = v2.normalized();
+ const float dot_product_vec = v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
+ float yaw = acos(dot_product_vec);
+ return yaw;
+ }
+
+ float ManagedObstacle::getXAxisAngle(const Eigen::Vector2f& v) const
+ {
+ // Returns an angle between 0 and 360 degree (counterclockwise from x axis)
+ const Eigen::Vector2f v_vec_normed = v.normalized();
+ const float dot_product_vec = v_vec_normed(0);
+ const float cross_product_vec = v_vec_normed(1);
+ float yaw = acos(dot_product_vec);
+ if (cross_product_vec < 0)
+ {
+ yaw = 2 * M_PI - yaw;
+ }
+ return yaw;
+ }
+}
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
new file mode 100644
index 0000000000000000000000000000000000000000..95b7491d2d08e989321dcfdf30463acd46a95e9d
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
@@ -0,0 +1,90 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <array>
+#include <tuple>
+#include <shared_mutex>
+#include <string>
+#include <vector>
+
+// Eigen
+#include <Eigen/Geometry>
+
+// ArmarX
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
+
+
+namespace armarx
+{
+
+ class ManagedObstacle;
+ typedef std::shared_ptr<ManagedObstacle> ManagedObstaclePtr;
+
+ class ManagedObstacle
+ {
+ public:
+
+ static bool ComparatorDESCPrt(ManagedObstaclePtr& i, ManagedObstaclePtr& j)
+ {
+ return (ManagedObstacle::calculateObstacleArea(i->m_obstacle) > ManagedObstacle::calculateObstacleArea(j->m_obstacle));
+ }
+
+ void update_position(const unsigned int);
+
+ static double calculateObstacleArea(const obstacledetection::Obstacle& o);
+
+ static bool point_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f point);
+
+ static float ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin, const float e1_rx, const float e1_ry, const float e1_yaw, const Eigen::Vector2f e2_origin, const float e2_rx, const float e2_ry, const float e2_yaw);
+
+ static float 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);
+
+
+ obstacledetection::Obstacle m_obstacle;
+ IceUtil::Time m_last_matched;
+ bool m_published;
+ bool m_updated;
+ float m_value;
+ std::shared_mutex m_mutex;
+
+ unsigned int position_buffer_fillctr = 0;
+ unsigned int position_buffer_index = 0;
+ std::array<std::tuple<double, double, double, double, double>, 6> position_buffer; //x, y, yaw, axisX, axisY
+ std::vector<Eigen::Vector2f> line_matches; // points of line segments
+
+
+ unsigned long m_input_index = 0;
+ protected:
+
+ private:
+ float normalizeYaw(float yaw) const;
+ float getAngleBetweenVectors(const Eigen::Vector2f&, const Eigen::Vector2f&) const;
+ float getXAxisAngle(const Eigen::Vector2f&) const;
+
+ };
+}
diff --git a/source/RobotAPI/components/DynamicObstacleManager/test/CMakeLists.txt b/source/RobotAPI/components/DynamicObstacleManager/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7c1102988e196a553e2961ac2fda0a02cf286f15
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore DynamicObstacleManager)
+
+armarx_add_test(DynamicObstacleManagerTest DynamicObstacleManagerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52fa2345cd6c12f0cd6fd7cecb3a43564731605a
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Armar6Skills::ArmarXObjects::DynamicObstacleManager
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <Armar6Skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::DynamicObstacleManager instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 6ca8b8ee106668475988c67a2b26db34f895b640..c13ab4dcd519f7a19aec3f2af919b8a0a793604f 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -81,9 +81,7 @@ set(SLICE_FILES
units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
- components/DSObstacleAvoidanceInterface.ice
- components/FrameTrackingInterface.ice
- components/ObstacleAvoidanceInterface.ice
+ components/FrameTrackingInterface.ice
components/RobotHealthInterface.ice
components/RobotNameServiceInterface.ice
components/TrajectoryPlayerInterface.ice
@@ -98,6 +96,11 @@ set(SLICE_FILES
aron.ice
armem.ice
+
+ components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
+ components/ObstacleAvoidance/ObstacleDetectionInterface.ice
+ components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
+ components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
)
#core/RobotIK.ice
set(SLICE_FILES_ADDITIONAL_HEADERS
diff --git a/source/RobotAPI/interface/components/DSObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
similarity index 55%
rename from source/RobotAPI/interface/components/DSObstacleAvoidanceInterface.ice
rename to source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
index 13dead6ac09b9b1028c204e56bbc8e34285d065c..04176dc066828cfc28577851d556464880db076d 100644
--- a/source/RobotAPI/interface/components/DSObstacleAvoidanceInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
@@ -28,8 +28,8 @@
#include <ArmarXCore/interface/serialization/Eigen.ice>
// RobotAPI
-#include <RobotAPI/interface/components/ObstacleAvoidanceInterface.ice>
-
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice>
module armarx
{
@@ -49,51 +49,15 @@ module armarx
double agent_safety_margin;
};
-
- struct Obstacle
- {
- string name;
- double posX = 0;
- double posY = 0;
- double posZ = 0;
- double yaw = 0;
- double axisLengthX = 0;
- double axisLengthY = 0;
- double axisLengthZ = 0;
- double refPosX = 0;
- double refPosY = 0;
- double refPosZ = 0;
- double safetyMarginX = 0;
- double safetyMarginY = 0;
- double safetyMarginZ = 0;
- double curvatureX = 1;
- double curvatureY = 1;
- double curvatureZ = 1;
- };
-
-
- sequence<dsobstacleavoidance::Obstacle> ObstacleList;
-
};
- interface DSObstacleAvoidanceInterface extends ObstacleAvoidanceInterface
+ interface DSObstacleAvoidanceInterface extends
+ ObstacleAvoidanceInterface, ObstacleDetectionInterface
{
- dsobstacleavoidance::Config
- getConfig();
-
- void
- updateObstacle(dsobstacleavoidance::Obstacle obstacle);
-
- void
- removeObstacle(string obstacle_name);
-
- dsobstacleavoidance::ObstacleList
- getObstacles();
-
- void
- updateEnvironment();
+ dsobstacleavoidance::Config
+ getConfig();
void
writeDebugPlots(string filename);
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..9552091c342285ccbfe1be5603ed105a1708e792
--- /dev/null
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -0,0 +1,51 @@
+/*
+ * 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 Armar6Skills::ArmarXObjects::HumanAvoidance
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @author Fabian Peller
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+// ArmarX
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+
+module armarx
+{
+ interface DynamicObstacleManagerInterface
+ {
+ void
+ add_decayable_obstacle(Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
+
+ void
+ add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+
+ void
+ remove_all_decayable_obstacles();
+
+ void
+ directly_update_obstacle(string name, Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
+
+ void
+ remove_obstacle(string name);
+ };
+};
+
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
similarity index 100%
rename from source/RobotAPI/interface/components/ObstacleAvoidanceInterface.ice
rename to source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0a0435b203c00767deeb7e52fc4af93244c3a080
--- /dev/null
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice
@@ -0,0 +1,79 @@
+/*
+ * 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::ObstacleAvoidance
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// ArmarX
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+
+module armarx
+{
+
+ module obstacledetection
+ {
+
+ struct Obstacle
+ {
+ string name;
+ double posX = 0;
+ double posY = 0;
+ double posZ = 0;
+ double yaw = 0;
+ double axisLengthX = 0;
+ double axisLengthY = 0;
+ double axisLengthZ = 0;
+ double refPosX = 0;
+ double refPosY = 0;
+ double refPosZ = 0;
+ double safetyMarginX = 0;
+ double safetyMarginY = 0;
+ double safetyMarginZ = 0;
+ double curvatureX = 1;
+ double curvatureY = 1;
+ double curvatureZ = 1;
+ };
+
+ sequence<obstacledetection::Obstacle> ObstacleList;
+
+ };
+
+
+ interface ObstacleDetectionInterface
+ {
+ void
+ updateObstacle(obstacledetection::Obstacle obstacle);
+
+ void
+ removeObstacle(string obstacle_name);
+
+ obstacledetection::ObstacleList
+ getObstacles();
+
+ void
+ updateEnvironment();
+
+ };
+
+};