diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index ef43ecebfd31e5a6e7e8db6ac1fa89acfb06e715..9d57e40ea3d4b9902bc04738f66cb15ab532f233 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -267,17 +267,18 @@
<Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h"
humanName="Platform obstacle avoidance interface"
typeName="ObstacleAvoidanceInterfacePrx"
- memberName="platformObstacleAvoidance"
- getterName="getPlatformObstacleAvoidance"
- propertyName="PlatformObstacleAvoidanceName"
+ memberName="obstacleAvoidance"
+ getterName="getObstacleAvoidance"
+ propertyName="ObstacleAvoidanceName"
propertyIsOptional="true"
propertyDefaultValue="PlatformObstacleAvoidance" />
- <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectorInterface.h"
+ <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h"
humanName="Platform obstacle detection interface"
typeName="ObstacleDetectionInterfacePrx"
- memberName="platformObstacleDetection"
- getterName="getPlatformObstacleDetection"
- propertyName="PlatformObstacleDetectionName"
+ memberName="obstacleDetection"
+ getterName="getObstacleDetection"
+ propertyName="ObstacleDetectionName"
+ propertyIsOptional="true"
propertyDefaultValue="PlatformObstacleAvoidance" />
<Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
humanName="Dynamic obstacle manager interface"
@@ -285,6 +286,7 @@
memberName="dynamicObstacleManager"
getterName="getDynamicObstacleManager"
propertyName="DynamicObstacleManagerName"
+ propertyIsOptional="true"
propertyDefaultValue="DynamicObstacleManager" />
<Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
humanName="Grasp Candidate Observer"
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
index ee891427aebe42fd391a64efee478ab2e9b260f0..4a83bdedaabbabcc2f46bc4e2815a826ee661156 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
@@ -316,22 +316,22 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co
std::vector<armarx::obstacledetection::Obstacle>
armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
{
+ ARMARX_DEBUG << "Get Obstacles";
std::shared_lock l{m_doa.mutex};
std::vector<obstacledetection::Obstacle> obstacle_list;
for (auto& [doa_name, doa_obstacle] : m_doa.env)
{
- std::shared_ptr<doa::Ellipsoid> doa_ellipsoid =
- std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
+ ARMARX_DEBUG << "Got an obtascle";
+ std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
obstacledetection::Obstacle obstacle;
obstacle.name = doa_name;
obstacle.posX = doa_ellipsoid->get_position()(0) * 1000;
obstacle.posY = doa_ellipsoid->get_position()(1) * 1000;
obstacle.posZ = doa_ellipsoid->get_position()(2) * 1000;
- obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation()
- .toRotationMatrix().cast<float>()).z();
+ obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>()).z();
obstacle.axisLengthX = doa_ellipsoid->get_axis_lengths(0) * 1000;
obstacle.axisLengthY = doa_ellipsoid->get_axis_lengths(1) * 1000;
obstacle.axisLengthZ = doa_ellipsoid->get_axis_lengths(2) * 1000;
@@ -345,9 +345,11 @@ armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
obstacle.curvatureY = doa_ellipsoid->get_curvature_factor()(1);
obstacle.curvatureZ = doa_ellipsoid->get_curvature_factor()(2);
+ ARMARX_DEBUG << "push back " << obstacle.name;
obstacle_list.push_back(std::move(obstacle));
}
+ ARMARX_DEBUG << "Return obstacle list";
return obstacle_list;
}
@@ -357,6 +359,8 @@ armarx::DSObstacleAvoidance::modulateVelocity(
const obstacleavoidance::Agent& agent,
const Ice::Current&)
{
+
+ ARMARX_DEBUG << "Modulate velocity";
std::shared_lock l{m_doa.mutex};
// Create and initialize agent.
@@ -404,7 +408,7 @@ void
armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
{
ARMARX_VERBOSE << "Updating environment.";
-
+ std::lock_guard l2{m_vis.mutex};
std::lock_guard l{m_doa.mutex};
// Make working copies of the updates and free them again.
@@ -471,7 +475,6 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
m_doa.env.update();
{
- std::lock_guard l{m_vis.mutex};
m_vis.needs_update = true;
}
@@ -483,6 +486,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
void
armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const Ice::Current&)
{
+ ARMARX_DEBUG << "Write debug plots";
std::shared_lock l{m_doa.mutex};
const std::string filename_annotated = getName() + "_" + filename;
@@ -532,6 +536,7 @@ const
void
armarx::DSObstacleAvoidance::run_visualization()
{
+ ARMARX_DEBUG << "Run visualization";
using namespace armarx::viz;
std::lock_guard l{m_vis.mutex};
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index ccc5409fc336854e175d0245f7a24a5b973f5bb7..46515a2c2f9a61acbaf63c186920478dab82cb1c 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -143,6 +143,7 @@ namespace armarx
// 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,
@@ -219,16 +220,16 @@ namespace armarx
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)
+ 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
+ break;
}
}
+ m_obstacle_detection->removeObstacle(name);
+ m_obstacle_detection->updateEnvironment();
}
void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
@@ -257,6 +258,7 @@ namespace armarx
void DynamicObstacleManager::update_decayable_obstacles()
{
+ ARMARX_DEBUG << "update obstacles";
IceUtil::Time started = IceUtil::Time::now();
std::vector<std::string> remove_obstacles;
@@ -265,6 +267,7 @@ namespace armarx
// Update positions
{
+ ARMARX_DEBUG << "update obstacle position";
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
if (!m_managed_obstacles.size())
{
@@ -285,6 +288,7 @@ namespace armarx
int published_obstacles = 0;
int updated_obstacles = 0;
{
+ ARMARX_DEBUG << "update obstacle values and publish";
std::lock_guard l(m_managed_obstacles_mutex);
std::sort(m_managed_obstacles.begin(), m_managed_obstacles.end(), ManagedObstacle::ComparatorDESCPrt);
@@ -366,6 +370,7 @@ namespace armarx
if (updated)
{
+ ARMARX_DEBUG << "update environment";
m_obstacle_detection->updateEnvironment();
}
arviz.commit({obstacleLayer});
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
index 04176dc066828cfc28577851d556464880db076d..42ed92db3ba81fe2a0255bab1e9a98c9f386991a 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
@@ -56,8 +56,7 @@ module armarx
ObstacleAvoidanceInterface, ObstacleDetectionInterface
{
- dsobstacleavoidance::Config
- getConfig();
+ dsobstacleavoidance::Config getConfig();
void
writeDebugPlots(string filename);