diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 0913ca80116f59b1fdb69f4fd105c7b0c72fb598..9d57e40ea3d4b9902bc04738f66cb15ab532f233 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -264,14 +264,30 @@
propertyName="TextToSpeechTopicName"
propertyIsOptional="true"
propertyDefaultValue="TextToSpeech" />
- <Proxy include="RobotAPI/interface/components/DSObstacleAvoidanceInterface.h"
- humanName="Platform obstacle avoidance"
+ <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/ObstacleDetectionInterface.h"
+ humanName="Platform obstacle detection interface"
+ typeName="ObstacleDetectionInterfacePrx"
+ memberName="obstacleDetection"
+ getterName="getObstacleDetection"
+ propertyName="ObstacleDetectionName"
+ propertyIsOptional="true"
+ propertyDefaultValue="PlatformObstacleAvoidance" />
+ <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
+ humanName="Dynamic obstacle manager interface"
+ typeName="DynamicObstacleManagerInterfacePrx"
+ memberName="dynamicObstacleManager"
+ getterName="getDynamicObstacleManager"
+ propertyName="DynamicObstacleManagerName"
+ propertyIsOptional="true"
+ propertyDefaultValue="DynamicObstacleManager" />
<Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
humanName="Grasp Candidate Observer"
typeName="grasping::GraspCandidateObserverInterfacePrx"
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/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index 0afaed33a07d95b74c2325e6fd7dfa64e018938b..08b7d20ff5440d018e2c39604ab00dd7b4918f23 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -28,8 +28,6 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <boost/circular_buffer.hpp>
-
#include <mutex>
#include <atomic>
#include <condition_variable>
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
index d1e10e97f8a89dafba0671a704f5814f5ca5ca84..3a066c978c6464055b8968e9c8c893b6b91412f4 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
@@ -9,6 +9,8 @@
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <boost/algorithm/string/predicate.hpp>
+
namespace armarx::viz::coin
{
@@ -46,7 +48,19 @@ namespace armarx::viz::coin
try
{
ARMARX_INFO << "Loading object from " << fullFilename;
- result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename);
+
+ if (boost::ends_with(fullFilename, ".wrl"))
+ {
+ VirtualRobot::VisualizationFactoryPtr factory = VirtualRobot::VisualizationFactory::fromName("inventor", NULL);
+ VirtualRobot::VisualizationNodePtr vis = factory->getVisualizationFromFile(fullFilename);
+ result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, vis));
+
+ }
+ else
+ {
+ result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename);
+
+ }
}
catch (std::exception const& ex)
{
@@ -55,6 +69,7 @@ namespace armarx::viz::coin
}
+
return result;
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index d9beef41b45235611beec0b6d02bc66fadcc4178..f11c7aa9de6672cafcda2bbcfdb2e5114eb2be69 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -1,13 +1,12 @@
#include "Visualizer.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoUnits.h>
#include <thread>
-#include <boost/core/demangle.hpp>
-
#include "VisualizationRobot.h"
namespace armarx::viz
@@ -168,7 +167,7 @@ namespace armarx::viz
{
ARMARX_WARNING << deactivateSpam(1)
<< "No visualizer for element type found: "
- << boost::core::demangle(elementType.name());
+ << armarx::GetTypeString(elementType);
continue;
}
coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
@@ -216,7 +215,7 @@ namespace armarx::viz
}
else
{
- std::string typeName = boost::core::demangle(elementType.name());
+ std::string typeName = armarx::GetTypeString(elementType);
ARMARX_WARNING << deactivateSpam(typeName, 1)
<< "CoinElementVisualizer returned null for type: " << typeName << "\n"
<< "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
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/CyberGloveObserver/CyberGloveObserver.cpp b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
index a68eb49d875c6d8eece619f72e2b6f64bd82a08a..96b0f863c87245ddf16d84c983e08407f39e3331 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
@@ -56,7 +56,7 @@ namespace armarx
void CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
IceUtil::Time now = TimeUtil::GetTime();
@@ -107,7 +107,7 @@ namespace armarx
CyberGloveValues armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
return latestValues;
}
}
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
index 8eb91e4654620598605f4b04fc1f4c8629ef0048..45eb2f52a2c86c47c8c0272a6f2c55ce9c39b865 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
@@ -23,13 +23,12 @@
#pragma once
-//#include <ArmarXCore/core/Component.h>
-//#include <RobotAPI/interface/units/CyberGloveInterface.h>
#include <RobotAPI/interface/units/CyberGloveObserverInterface.h>
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <mutex>
namespace armarx
{
@@ -103,7 +102,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
CyberGloveValues latestValues;
IceUtil::Time lastUpdate;
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/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
index 5278fcda731543f4cdb98c8a05e872067d214bef..4a83bdedaabbabcc2f46bc4e2815a826ee661156 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
@@ -59,7 +59,7 @@ namespace
{
std::shared_ptr<doa::Obstacle>
- create_doa_obstacle(const armarx::dsobstacleavoidance::Obstacle& obstacle)
+ create_doa_obstacle(const armarx::obstacledetection::Obstacle& obstacle)
{
Eigen::Vector3d pos(obstacle.posX, obstacle.posY, obstacle.posZ);
Eigen::Quaterniond ori{Eigen::AngleAxisd(obstacle.yaw, Eigen::Vector3d::UnitZ())};
@@ -87,11 +87,11 @@ namespace
}
-namespace armarx::dsobstacleavoidance
+namespace armarx::obstacledetection
{
void
- from_json(const nlohmann::json& j, Obstacle& obstacle)
+ from_json(const nlohmann::json& j, armarx::obstacledetection::Obstacle& obstacle)
{
j.at("name").get_to(obstacle.name);
j.at("posX").get_to(obstacle.posX);
@@ -176,8 +176,8 @@ armarx::DSObstacleAvoidance::onInitComponent()
std::ifstream ifs(scene_obstacles_path);
nlohmann::json j = nlohmann::json::parse(ifs);
- std::vector<dsobstacleavoidance::Obstacle> obstacles = j;
- for (dsobstacleavoidance::Obstacle& obstacle : obstacles)
+ std::vector<obstacledetection::Obstacle> obstacles = j;
+ for (obstacledetection::Obstacle& obstacle : obstacles)
{
if (m_doa.cfg.only_2d)
{
@@ -272,7 +272,7 @@ armarx::DSObstacleAvoidance::getConfig(const Ice::Current&)
void
armarx::DSObstacleAvoidance::updateObstacle(
- const armarx::dsobstacleavoidance::Obstacle& obstacle,
+ const armarx::obstacledetection::Obstacle& obstacle,
const Ice::Current&)
{
ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << ".";
@@ -313,25 +313,25 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co
}
-std::vector<armarx::dsobstacleavoidance::Obstacle>
+std::vector<armarx::obstacledetection::Obstacle>
armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
{
+ ARMARX_DEBUG << "Get Obstacles";
std::shared_lock l{m_doa.mutex};
- std::vector<dsobstacleavoidance::Obstacle> obstacle_list;
+ 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);
- dsobstacleavoidance::Obstacle 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,12 +408,12 @@ 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.
std::vector<buffer::update> update_log;
- std::map<std::string, dsobstacleavoidance::Obstacle> obstacles;
+ std::map<std::string, obstacledetection::Obstacle> obstacles;
{
std::lock_guard l{m_buf.mutex};
@@ -461,7 +465,7 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
ARMARX_DEBUG << "Adding obstacle " << update.name << ".";
}
- const dsobstacleavoidance::Obstacle& obstacle = obstacles.at(update.name);
+ const obstacledetection::Obstacle& obstacle = obstacles.at(update.name);
m_doa.env.add_obstacle(::create_doa_obstacle(obstacle));
}
break;
@@ -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;
@@ -504,7 +508,7 @@ armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const
void
armarx::DSObstacleAvoidance::sanity_check_obstacle(
- const armarx::dsobstacleavoidance::Obstacle& obstacle)
+ const armarx::obstacledetection::Obstacle& obstacle)
const
{
ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`.";
@@ -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};
@@ -553,7 +558,7 @@ armarx::DSObstacleAvoidance::run_visualization()
color_m = Color::blue(255, 64);
}
- for (const dsobstacleavoidance::Obstacle& obstacle : getObstacles())
+ for (const obstacledetection::Obstacle& obstacle : getObstacles())
{
const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ;
const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ;
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
index eb48fee2314c69b4d70103b1b133dd54960a6305..2f9035f45eb97f749f17e083ae8a150abf061ada 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
@@ -50,7 +50,7 @@
// RobotAPI
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/interface/components/DSObstacleAvoidanceInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
namespace armarx
@@ -79,11 +79,11 @@ namespace armarx
void
updateObstacle(
- const dsobstacleavoidance::Obstacle& obstacle,
+ const obstacledetection::Obstacle& obstacle,
const Ice::Current& = Ice::emptyCurrent)
override;
- std::vector<dsobstacleavoidance::Obstacle>
+ std::vector<obstacledetection::Obstacle>
getObstacles(const Ice::Current& = Ice::emptyCurrent)
override;
@@ -147,7 +147,7 @@ namespace armarx
private:
void
- sanity_check_obstacle(const dsobstacleavoidance::Obstacle& obstacle)
+ sanity_check_obstacle(const obstacledetection::Obstacle& obstacle)
const;
void
@@ -205,7 +205,7 @@ namespace armarx
mutable std::mutex mutex;
std::vector<DSObstacleAvoidance::buffer::update> update_log;
- std::map<std::string, dsobstacleavoidance::Obstacle> obstacles;
+ std::map<std::string, obstacledetection::Obstacle> obstacles;
bool needs_env_update;
IceUtil::Time last_env_update;
};
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index eacd69f2afe16f464dc4486665e7c4e6674a7147..b59531d4c840f9d7bbfb773f2a5da361bedbcf39 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -56,7 +56,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <boost/algorithm/string/predicate.hpp>
-#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string/replace.hpp>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
@@ -66,13 +66,13 @@
using namespace VirtualRobot;
-#define SELECTION_NAME_PREFIX ("selection_" + boost::lexical_cast<std::string>(this))
+#define SELECTION_NAME_PREFIX ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
#define SELECTION_NAME_SPLITTER "____"
#define SELECTION_NAME(layerName, elementName) boost::replace_all_copy(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
namespace armarx
{
- boost::recursive_mutex DebugDrawerComponent::selectionMutex;
+ std::recursive_mutex DebugDrawerComponent::selectionMutex;
void selection_callback(void* userdata, SoPath* path)
{
@@ -124,7 +124,7 @@ namespace armarx
void DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_INFO << "Enabling selections for layer " << layerName;
selectableLayers.insert(layerName);
@@ -132,7 +132,7 @@ namespace armarx
void DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_INFO << "Disabling selections for layer " << layerName;
if (layerName == "")
@@ -147,8 +147,8 @@ namespace armarx
void DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+ auto l = getScopedVisuLock();
+ std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
@@ -174,8 +174,8 @@ namespace armarx
void DebugDrawerComponent::select(const std::string& layerName, const std::string& elementName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+ auto l = getScopedVisuLock();
+ std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
@@ -199,8 +199,8 @@ namespace armarx
void DebugDrawerComponent::deselect(const std::string& layerName, const std::string& elementName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+ auto l = getScopedVisuLock();
+ std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
@@ -224,34 +224,34 @@ namespace armarx
void DebugDrawerComponent::selectionCallback()
{
- ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*topicMutex));
+ std::unique_lock l(*topicMutex);
listenerPrx->reportSelectionChanged(getSelections());
}
void DebugDrawerComponent::deselectionCallback()
{
- ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*topicMutex));
+ std::unique_lock l(*topicMutex);
listenerPrx->reportSelectionChanged(getSelections());
}
void DebugDrawerComponent::installSelectionCallbacks()
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
selectionNode->addSelectionCallback(selection_callback, this);
selectionNode->addDeselectionCallback(deselection_callback, this);
}
void DebugDrawerComponent::removeSelectionCallbacks()
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
selectionNode->removeSelectionCallback(selection_callback, this);
selectionNode->removeDeselectionCallback(deselection_callback, this);
}
void DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
- ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+ auto l = getScopedVisuLock();
+ std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
selectionNode->deselectAll();
@@ -273,7 +273,7 @@ namespace armarx
DebugDrawerSelectionList DebugDrawerComponent::getSelections(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
DebugDrawerSelectionList selectedElements;
@@ -344,7 +344,7 @@ namespace armarx
DebugDrawerComponent::~DebugDrawerComponent()
{
/*{
- ScopedRecursiveLockPtr l = getScopedLock();
+ auto l = getScopedLock();
layerMainNode->unref();
coinVisu->unref();
}*/
@@ -387,7 +387,7 @@ namespace armarx
return;
}
- ScopedLock lock(drawer->timerSensorMutex);
+ std::unique_lock lock(drawer->timerSensorMutex);
drawer->updateVisualization();
}
@@ -412,7 +412,7 @@ namespace armarx
counter++; // sec counter
}
{
- ScopedRecursiveLockPtr l = getScopedLock();
+ auto l = getScopedLock();
coinVisu->removeAllChildren();
}*/
}
@@ -425,7 +425,7 @@ namespace armarx
//ARMARX_DEBUG << "onExitComponent";
if (timerSensor)
{
- ScopedLock lock(timerSensorMutex);
+ std::unique_lock lock(timerSensorMutex);
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
sensor_mgr->removeTimerSensor(timerSensor);
}
@@ -441,7 +441,7 @@ namespace armarx
}
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
coinVisu->removeAllChildren();
layerMainNode->unref();
selectionNode->unref();
@@ -451,7 +451,7 @@ namespace armarx
void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_INFO << "Exporting scene to '" << filename << "'";
@@ -472,7 +472,7 @@ namespace armarx
void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_INFO << "Exporting layer '" << layerName << "' to '" << filename << "'";
@@ -499,7 +499,7 @@ namespace armarx
void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawing coord system";
auto& layer = requestLayer(d.layerName);
@@ -531,7 +531,7 @@ namespace armarx
void DebugDrawerComponent::drawLine(const LineData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawLine1" << flush;
auto& layer = requestLayer(d.layerName);
@@ -560,7 +560,7 @@ namespace armarx
void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawLineSet";
auto& layer = requestLayer(d.layerName);
@@ -656,7 +656,7 @@ namespace armarx
void DebugDrawerComponent::drawBox(const BoxData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawBox";
auto& layer = requestLayer(d.layerName);
@@ -691,7 +691,7 @@ namespace armarx
void DebugDrawerComponent::drawText(const TextData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawText1";
auto& layer = requestLayer(d.layerName);
@@ -737,7 +737,7 @@ namespace armarx
void DebugDrawerComponent::drawSphere(const SphereData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawSphere";
auto& layer = requestLayer(d.layerName);
@@ -777,7 +777,7 @@ namespace armarx
void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawCylinder";
auto& layer = requestLayer(d.layerName);
@@ -838,7 +838,7 @@ namespace armarx
void DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -878,7 +878,7 @@ namespace armarx
d.color.b,
d.color.transparency,
16, 30);
- SoNode* circle = boost::dynamic_pointer_cast<CoinVisualizationNode>(node)->getCoinVisualization();
+ SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
circle->setName(SELECTION_NAME(d.layerName, d.name));
sep->addChild(circle);
@@ -888,7 +888,7 @@ namespace armarx
void DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -937,7 +937,7 @@ namespace armarx
void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -964,7 +964,7 @@ namespace armarx
void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -1018,7 +1018,7 @@ namespace armarx
void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -1043,7 +1043,7 @@ namespace armarx
void DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!d.active)
{
@@ -1076,7 +1076,7 @@ namespace armarx
void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -1231,7 +1231,7 @@ namespace armarx
VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
std::string entryName = boost::replace_all_copy(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
@@ -1315,7 +1315,7 @@ namespace armarx
nodeMat->setOverride(false);
nodeSep->addChild(nodeMat);
- boost::shared_ptr<CoinVisualization> robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
+ CoinVisualizationPtr robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
if (robNodeVisu)
{
@@ -1340,7 +1340,7 @@ namespace armarx
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1360,7 +1360,7 @@ namespace armarx
void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1380,7 +1380,7 @@ namespace armarx
void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1400,7 +1400,7 @@ namespace armarx
void DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1420,7 +1420,7 @@ namespace armarx
void DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1440,7 +1440,7 @@ namespace armarx
void DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1460,7 +1460,7 @@ namespace armarx
void DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1480,7 +1480,7 @@ namespace armarx
void DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1500,7 +1500,7 @@ namespace armarx
void DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1520,7 +1520,7 @@ namespace armarx
void DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1540,7 +1540,7 @@ namespace armarx
void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1560,7 +1560,7 @@ namespace armarx
void DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
// process active robots
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + name + "__"), " ", "_");
@@ -1614,7 +1614,7 @@ namespace armarx
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1634,7 +1634,7 @@ namespace armarx
void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -1662,7 +1662,7 @@ namespace armarx
void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (selectionNode->findChild(layerMainNode) >= 0)
{
@@ -1672,7 +1672,7 @@ namespace armarx
void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (selectionNode->findChild(layerMainNode) < 0)
{
@@ -1685,7 +1685,7 @@ namespace armarx
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.globalPose = gp;
@@ -1714,7 +1714,7 @@ namespace armarx
void DebugDrawerComponent::removePoseVisu(const std::string& layerName, const std::string& poseName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.layerName = layerName;
@@ -1734,7 +1734,7 @@ namespace armarx
Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.p1 = p1;
@@ -1755,7 +1755,7 @@ namespace armarx
void DebugDrawerComponent::removeLineVisu(const std::string& layerName, const std::string& lineName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.layerName = layerName;
@@ -1773,7 +1773,7 @@ namespace armarx
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.lineSet = lineSet;
@@ -1791,7 +1791,7 @@ namespace armarx
void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.layerName = layerName;
@@ -1810,7 +1810,7 @@ namespace armarx
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.width = dimensions->x;
@@ -1832,7 +1832,7 @@ namespace armarx
void DebugDrawerComponent::removeBoxVisu(const std::string& layerName, const std::string& boxName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.layerName = layerName;
@@ -1850,7 +1850,7 @@ namespace armarx
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.text = text;
@@ -1871,7 +1871,7 @@ namespace armarx
void DebugDrawerComponent::removeTextVisu(const std::string& layerName, const std::string& textName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.layerName = layerName;
@@ -1889,7 +1889,7 @@ namespace armarx
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.radius = radius;
@@ -1909,7 +1909,7 @@ namespace armarx
void DebugDrawerComponent::removeSphereVisu(const std::string& layerName, const std::string& sphereName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.layerName = layerName;
@@ -1927,7 +1927,7 @@ namespace armarx
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -1949,7 +1949,7 @@ namespace armarx
void DebugDrawerComponent::removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.layerName = layerName;
@@ -1967,7 +1967,7 @@ namespace armarx
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.pointCloud = pointCloud;
@@ -1985,7 +1985,7 @@ namespace armarx
void DebugDrawerComponent::removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.layerName = layerName;
@@ -2002,7 +2002,7 @@ namespace armarx
void DebugDrawerComponent::setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
@@ -2033,7 +2033,7 @@ namespace armarx
void DebugDrawerComponent::removePolygonVisu(const std::string& layerName, const std::string& polygonName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
d.layerName = layerName;
@@ -2050,7 +2050,7 @@ namespace armarx
void DebugDrawerComponent::setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.triMesh = triMesh;
@@ -2068,7 +2068,7 @@ namespace armarx
void DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.layerName = layerName;
@@ -2085,7 +2085,7 @@ namespace armarx
void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.position = Vector3Ptr::dynamicCast(position)->toEigen();
@@ -2107,7 +2107,7 @@ namespace armarx
void DebugDrawerComponent::removeArrowVisu(const std::string& layerName, const std::string& arrowName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.layerName = layerName;
@@ -2123,7 +2123,7 @@ namespace armarx
void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "setting robot visualization for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2139,7 +2139,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot pose for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2156,7 +2156,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot config for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2177,7 +2177,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2202,7 +2202,7 @@ namespace armarx
void DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const DrawColor& c, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2228,7 +2228,7 @@ namespace armarx
void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "removing robot visu for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2248,7 +2248,7 @@ namespace armarx
void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ auto lockData = getScopedAccumulatedDataLock();
//updates should only contain elements to add for each layer after the last clear for this layer was executed
//this prevents a sequence add,clear,add to result in an empty layer
//=>remove all objects pending for this layer
@@ -2259,8 +2259,8 @@ namespace armarx
void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+ auto lockData = getScopedAccumulatedDataLock();
+ auto lockVisu = getScopedVisuLock();
ARMARX_DEBUG << "clearing layer " << layerName;
@@ -2369,20 +2369,19 @@ namespace armarx
clearLayer(DEBUG_LAYER_NAME);
}
- void DebugDrawerComponent::setMutex(boost::shared_ptr<boost::recursive_mutex> m)
+ void DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
{
- //ARMARX_IMPORTANT << "set mutex3d:" << m.get();
mutex = m;
}
- ScopedRecursiveLockPtr DebugDrawerComponent::getScopedVisuLock()
+ auto DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
{
- ScopedRecursiveLockPtr l;
+ RecursiveMutexLockPtr l;
if (mutex)
{
//ARMARX_IMPORTANT << mutex.get();
- l.reset(new ScopedRecursiveLock(*mutex));
+ l.reset(new RecursiveMutexLock(*mutex));
}
else
{
@@ -2392,9 +2391,9 @@ namespace armarx
return l;
}
- ScopedRecursiveLockPtr DebugDrawerComponent::getScopedAccumulatedDataLock()
+ auto DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
{
- ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*dataUpdateMutex));
+ RecursiveMutexLockPtr l(new RecursiveMutexLock(*dataUpdateMutex));
return l;
}
@@ -2405,7 +2404,7 @@ namespace armarx
DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (hasLayer(layerName))
{
@@ -2428,8 +2427,8 @@ namespace armarx
void DebugDrawerComponent::updateVisualization()
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+ auto lockData = getScopedAccumulatedDataLock();
+ auto lockVisu = getScopedVisuLock();
// check for clear&remove
//(updates only contain elements to add for each layer after the last clear for this layer was executed)
//this prevents a sequence add,clear,add to result in an empty layer
@@ -2545,13 +2544,13 @@ namespace armarx
bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
return layers.find(layerName) != layers.end();
}
void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ auto lockData = getScopedAccumulatedDataLock();
//updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
//this prevents a sequence add,clear,add to result in an empty layer
//=>remove all objects pending for this layer
@@ -2561,8 +2560,8 @@ namespace armarx
void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
- ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
+ auto lockData = getScopedAccumulatedDataLock();
+ auto lockVisu = getScopedVisuLock();
if (!hasLayer(layerName))
{
if (verbose)
@@ -2603,7 +2602,7 @@ namespace armarx
void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
{
- ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
+ auto lockData = getScopedAccumulatedDataLock();
removeUpdateDataFromMap(layerName, accumulatedUpdateData.coord);
removeUpdateDataFromMap(layerName, accumulatedUpdateData.line);
@@ -2636,7 +2635,7 @@ namespace armarx
Ice::StringSeq DebugDrawerComponent::layerNames(const ::Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
Ice::StringSeq seq {};
for (const auto& layer : layers)
@@ -2650,7 +2649,7 @@ namespace armarx
::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
{
::armarx::LayerInformationSequence seq {};
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
for (const auto& layer : layers)
{
@@ -2679,7 +2678,7 @@ namespace armarx
void DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -2739,7 +2738,7 @@ namespace armarx
void DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -2760,7 +2759,7 @@ namespace armarx
void DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.pointCloud = pointCloud;
@@ -2778,7 +2777,7 @@ namespace armarx
void DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.layerName = layerName;
@@ -2794,7 +2793,7 @@ namespace armarx
void DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
@@ -2855,7 +2854,7 @@ namespace armarx
void DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName, const std::string& name)
{
- ScopedRecursiveLockPtr l = getScopedVisuLock();
+ auto l = getScopedVisuLock();
if (!hasLayer(layerName))
{
@@ -2876,7 +2875,7 @@ namespace armarx
void DebugDrawerComponent::set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.pointCloud = pointCloud;
@@ -2894,7 +2893,7 @@ namespace armarx
void DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
{
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.layerName = layerName;
@@ -2911,7 +2910,7 @@ namespace armarx
void DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
@@ -2934,7 +2933,7 @@ namespace armarx
void DebugDrawerComponent::removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current&)
{
- ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ auto l = getScopedAccumulatedDataLock();
std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.layerName = layerName;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index c91d4aca1747454bd6533595ef0bda8359bae6cc..9b7c3709bc92be7256deae774a11b748be5ee14a 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -37,7 +37,6 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -47,6 +46,7 @@
//std
#include <memory>
+#include <mutex>
namespace armarx
{
@@ -274,11 +274,17 @@ namespace armarx
void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = Ice::emptyCurrent) override;
+ // This is all total madness. Why do we put mutexes and locks into shared pointers?
+ using RecursiveMutex = std::recursive_mutex;
+ using RecursiveMutexPtr = std::shared_ptr<RecursiveMutex>;
+ using RecursiveMutexLock = std::unique_lock<RecursiveMutex>;
+ using RecursiveMutexLockPtr = std::shared_ptr<RecursiveMutexLock>;
+
/*!
* \brief getScopedLock If using the coin visualization it must be ensured that all rendering calls are protected with this mutex
* \return The lock that is automatically destructed when leaving the current scope.
*/
- ScopedRecursiveLockPtr getScopedVisuLock();
+ RecursiveMutexLockPtr getScopedVisuLock();
/*!
* \brief getVisualization Ensure that all access to this node is protected with the scoped lock mutex.
@@ -286,7 +292,7 @@ namespace armarx
*/
SoSeparator* getVisualization();
- void setMutex(boost::shared_ptr<boost::recursive_mutex> m);
+ void setMutex(RecursiveMutexPtr const& m);
~DebugDrawerComponent() override;
@@ -590,24 +596,24 @@ namespace armarx
std::map<const std::string, Layer> layers;
//! The coin mutex
- boost::shared_ptr<boost::recursive_mutex> mutex;
+ RecursiveMutexPtr mutex;
//! The data update mutex
- std::shared_ptr<boost::recursive_mutex> dataUpdateMutex;
+ RecursiveMutexPtr dataUpdateMutex;
- std::shared_ptr<boost::recursive_mutex> topicMutex;
+ RecursiveMutexPtr topicMutex;
- static boost::recursive_mutex selectionMutex;
+ static std::recursive_mutex selectionMutex;
std::set<std::string> selectableLayers;
- ScopedRecursiveLockPtr getScopedAccumulatedDataLock();
+ RecursiveMutexLockPtr getScopedAccumulatedDataLock();
DebugDrawerListenerPrx listenerPrx;
float cycleTimeMS;
//PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
SoTimerSensor* timerSensor;
- Mutex timerSensorMutex;
+ std::mutex timerSensorMutex;
void removeAccumulatedData(const std::string& layerName);
std::map<std::string, VirtualRobot::RobotPtr> loadedRobots;
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..46515a2c2f9a61acbaf63c186920478dab82cb1c
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -0,0 +1,441 @@
+/*
+ * 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;
+ break;
+ }
+ }
+ m_obstacle_detection->removeObstacle(name);
+ m_obstacle_detection->updateEnvironment();
+ }
+
+ void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
+ {
+ usleep(m_min_value_for_accepting);
+ }
+
+
+ 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()
+ {
+ ARMARX_DEBUG << "update 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
+ {
+ ARMARX_DEBUG << "update obstacle position";
+ 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;
+ {
+ 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);
+
+ 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)
+ {
+ ARMARX_DEBUG << "update environment";
+ 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..1629b7efe3102dccf85521ac0fac6379e09225cc
--- /dev/null
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -0,0 +1,124 @@
+/*
+ * 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;
+ void wait_unitl_obstacles_are_ready(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/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 57bd492d1e56a1e54513c4af3ac5431b1ff68e25..b7d21d33b49202aa16928be602d306ed522ad9a1 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -26,6 +26,8 @@
+
+
namespace armarx
{
void GamepadControlUnit::onInitComponent()
@@ -33,12 +35,14 @@ namespace armarx
ARMARX_INFO << "oninit GamepadControlUnit";
usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
-
+ usingProxy("EmergencyStopMaster");
scaleX = getProperty<float>("ScaleX").getValue();
scaleY = getProperty<float>("ScaleY").getValue();
scaleRotation = getProperty<float>("ScaleAngle").getValue();
ARMARX_INFO << "oninit GamepadControlUnit end";
+
+
}
@@ -46,6 +50,7 @@ namespace armarx
{
ARMARX_INFO << "onConnect GamepadControlUnit";
platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
+ emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster");
}
@@ -68,6 +73,10 @@ namespace armarx
void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
+ if (data.leftTrigger > 0)
+ {
+ emergencyStop->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ }
//scales are for the robot axis
if (data.rightTrigger > 0)
{
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index 2614a96fff87084ee3383bd705c05e6c2c6bbc5f..abdf77a77d42aa73a48bce6c93a15416bb8c64bc 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
@@ -105,6 +106,7 @@ namespace armarx
float scaleX;
float scaleY;
float scaleRotation;
+ EmergencyStopMasterInterfacePrx emergencyStop;
public:
void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data,
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 9397d1bb222a8b93d17425e2ac9135b904fb811e..fa3c189dcd4a46055b33ef4566ede2f73a405317 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -4,22 +4,28 @@ armarx_component_set_name("ObjectPoseObserver")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces
ArmarXGuiComponentPlugins
- RobotAPIComponentPlugins
+ RobotAPIArmarXObjects RobotAPIComponentPlugins
${PROJECT_NAME}Interfaces
)
set(SOURCES
ObjectPoseObserver.cpp
- ObjectPoseProviderPlugin.cpp
- ObjectFinder.cpp
+ plugins/ObjectPoseProviderPlugin.cpp
+ plugins/ObjectPoseClientPlugin.cpp
+
+ ice_conversions.cpp
+ ObjectPose.cpp
)
set(HEADERS
ObjectPoseObserver.h
- ObjectPoseProviderPlugin.h
- ObjectFinder.h
+ plugins/ObjectPoseProviderPlugin.h
+ plugins/ObjectPoseClientPlugin.h
+
+ ice_conversions.h
+ ObjectPose.h
)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
deleted file mode 100644
index af1dadc6edb222a64c836ba88d5648db6db0dc2e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ /dev/null
@@ -1,238 +0,0 @@
-#include "ObjectFinder.h"
-
-#include <SimoxUtility/filesystem/list_directory.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
-
-
-namespace armarx
-{
- namespace fs = std::filesystem;
-
-
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
- {
- Logging::setTag("ObjectFinder");
- }
-
- void ObjectFinder::init()
- {
- if (packageDataDir.empty())
- {
- CMakePackageFinder packageFinder(packageName);
- packageDataDir = packageFinder.getDataDir();
- if (packageDataDir.empty())
- {
- ARMARX_WARNING << "Could not find package '" << packageName << "'.";
- throw LocalException() << "Could not find package '" << packageName << "'.";
- }
- else
- {
- ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
- }
- }
- }
-
-
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& project, const std::string& name)
- {
- init();
- if (!project.empty())
- {
- return ObjectInfo(packageName, packageDataDir, project, name);
- }
- // Search for object in projects.
- const auto& projects = getProjects();
- for (const path& project : projects)
- {
- if (fs::is_directory(_rootDirAbs() / project / name))
- {
- return ObjectInfo(packageName, packageDataDir, project, name);
- }
- }
-
- std::stringstream ss;
- ss << "Did not find object '" << name << "' in any of these projects:\n";
- for (const path& project : projects)
- {
- ss << "- " << project << "\n";
- }
- ss << "Objects root directory: " << _rootDirAbs();
- ARMARX_WARNING << ss.str();
-
- return std::nullopt;
- }
-
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID)
- {
- init();
- if (nameOrID.find("/") != nameOrID.npos)
- {
- const std::vector<std::string> split = armarx::split(nameOrID, "/", true);
- ARMARX_CHECK_EQUAL(split.size(), 2) << "Expected ID of format 'Project/Name', but got: '" << nameOrID
- << "' (too many '/').";
- return findObject(split[0], split[1]);
- }
- else
- {
- return findObject("", nameOrID);
- }
- }
-
- std::vector<std::string> ObjectFinder::getProjects()
- {
- init();
- const bool local = true;
- std::vector<path> paths = simox::fs::list_directory(_rootDirAbs(), local);
- return std::vector<std::string>(paths.begin(), paths.end());
- }
-
- std::vector<ObjectFinder::path> ObjectFinder::getProjectDirectories()
- {
- init();
- const bool local = false;
- return simox::fs::list_directory(_rootDirAbs(), local);
- }
-
- std::vector<ObjectInfo> ObjectFinder::findAllObjects()
- {
- init();
- const bool local = true;
- std::vector<ObjectInfo> objects;
- for (const path& projectDir : simox::fs::list_directory(_rootDirAbs(), local))
- {
- if (fs::is_directory(_rootDirAbs() / projectDir))
- {
- std::vector<ObjectInfo> project = findAllObjectsOfProject(projectDir);
- objects.insert(objects.end(), project.begin(), project.end());
- }
- }
- return objects;
- }
-
- std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfProject(const std::string& project)
- {
- init();
- path projectDir = _rootDirAbs() / project;
- if (!fs::is_directory(projectDir))
- {
- ARMARX_WARNING << "Expected project directory for project '" << project << "': \n"
- << projectDir;
- return {};
- }
-
- std::vector<ObjectInfo> objects;
- const bool local = true;
- for (const path& dir : simox::fs::list_directory(projectDir, local))
- {
- if (fs::is_directory(projectDir / dir))
- {
- ObjectInfo object(packageName, packageDataDir, project, dir.filename());
- if (object.checkPaths())
- {
- objects.push_back(object);
- }
- }
- }
- return objects;
- }
-
- ObjectFinder::path ObjectFinder::_rootDirAbs() const
- {
- return packageDataDir / packageName;
- }
-
- ObjectFinder::path ObjectFinder::_rootDirRel() const
- {
- return packageName;
- }
-
-
- ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const std::string& project, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _project(project), _name(name)
- {
- }
-
- std::string ObjectInfo::package() const
- {
- return _packageName;
- }
-
- std::string ObjectInfo::project() const
- {
- return _project;
- }
-
- std::string ObjectInfo::name() const
- {
- return _name;
- }
-
- std::string ObjectInfo::id() const
- {
- return _project + "/" + _name;
- }
-
- ObjectInfo::path ObjectInfo::objectDirectory() const
- {
- return path(_packageName) / _project / _name;
- }
-
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
- {
- std::string extension = _extension;
- if (extension.at(0) != '.')
- {
- extension = "." + extension;
- }
- std::string filename = _name + suffix + extension;
-
- PackageFileLocation loc;
- loc.package = _packageName;
- loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _packageDataDir / loc.relativePath;
- return loc;
- }
-
- PackageFileLocation ObjectInfo::simoxXML() const
- {
- return file(".xml");
- }
-
- PackageFileLocation ObjectInfo::wavefrontObj() const
- {
- return file(".obj");
- }
-
- bool ObjectInfo::checkPaths() const
- {
- namespace fs = std::filesystem;
- bool result = true;
-
- if (!fs::is_regular_file(simoxXML().absolutePath))
- {
- ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
- result = false;
- }
- if (!fs::is_regular_file(wavefrontObj().absolutePath))
- {
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
- result = false;
- }
-
- return result;
- }
-
-}
-
-namespace armarx
-{
- std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
- {
- return os << "'" << rhs.id() << "'";
- }
-}
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd24a1ecc8cc9cf390684902112e5ac44a49c624
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -0,0 +1,160 @@
+#include "ObjectPose.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+namespace armarx::objpose
+{
+
+ Eigen::Matrix4f toEigen(const PoseBasePtr pose)
+ {
+ auto cast = PosePtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+
+ ObjectPose::ObjectPose()
+ {
+ }
+
+ ObjectPose::ObjectPose(const data::ObjectPose& ice)
+ {
+ fromIce(ice);
+ }
+
+ void ObjectPose::fromIce(const data::ObjectPose& ice)
+ {
+ providerName = ice.providerName;
+ objectType = ice.objectType;
+ objectID = ice.objectID;
+
+ objectPoseRobot = toEigen(ice.objectPoseRobot);
+ objectPoseGlobal = toEigen(ice.objectPoseGlobal);
+ objectPoseOriginal = toEigen(ice.objectPoseOriginal);
+ objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+
+ robotConfig = ice.robotConfig;
+ robotPose = toEigen(ice.robotPose);
+
+ confidence = ice.confidence;
+ timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+
+ localOOBB = ice.localOOBB;
+ }
+
+ data::ObjectPose ObjectPose::toIce() const
+ {
+ data::ObjectPose ice;
+ toIce(ice);
+ return ice;
+ }
+
+ void ObjectPose::toIce(data::ObjectPose& ice) const
+ {
+ ice.providerName = providerName;
+ ice.objectType = objectType;
+ ice.objectID = objectID;
+
+ ice.objectPoseRobot = new Pose(objectPoseRobot);
+ ice.objectPoseGlobal = new Pose(objectPoseGlobal);
+ ice.objectPoseOriginal = new Pose(objectPoseOriginal);
+ ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+
+ ice.robotConfig = robotConfig;
+ ice.robotPose = new Pose(robotPose);
+
+ ice.confidence = confidence;
+ ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+
+ ice.localOOBB = localOOBB;
+ }
+
+ void ObjectPose::fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
+ {
+ providerName = provided.providerName;
+ objectType = provided.objectType;
+
+ objectID = provided.objectID;
+
+ objectPoseOriginal = toEigen(provided.objectPose);
+ objectPoseOriginalFrame = provided.objectPoseFrame;
+
+ armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
+ framed.changeFrame(robot, robot->getRootNode()->getName());
+ objectPoseRobot = framed.toEigen();
+ framed.changeToGlobal(robot);
+ objectPoseGlobal = framed.toEigen();
+
+ robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
+ robotPose = robot->getGlobalPose();
+
+ confidence = provided.confidence;
+ timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ localOOBB = provided.localOOBB;
+ }
+
+}
+
+namespace armarx
+{
+
+ void objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
+ {
+ pose.fromIce(ice);
+ }
+ objpose::ObjectPose objpose::fromIce(const data::ObjectPose& ice)
+ {
+ return ObjectPose(ice);
+ }
+
+ void objpose::fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
+ {
+ poses.clear();
+ poses.reserve(ice.size());
+ for (const auto& i : ice)
+ {
+ poses.emplace_back(i);
+ }
+ }
+ objpose::ObjectPoseSeq objpose::fromIce(const data::ObjectPoseSeq& ice)
+ {
+ ObjectPoseSeq poses;
+ fromIce(ice, poses);
+ return poses;
+ }
+
+
+ void objpose::toIce(data::ObjectPose& ice, const ObjectPose& pose)
+ {
+ pose.toIce(ice);
+ }
+ objpose::data::ObjectPose objpose::toIce(const ObjectPose& pose)
+ {
+ return pose.toIce();
+ }
+
+ void objpose::toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses)
+ {
+ ice.clear();
+ ice.reserve(poses.size());
+ for (const auto& p : poses)
+ {
+ ice.emplace_back(p.toIce());
+ }
+ }
+ objpose::data::ObjectPoseSeq objpose::toIce(const ObjectPoseSeq& poses)
+ {
+ data::ObjectPoseSeq ice;
+ toIce(ice, poses);
+ return ice;
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a237e319daaae4c6e43ff0d6cd933990d2ae2f2
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <IceUtil/Time.h>
+
+#include <RobotAPI/interface/objectpose/types.h>
+
+
+namespace armarx::objpose
+{
+
+ /**
+ * @brief An object pose as stored by the ObjectPoseObserver.
+ */
+ struct ObjectPose
+ {
+ ObjectPose();
+ ObjectPose(const data::ObjectPose& ice);
+
+ void fromIce(const data::ObjectPose& ice);
+
+ data::ObjectPose toIce() const;
+ void toIce(data::ObjectPose& ice) const;
+
+ void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
+
+
+ /// Name of the providing component.
+ std::string providerName;
+ /// Known or unknown object.
+ ObjectTypeEnum objectType = AnyObject;
+
+ /// The object ID, i.e. dataset and name.
+ ObjectID objectID;
+
+ Eigen::Matrix4f objectPoseRobot;
+ Eigen::Matrix4f objectPoseGlobal;
+ Eigen::Matrix4f objectPoseOriginal;
+ std::string objectPoseOriginalFrame;
+
+ std::map<std::string, float> robotConfig;
+ Eigen::Matrix4f robotPose;
+
+ /// Confidence in [0, 1] (1 = full, 0 = none).
+ float confidence = 0;
+ /// Source timestamp.
+ IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
+
+ /// Object bounding box in object's local coordinate frame.
+ Box localOOBB;
+ };
+ using ObjectPoseSeq = std::vector<ObjectPose>;
+
+
+ void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
+ ObjectPose fromIce(const data::ObjectPose& ice);
+
+ void fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses);
+ ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice);
+
+
+ void toIce(data::ObjectPose& ice, const ObjectPose& pose);
+ data::ObjectPose toIce(const ObjectPose& pose);
+
+ void toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses);
+ data::ObjectPoseSeq toIce(const ObjectPoseSeq& poses);
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index dd60b2c6523726e1233478eef2cd83cf45b97d4b..c9e12b8e92a20aed2b2c9a1d60b5b75d3dd7f2a7 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -22,32 +22,21 @@
#include "ObjectPoseObserver.h"
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/meta/EnumNames.hpp>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
-std::ostream& armarx::objpose::operator<<(std::ostream& os, const ObjectID& id)
-{
- return os << "'" << id.project << "/" << id.name << "'";
-}
-
+#include "ice_conversions.h"
namespace armarx
{
- const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames =
- {
- { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
- { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
- { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
- };
-
-
ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
armarx::ObserverPropertyDefinitions(prefix)
{
@@ -62,6 +51,8 @@ namespace armarx
defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+ defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated");
+ defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated");
return defs;
}
@@ -126,44 +117,30 @@ namespace armarx
{
offerChannel(providerName, "Channel of provider '" + providerName + "'.");
}
- offerOrUpdateDataField(providerName, "objectType", ObjectTypeEnumNames.to_name(info.objectType), "");
+ offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
}
void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
objpose::ObjectPoseSeq objectPoses;
{
std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone("robot");
+ synchronizeLocalClone(robot);
+
+ if (robot->hasRobotNode(calibration.robotNode))
+ {
+ VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+ float value = robotNode->getJointValue();
+ robotNode->setJointValue(value + calibration.offset);
+ }
for (const auto& provided : providedPoses)
{
objpose::ObjectPose& pose = objectPoses.emplace_back();
- pose.objectType = provided.objectType;
- pose.objectID = provided.objectID;
-
- pose.objectPoseOriginal = provided.objectPose;
- pose.objectPoseOriginalFrame = provided.objectPoseFrame;
-
- armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose);
- ARMARX_CHECK_NOT_NULL(p);
- {
- armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName());
- framed.changeFrame(robot, robot->getRootNode()->getName());
- pose.objectPoseRobot = new Pose(framed.toEigen());
- framed.changeToGlobal(robot);
- pose.objectPoseGlobal = new Pose(framed.toEigen());
-
- pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
- pose.robotPose = new Pose(robot->getGlobalPose());
- }
-
- pose.confidence = provided.confidence;
- pose.timestampMicroSeconds = provided.timestampMicroSeconds;
- pose.providerName = provided.providerName;
+ pose.fromProvidedPose(provided, robot);
}
}
@@ -175,19 +152,24 @@ namespace armarx
}
- objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
+ objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
- objpose::ObjectPoseSeq result;
+ std::scoped_lock lock(dataMutex);
+ objpose::data::ObjectPoseSeq result;
for (const auto& [name, poses] : objectPoses)
{
- result.insert(result.end(), poses.begin(), poses.end());
+ for (const auto& pose : poses)
+ {
+ result.push_back(pose.toIce());
+ }
}
return result;
}
- objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
- return objectPoses.at(providerName);
+ std::scoped_lock lock(dataMutex);
+ return objpose::toIce(objectPoses.at(providerName));
}
@@ -201,7 +183,7 @@ namespace armarx
{
for (const auto& [name, info] : providers)
{
- // ToDo: optimize look.
+ // ToDo: optimize look up.
if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
{
requests[name].push_back(objectID);
@@ -224,17 +206,14 @@ namespace armarx
objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return providers;
}
Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
{
- Ice::StringSeq names;
- for (const auto& [name, _] : providers)
- {
- names.push_back(name);
- }
- return names;
+ std::scoped_lock lock(dataMutex);
+ return simox::get_keys(providers);
}
objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
@@ -258,19 +237,20 @@ namespace armarx
bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return providers.count(providerName) > 0;
}
-
Ice::Int ObjectPoseObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
{
- return {};
+ std::scoped_lock lock(dataMutex);
+ return updateCounters.at(providerName);
}
StringIntDictionary ObjectPoseObserver::getAllUpdateCounters(const Ice::Current&)
{
- return {};
+ return updateCounters;
}
@@ -318,20 +298,20 @@ namespace armarx
for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
{
const objpose::ObjectID id = objectPose.objectID;
- std::string key = id.project + "/" + id.name;
+ std::string key = id.dataset + "/" + id.name;
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.project, id.name);
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.dataset, id.name);
if (!objectInfo)
{
ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
continue;
}
- PoseBasePtr pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
- viz::Object object = viz::Object(id.project + "/" + id.name)
+ viz::Object object = viz::Object(id.dataset + "/" + id.name)
.file(objectInfo->package(), objectInfo->simoxXML().relativePath)
- .pose(armarx::PosePtr::dynamicCast(pose)->toEigen());
+ .pose(pose);
if (visu.alpha < 1)
{
object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index a50d3244b104722b417e283a0d90a9dfebf8f7b4..2d6452899537ad83df272130162802062f544700 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -31,17 +31,13 @@
#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include "ObjectFinder.h"
+#include "ObjectPose.h"
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-namespace armarx::objpose
-{
- std::ostream& operator<<(std::ostream& os, const ObjectID& id);
-}
-
namespace armarx
{
@@ -87,13 +83,13 @@ namespace armarx
// ObjectPoseTopic interface
public:
void reportProviderAvailable(const std::string& providerName, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+ void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
// ObjectPoseObserverInterface interface
public:
- objpose::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+ objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, ICE_CURRENT_ARG) override;
@@ -136,6 +132,7 @@ namespace armarx
VirtualRobot::RobotPtr robot;
std::mutex dataMutex;
+
objpose::ProviderInfoMap providers;
std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
@@ -153,6 +150,13 @@ namespace armarx
};
Visu visu;
+ struct Calibration
+ {
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+ };
+ Calibration calibration;
+
struct RemoteGuiTab : RemoteGui::Client::Tab
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecf05de2b31c102245e4fccfbf9663d43ab19f9b
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
@@ -0,0 +1,12 @@
+#include "ProvidedObjectPose.h"
+
+
+namespace armarx::objpose
+{
+ ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
+ {
+
+ }
+}
+
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..afc47fe515c61511c1ab225df37b2c96a48eaa2f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <RobotAPI/interface/objectpose/types.h>
+
+
+namespace armarx::objpose
+{
+
+ struct ProvidedObjectPose
+ {
+ ProvidedObjectPose();
+ ProvidedObjectPose(const data::ProvidedObjectPose& ice);
+
+
+ /// Name of the providing component.
+ std::string providerName;
+ /// Known or unknown object.
+ ObjectTypeEnum objectType = AnyObject;
+
+ /// The object ID, i.e. dataset and name.
+ ObjectID objectID;
+
+ /// Pose in `objectPoseFrame`.
+ Eigen::Matrix4f objectPose;
+ std::string objectPoseFrame;
+
+ /// Confidence in [0, 1] (1 = full, 0 = none).
+ float confidence = 0;
+ /// Source timestamp.
+ long timestampMicroSeconds = -1;
+
+ /// Object bounding box in object's local coordinate frame.
+ Box localOOBB;
+ };
+
+
+
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0085e76c8cca45298aad7c3cf11617c08c3e4ed8
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -0,0 +1,51 @@
+#include "ice_conversions.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+namespace armarx
+{
+
+ std::ostream& objpose::operator<<(std::ostream& os, const ObjectID& id)
+ {
+ return os << "'" << id.dataset << "/" << id.name << "'";
+ }
+
+ const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
+ {
+ { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
+ { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
+ { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
+ };
+
+
+ objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
+ {
+ objpose::AABB ice;
+ ice.center = new Vector3(aabb.center());
+ ice.extents = new Vector3(aabb.extents());
+ return ice;
+ }
+
+ objpose::Box objpose::toIce(const simox::OrientedBox<float>& oobb)
+ {
+ objpose::Box ice;
+ ice.position = new Vector3(oobb.center());
+ ice.orientation = new Quaternion(oobb.rotation().eval());
+ ice.extents = new Vector3(oobb.dimensions());
+ return ice;
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d71b34cae0b1e09f19ea32e37e5c5564aa70e2ea
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+#include <RobotAPI/interface/objectpose/types.h>
+
+#include "ObjectPose.h"
+
+
+namespace simox
+{
+ // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+ struct AxisAlignedBoundingBox;
+ // #include <SimoxUtility/shapes/OrientedBox.h>
+ template<class FloatT> class OrientedBox;
+}
+
+
+namespace armarx::objpose
+{
+
+ std::ostream& operator<<(std::ostream& os, const ObjectID& id);
+
+ extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
+
+
+ objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb);
+ objpose::Box toIce(const simox::OrientedBox<float>& oobb);
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0b74b4c206e872eb15703fce38b4199a99ca559
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
@@ -0,0 +1,59 @@
+#include "ObjectPoseClientPlugin.h"
+
+
+namespace armarx::plugins
+{
+
+ void ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(PROPERTY_NAME),
+ "ObjectPoseObserver",
+ "Name of the object pose observer.");
+ }
+ }
+
+ void ObjectPoseClientPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(makePropertyName(PROPERTY_NAME));
+ }
+
+ void ObjectPoseClientPlugin::preOnConnectComponent()
+ {
+ parent<ObjectPoseClientPluginUser>().objectPoseObserver = createObjectPoseObserver();
+ }
+
+ objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPlugin::createObjectPoseObserver()
+ {
+ return parent<Component>().getProxyFromProperty<objpose::ObjectPoseObserverInterfacePrx>(makePropertyName(PROPERTY_NAME));
+ }
+
+}
+
+
+namespace armarx
+{
+
+ ObjectPoseClientPluginUser::ObjectPoseClientPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPluginUser::createObjectPoseObserver()
+ {
+ return plugin->createObjectPoseObserver();
+ }
+
+ objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
+ {
+ if (!objectPoseObserver)
+ {
+ ARMARX_WARNING << "No object pose observer.";
+ return {};
+ }
+ return objpose::fromIce(objectPoseObserver->getObjectPoses());
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..667c22b4ed44a887c9dcd453836fcb67e50d4c9f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+
+#include "../ObjectPose.h"
+
+
+namespace armarx::plugins
+{
+
+ class ObjectPoseClientPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+
+
+ private:
+
+ static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+
+ };
+
+}
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseClientPluginUser :
+ virtual public ManagedIceObject
+ {
+ public:
+
+ /// Allow usage like: ObjectPose::getObjects()
+ using ObjectPose = ObjectPoseClientPluginUser;
+
+ ObjectPoseClientPluginUser();
+
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+ objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+
+
+ objpose::ObjectPoseSeq getObjectPoses();
+
+
+ private:
+
+ armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.cpp
rename to source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
similarity index 95%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
rename to source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
index 698c7d37592c76b9eb9f1b2aa52ea9b4cec65499..f15a5b813e130bec95ffaa6ce4339b52be87d6ec 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
@@ -39,7 +39,7 @@ namespace armarx
*/
class ObjectPoseProviderPluginUser :
virtual public ManagedIceObject
- , virtual objpose::ObjectPoseProvider
+ , virtual public objpose::ObjectPoseProvider
{
public:
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 1365ca76a761f5956aaebc5d96ef1adcca8034f5..934b88073535870c139d3677e45d8dcdc19bbf57 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -187,7 +187,7 @@ namespace armarx
return entries.at(i);
}
}
- ScopedLock lock(mutex);
+ std::unique_lock lock(mutex);
ARMARX_INFO << "registering '" << name << "'";
entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
validEntries++;
@@ -219,7 +219,7 @@ namespace armarx
e.isRunning = true;
e.enabled = true;
{
- ScopedLock lock(e.messageMutex);
+ std::unique_lock lock(e.messageMutex);
e.message = args.message;
}
}
@@ -285,7 +285,7 @@ namespace armarx
}
ARMARX_TRACE;
- ScopedLock lock(e.messageMutex);
+ std::unique_lock lock(e.messageMutex);
if (e.message.size())
{
ss << " - " << e.message;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 97852dab631cc1e6e1df0c4f738585f43395b0f4..49ca51167f87d7c83636c7d7d8f744797dd2d5e2 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -31,10 +31,10 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
#include <atomic>
+#include <mutex>
namespace armarx
{
@@ -111,7 +111,7 @@ namespace armarx
bool isRunning = false;
bool required = false;
bool enabled = true;
- Mutex messageMutex;
+ std::mutex messageMutex;
std::vector<long> history;
size_t historyIndex = 0;
};
@@ -145,7 +145,7 @@ namespace armarx
void monitorHealthTaskClb();
Entry& findOrCreateEntry(const std::string& name);
- Mutex mutex;
+ std::mutex mutex;
std::deque<Entry> entries;
std::atomic_size_t validEntries {0};
PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 52bada6ff48ffdde36652b751b314eb777de2385..473ec9a16d21f415925efedbfa8c5150ae2f55ea 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -23,8 +23,8 @@
*/
#include "RobotStateComponent.h"
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
+
+#include <Ice/ObjectAdapter.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
@@ -321,7 +321,7 @@ namespace armarx
}
{
- boost::unique_lock<SharedMutex> lock(jointHistoryMutex);
+ std::unique_lock lock(jointHistoryMutex);
jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
if (jointHistory.size() > jointHistoryLength)
@@ -510,7 +510,7 @@ namespace armarx
IceUtil::Time duration;
{
IceUtil::Time start = IceUtil::Time::now();
- boost::unique_lock<SharedMutex> lock(poseHistoryMutex);
+ std::unique_lock lock(poseHistoryMutex);
duration = IceUtil::Time::now() - start;
poseHistory.emplace_hint(poseHistory.end(),
@@ -551,7 +551,7 @@ namespace armarx
auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
{
- boost::shared_lock<SharedMutex> lock(jointHistoryMutex);
+ std::shared_lock lock(jointHistoryMutex);
if (jointHistory.empty())
{
ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
@@ -626,7 +626,7 @@ namespace armarx
auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
{
- boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
+ std::shared_lock lock(poseHistoryMutex);
if (poseHistory.empty())
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 27751d203b73c11313caa6a87bbaeabe3cf675ff..2a1da853b696150112fc5ee982cf3a7734c9a4d2 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -37,6 +37,9 @@
#include "SharedRobotServants.h"
+#include <shared_mutex>
+#include <mutex>
+
namespace armarx
{
@@ -216,11 +219,11 @@ namespace armarx
std::string robotFile;
std::string relativeRobotFile;
- mutable SharedMutex jointHistoryMutex;
+ mutable std::shared_mutex jointHistoryMutex;
std::map<IceUtil::Time, NameValueMap> jointHistory;
size_t jointHistoryLength;
- mutable SharedMutex poseHistoryMutex;
+ mutable std::shared_mutex poseHistoryMutex;
std::map<IceUtil::Time, FramedPosePtr> poseHistory;
size_t poseHistoryLength;
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index e49aac6a5abb3f596201cdc23f823805eba10654..cfc854b4c19309b39c56591fda88ca4b54300086 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -25,8 +25,6 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <boost/foreach.hpp>
-
#include <Eigen/Geometry>
#include <VirtualRobot/Nodes/RobotNode.h>
@@ -68,7 +66,7 @@ namespace armarx
void SharedObjectBase::ref(const Current& current)
{
- boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+ std::unique_lock lock(this->_counterMutex);
_referenceCount++;
@@ -79,7 +77,7 @@ namespace armarx
void SharedObjectBase::unref(const Current& current)
{
- boost::lock_guard<boost::mutex> lock(this->_counterMutex);
+ std::unique_lock lock(this->_counterMutex);
#ifdef VERBOSE
ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
@@ -184,7 +182,7 @@ namespace armarx
Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
- boost::shared_ptr<RobotNodePrismatic> prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(_node);
+ RobotNodePrismatic* prismatic = dynamic_cast<RobotNodePrismatic*>(_node.get());
if (prismatic)
{
@@ -199,7 +197,7 @@ namespace armarx
Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
- boost::shared_ptr<RobotNodeRevolute> revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(_node);
+ RobotNodeRevolute* revolute = dynamic_cast<RobotNodeRevolute*>(_node.get());
if (revolute)
{
@@ -237,7 +235,7 @@ namespace armarx
ReadLockPtr lock = _node->getRobot()->getReadLock();
std::vector<SceneObjectPtr> children = _node->getChildren();
NameList names;
- BOOST_FOREACH(SceneObjectPtr node, children)
+ for (SceneObjectPtr const& node : children)
{
names.push_back(node->getName());
}
@@ -249,7 +247,7 @@ namespace armarx
ReadLockPtr lock = _node->getRobot()->getReadLock();
std::vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
NameList names;
- BOOST_FOREACH(RobotNodePtr node, parents)
+ for (RobotNodePtr const& node : parents)
{
names.push_back(node->getName());
}
@@ -320,7 +318,7 @@ namespace armarx
#ifdef VERBOSE
ARMARX_WARNING_S << "destruct " << this << flush;
#endif
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::unique_lock cloneLock(m);
for (auto value : this->_cachedNodes)
{
@@ -341,7 +339,7 @@ namespace armarx
{
// ARMARX_LOG_S << "Looking for node: " << name << flush;
assert(_robot);
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::unique_lock cloneLock(m);
SharedRobotNodeInterfacePrx prx;
if (this->_cachedNodes.find(name) == this->_cachedNodes.end())
@@ -369,7 +367,7 @@ namespace armarx
SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current& current)
{
assert(_robot);
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::unique_lock cloneLock(m);
std::string name = _robot->getRootNode()/*,current*/->getName();
return this->getRobotNode(name, current);
}
@@ -384,7 +382,7 @@ namespace armarx
{
std::vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
NameList names;
- BOOST_FOREACH(RobotNodePtr node, robotNodes)
+ for (RobotNodePtr const& node : robotNodes)
{
names.push_back(node->getName());
}
@@ -402,7 +400,7 @@ namespace armarx
std::vector<RobotNodePtr> robotNodes = robotNodeSet->getAllRobotNodes();
NameList names;
- BOOST_FOREACH(RobotNodePtr node, robotNodes)
+ for (RobotNodePtr const& node : robotNodes)
{
names.push_back(node->getName());
}
@@ -421,7 +419,7 @@ namespace armarx
{
std::vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
NameList names;
- BOOST_FOREACH(RobotNodeSetPtr set, robotNodeSets)
+ for (RobotNodeSetPtr const& set : robotNodeSets)
{
names.push_back(set->getName());
}
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 922588ddbdbf15100fa4a3e358a5abd232de3546..5dff6c3891075d67a6a3e0ff9fa1f3a9a5d615f3 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -26,19 +26,11 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
-
#include <RobotAPI/interface/core/RobotState.h>
-#include <boost/thread.hpp>
+#include <VirtualRobot/VirtualRobot.h>
-namespace VirtualRobot
-{
- class Robot;
- using RobotPtr = boost::shared_ptr<Robot>;
- class RobotNode;
- using RobotNodePtr = boost::shared_ptr<RobotNode>;
-}
+#include <mutex>
namespace armarx
{
@@ -59,7 +51,7 @@ namespace armarx
SharedObjectBase();
private:
unsigned int _referenceCount;
- boost::mutex _counterMutex;
+ std::mutex _counterMutex;
};
/**
@@ -149,7 +141,7 @@ namespace armarx
RobotStateComponentInterfacePrx getRobotStateComponent(const Ice::Current&) const override;
protected:
VirtualRobot::RobotPtr _robot;
- boost::recursive_mutex m;
+ std::recursive_mutex m;
std::map<std::string, SharedRobotNodeInterfacePrx> _cachedNodes;
IceUtil::Time updateTimestamp;
RobotStateComponentInterfacePrx robotStateComponent;
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index 3cb860a4dbc16e48e0c5d60ba0b6985f0d99990c..a44d697794ceaf47d46266b6e6b515dec9acf84d 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -31,6 +31,8 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
+#include <thread>
+
namespace armarx
{
void ViewSelection::onInitComponent()
@@ -142,7 +144,7 @@ namespace armarx
void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
{
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
for (const auto& p : saliencyMaps)
@@ -180,7 +182,7 @@ namespace armarx
int maxIndex = -1;
float maxSaliency = std::numeric_limits<float>::min();
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
hasNewSaliencyMap = false;
@@ -284,7 +286,7 @@ namespace armarx
if (!manualViewTargets.empty())
{
- ScopedLock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
ViewTargetBasePtr manualViewTarget = manualViewTargets.top();
@@ -332,11 +334,11 @@ namespace armarx
viewSelectionObserver->nextViewTarget(timeOfLastViewChange.toMilliSeconds() + duration);
- boost::this_thread::sleep(boost::posix_time::milliseconds(duration));
+ std::this_thread::sleep_for(std::chrono::milliseconds(duration));
}
else
{
- boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+ std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
}
@@ -344,7 +346,7 @@ namespace armarx
void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
//SharedRobotInterfacePrx sharedRobotInterface = robotStateComponent->getRobotSnapshot();
@@ -356,14 +358,14 @@ namespace armarx
manualViewTargets.push(viewTarget);
- boost::mutex::scoped_lock lock2(syncMutex);
+ std::unique_lock lock2(syncMutex);
condition.notify_all();
}
void ViewSelection::clearManualViewTargets(const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
while (!manualViewTargets.empty())
{
@@ -376,7 +378,7 @@ namespace armarx
ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
ViewTargetList result;
@@ -396,7 +398,7 @@ namespace armarx
void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
{
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
hasNewSaliencyMap = true;
@@ -439,7 +441,7 @@ namespace armarx
}
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
DebugDrawer24BitColoredPointCloud cloud;
@@ -492,7 +494,7 @@ namespace armarx
void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
if (saliencyMaps.count(name))
{
@@ -506,7 +508,7 @@ namespace armarx
{
std::vector<std::string> names;
- boost::mutex::scoped_lock lock(syncMutex);
+ std::unique_lock lock(syncMutex);
for (const auto& p : saliencyMaps)
{
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index f8e6cbdc93a26500fef455bc989b2149bd1554b4..98beeedd5e6d9a176d9b76e83b4b9659d99ef8d5 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -46,6 +46,8 @@
#include <Eigen/Geometry>
+#include <condition_variable>
+#include <mutex>
#include <queue>
namespace armarx
@@ -151,7 +153,7 @@ namespace armarx
void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
ARMARX_INFO << "activating automatic view selection";
@@ -161,7 +163,7 @@ namespace armarx
void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
ARMARX_INFO << "deactivating automatic view selection";
@@ -171,7 +173,7 @@ namespace armarx
bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
- boost::mutex::scoped_lock lock(manualViewTargetsMutex);
+ std::unique_lock lock(manualViewTargetsMutex);
return doAutomaticViewSelection;
}
@@ -214,16 +216,16 @@ namespace armarx
bool drawViewDirection;
- armarx::Mutex manualViewTargetsMutex;
+ std::mutex manualViewTargetsMutex;
std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
bool doAutomaticViewSelection;
Eigen::Vector3f offsetToHeadCenter;
- boost::condition_variable condition;
+ std::condition_variable condition;
bool hasNewSaliencyMap;
- boost::mutex syncMutex;
+ std::mutex syncMutex;
std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index a365530d8022e8ac1f7e51f7246c44f28903c5ef..a2f4135e4f835166d911f5fc373919cc1885ffe8 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -36,6 +36,8 @@
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <boost/algorithm/string/trim.hpp>
+
#define RAWFORCE "_rawforcevectors"
#define OFFSETFORCE "_forceswithoffsetvectors"
#define FILTEREDFORCE "_filteredforcesvectors"
@@ -123,7 +125,7 @@ namespace armarx
auto channels = getAvailableChannels(false);
auto batchPrx = debugDrawer->ice_batchOneway();
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
std::set<std::string> frameAlreadyReported;
for (auto& channel : channels)
@@ -242,7 +244,7 @@ namespace armarx
void ForceTorqueObserver::updateRobot()
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
RemoteRobot::synchronizeLocalClone(localRobot, robot);
}
@@ -304,7 +306,7 @@ namespace armarx
void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr & forces, const FramedDirectionBasePtr & torques)
{
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 24c031200b51613c9b3409129df79b2804d7a48b..0f17d8a498377664b98882e26dff3c9b8df1bb50 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -27,6 +27,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <mutex>
+
namespace armarx
{
/**
@@ -92,7 +94,7 @@ namespace armarx
private:
void updateRobot();
- armarx::Mutex dataMutex;
+ std::mutex dataMutex;
std::string topicName;
RobotStateComponentInterfacePrx robot;
VirtualRobot::RobotPtr localRobot;
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.cpp b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
index e6e5226324c46df8d35f6307660b9e016b71fd02..b1848c5325d931a623c828e1dd58973121beedb5 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.cpp
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
@@ -69,7 +69,7 @@ namespace armarx
void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.h b/source/RobotAPI/components/units/GamepadUnitObserver.h
index fcccf65fc017cda9f3cc69e89a2eebb747df61cc..2f4d59c2a7a44ede66adaec1663be2263b6d25de 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.h
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -79,7 +80,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 3a0cac2ec7835df8e0362b59b70d9d48ef36708e..527216c96a63d769c625cc5deeae30fab64fef47 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -122,21 +122,21 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
this->candidates[providerName] = candidates;
handleProviderUpdate(providerName, candidates.size());
}
void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
this->bimanualCandidates[providerName] = candidates;
handleProviderUpdate(providerName, candidates.size());
}
void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
providers[providerName] = info;
if (updateCounters.count(providerName) == 0)
{
@@ -153,13 +153,13 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
return providers;
}
StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
return getAvailableProviderNames();
}
@@ -167,14 +167,14 @@ StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return providers[providerName];
}
bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
return hasProvider(providerName);
}
@@ -182,7 +182,7 @@ bool GraspCandidateObserver::hasProvider(const std::string& providerName, const
GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
{
@@ -200,7 +200,7 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::str
GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
GraspCandidateSeq matching;
for (const std::pair<std::string, grasping::GraspCandidateSeq>& pair : candidates)
{
@@ -217,20 +217,20 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return updateCounters[providerName];
}
IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
return updateCounters;
}
bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (providers.count(providerName) == 0)
{
return false;
@@ -241,19 +241,19 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
{
- ScopedLock lock(selectedCandidatesMutex);
+ std::unique_lock lock(selectedCandidatesMutex);
selectedCandidates = candidates;
}
GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
{
- ScopedLock lock(selectedCandidatesMutex);
+ std::unique_lock lock(selectedCandidatesMutex);
return selectedCandidates;
}
BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
BimanualGraspCandidateSeq all;
for (const std::pair<std::string, grasping::BimanualGraspCandidateSeq>& pair : bimanualCandidates)
{
@@ -264,13 +264,13 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
{
- ScopedLock lock(selectedCandidatesMutex);
+ std::unique_lock lock(selectedCandidatesMutex);
selectedBimanualCandidates = candidates;
}
BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
{
- ScopedLock lock(selectedCandidatesMutex);
+ std::unique_lock lock(selectedCandidatesMutex);
return selectedBimanualCandidates;
}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index a481ec2791d6c97a5c882361109d96d494e44e9e..beb95c3a2bde2c9bd3c568031dd4ce2db7939d07 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -26,6 +26,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+#include <mutex>
+
namespace armarx
{
/**
@@ -106,7 +108,7 @@ namespace armarx
bool hasProvider(const std::string& providerName);
void checkHasProvider(const std::string& providerName);
grasping::StringSeq getAvailableProviderNames();
- Mutex dataMutex;
+ std::mutex dataMutex;
std::map<std::string, grasping::GraspCandidateSeq> candidates;
std::map<std::string, grasping::BimanualGraspCandidateSeq> bimanualCandidates;
grasping::InfoMap providers;
@@ -114,7 +116,7 @@ namespace armarx
grasping::GraspCandidateProviderInterfacePrx configTopic;
- Mutex selectedCandidatesMutex;
+ std::mutex selectedCandidatesMutex;
grasping::GraspCandidateSeq selectedCandidates;
grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 6f7cc714ab242930d2a2a32d6022b77889a473bc..3816605266f1bc44dce40a752bfe94b14af8177b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -32,15 +32,9 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+#include <VirtualRobot/VirtualRobot.h>
-namespace VirtualRobot
-{
- class Robot;
- using RobotPtr = boost::shared_ptr<Robot>;
- class EndEffector;
- using EndEffectorPtr = boost::shared_ptr<EndEffector>;
-}
namespace armarx
{
/**
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 759927bd8799c8d319ba69fe1e3e384402ac8d7a..0fe52df490098255ef86b56bd43025017a2997b5 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -76,7 +76,7 @@ namespace armarx
void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(values);
if (matrix->cols == 0)
@@ -159,7 +159,7 @@ namespace armarx
void HapticObserver::updateStatistics()
{
- /*ScopedLock lock(dataMutex);
+ /*std::unique_lock lock(dataMutex);
//ARMARX_LOG << "updateStatistics";
long now = TimestampVariant::nowLong();
for (std::map<std::string, HapticSampleStatistics>::iterator it = statistics.begin(); it != statistics.end(); ++it)
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index 5e75db5b05f74cf23cecb6ce3908f6f9ca1fd2c9..cac9a85c903e4c83125489f97c8c65dd3b368ff5 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -30,6 +30,7 @@
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <mutex>
namespace armarx
{
@@ -146,7 +147,7 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
- armarx::Mutex dataMutex;
+ std::mutex dataMutex;
std::string topicName;
PeriodicTask<HapticObserver>::pointer_type statisticsTask;
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index d240a3c60d2fa0a382ae4fe69cb33df69a643c9c..8e3037badb78a49b9cd6ef2847460c70995f448b 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -31,6 +31,13 @@
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
+#include <boost/algorithm/string/trim.hpp>
+
+#include <boost/shared_ptr.hpp>
+#include <memory>
+
+using boost::dynamic_pointer_cast;
+using std::dynamic_pointer_cast;
namespace armarx
{
@@ -46,7 +53,7 @@ namespace armarx
void HeadIKUnit::onInitComponent()
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
@@ -61,7 +68,7 @@ namespace armarx
void HeadIKUnit::onConnectComponent()
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
@@ -99,7 +106,7 @@ namespace armarx
{
release();
- //ScopedLock lock(accessMutex);
+ //std::unique_lock lock(accessMutex);
if (drawer)
{
drawer->removeSphereDebugLayerVisu("HeadViewTarget");
@@ -122,7 +129,7 @@ namespace armarx
void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
cycleTime = milliseconds;
@@ -135,7 +142,7 @@ namespace armarx
void HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c)
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
this->robotNodeSetNames = armarx::Split(robotNodeSetName, ",");
for (auto& setName : robotNodeSetNames)
@@ -204,7 +211,7 @@ namespace armarx
void HeadIKUnit::request(const Ice::Current& c)
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
requested = true;
ARMARX_IMPORTANT << "Requesting HeadIKUnit";
@@ -224,7 +231,7 @@ namespace armarx
void HeadIKUnit::release(const Ice::Current& c)
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
ARMARX_INFO << "Releasing HeadIKUnit";
requested = false;
@@ -256,9 +263,9 @@ namespace armarx
bool doCalculation = false;
{
- ScopedTryLock lock(accessMutex);
+ std::unique_lock lock(accessMutex, std::defer_lock);
- if (lock.owns_lock())
+ if (lock.try_lock())
{
doCalculation = requested && newTargetSet;
newTargetSet = false;
@@ -272,7 +279,7 @@ namespace armarx
if (doCalculation)
{
- ScopedLock lock(accessMutex);
+ std::unique_lock lock(accessMutex);
VirtualRobot::RobotNodeSetPtr kinematicChain;
bool foundSolution = false;
@@ -304,8 +311,7 @@ namespace armarx
auto tcpNode = kinematicChain->getTCP();
VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
-
- virtualPrismaticJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+ virtualPrismaticJoint = dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
if (!virtualPrismaticJoint)
{
ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 251c88d4d5a1cfd0822f79a744b3757a23cd451d..b814b519aae4f377a55e0610759e52663007ad50 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -25,13 +25,14 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/interface/units/HeadIKUnit.h>
+#include <mutex>
+
namespace armarx
{
@@ -98,7 +99,7 @@ namespace armarx
private:
void periodicExec();
- Mutex accessMutex;
+ std::mutex accessMutex;
bool requested;
int cycleTime;
PeriodicTask<HeadIKUnit>::pointer_type execTask;
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 52e6f92ea93cb4c5f4f58611a56fb6f73533b3af..dc73454fe838fafeddebbbfca0915efaca5752f0 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -71,7 +71,7 @@ namespace armarx
const std::string& device, const std::string& name, const IMUData& values,
const TimestampBasePtr& timestamp, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 4591228d92d457967b85106554898345895b6ee2..728e2dc385906114fa09c2b7b044990196a94a2a 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -82,7 +83,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 0459725252d769f7c117bb477a4482a887cd5c5f..ccd47dcb5ff5de17eadd3653c749d981da1a8265 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -34,6 +34,9 @@
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
namespace armarx
{
void KinematicUnit::onInitComponent()
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 15712aa55724e6eb29ffe97e9fa9226a5c9c9872..c1796936e47e97e06b6a923a4ca6832bcdf81f40 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -38,6 +38,9 @@
#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
namespace armarx
{
// ********************************************************************
@@ -287,7 +290,7 @@ namespace armarx
// ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
bool newChannel;
{
- ScopedLock lock(initializedChannelsMutex);
+ std::unique_lock lock(initializedChannelsMutex);
newChannel = initializedChannels.count(channelName) == 0;
initializedChannels.insert(channelName);
}
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 8d726600692d58d32b35c22f11b43fa7f4e3b4dd..5b21e89d0cb300c331cd105b907439da70cc937a 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -32,6 +32,7 @@
#include <ArmarXCore/observers/Observer.h>
#include <string>
+#include <mutex>
namespace armarx
{
@@ -154,7 +155,7 @@ namespace armarx
protected:
void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged);
std::set<std::string> initializedChannels;
- Mutex initializedChannelsMutex;
+ std::mutex initializedChannelsMutex;
private:
std::string robotNodeSetName;
std::set<std::string> robotNodes;
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 81f545e0d01e864ac1576718f841bb798987fa01..f830c604c404e232535cd1812e49154da09dc496 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -63,7 +63,7 @@ void KinematicUnitSimulation::onInitKinematicUnit()
ARMARX_TRACE;
{
// duplicate the loop because of locking
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
// initialize JoinStates
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
@@ -115,7 +115,7 @@ void KinematicUnitSimulation::simulationFunction()
};
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
{
@@ -221,7 +221,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
bool aValueChanged = false;
NameControlModeMap changedControlModes;
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
for (NameControlModeMap::const_iterator it = targetJointModes.begin(); it != targetJointModes.end(); it++)
{
@@ -256,7 +256,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
// name value maps for reporting
NameValueMap actualJointAngles;
bool aValueChanged = false;
@@ -321,7 +321,7 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
NameValueMap actualJointVelocities;
bool aValueChanged = false;
@@ -354,7 +354,7 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
NameValueMap actualJointTorques;
bool aValueChanged = false;
@@ -408,7 +408,7 @@ void KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJo
void KinematicUnitSimulation::stop(const Ice::Current& c)
{
- boost::mutex::scoped_lock lock(jointStatesMutex);
+ std::unique_lock lock(jointStatesMutex);
for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
{
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index cc3dae20ac7476572a4250c384a6395c7fd7d61b..f6512ecf5f261c3afa4047b6d87b5d6b3ff50811 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -29,10 +29,12 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <random>
#include <IceUtil/Time.h>
+#include <random>
+#include <mutex>
+
namespace armarx
{
/**
@@ -178,7 +180,7 @@ namespace armarx
KinematicUnitSimulationJointStates jointStates;
KinematicUnitSimulationJointStates previousJointStates;
KinematicUnitSimulationJointInfos jointInfos;
- boost::mutex jointStatesMutex;
+ std::mutex jointStatesMutex;
IceUtil::Time lastExecutionTime;
float noise;
int intervalMs;
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 0944336bfaf52400a6685ce58d4b1c409a3e86c3..04bd47d5a9cd4931ad9783391e346c88d29aa4ce 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -64,7 +64,7 @@ namespace armarx
void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!existsChannel(device))
{
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
index 4b093eb49cc12f3d07b457f744f3fda78e8872c0..05aa07cb9c36668d72d4fa8046c855c28e0ab735 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.h
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -78,7 +79,7 @@ namespace armarx
const TimestampBasePtr& timestamp, const Ice::Current& c) override;
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
};
}
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
index b6885037df8700f4f6c09b01d0483a3c277210b2..95937330857902c4127d0ffe3b35eebff3e8d86d 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
@@ -64,7 +64,7 @@ namespace armarx
void armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!existsChannel(name))
{
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.h b/source/RobotAPI/components/units/MetaWearIMUObserver.h
index 6dba1f74653563fc132706e39d23036b2595ae4a..87eb6b49fbd390a66aef98c4f0a22d174115d659 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.h
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -74,7 +75,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
void offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data);
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
index 02a1fc0d69dd2618b13664b3e52b77dfe4d42ed4..52f6af1c1df0c1fd925a7b5f59371a19c353015c 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -46,7 +46,7 @@
// RobotAPI
#include <RobotAPI/components/units/PlatformUnit.h>
-#include <RobotAPI/interface/components/ObstacleAvoidanceInterface.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
index ce09bacd4543fb2ac779229d5c02de89bf3d6e05..f87a5b56edb87cc841b71da64a5018d42ace1274 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -65,7 +65,7 @@ namespace armarx
void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
if (!existsChannel(name))
@@ -86,7 +86,7 @@ namespace armarx
/*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
index 3bdc63e1a6b874cb9005260801c8c11869b009c7..0d415cb2bc89396f9fc0d81669009f034b17c646 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.h
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -81,7 +82,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 514a1767b5c95ad06228d87f4c09df94ff87f833..75640877635972e5cbf11f078580487834f64d4d 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -59,7 +59,7 @@ namespace armarx
void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
std::stringstream ss;
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
index 033f9cd08bd4f82bf399eb33df20eb2ac5a4956c..70916163f96bfa87944269e5bcf3f85a88bb5278 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -29,6 +29,7 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <mutex>
namespace armarx
{
@@ -80,7 +81,7 @@ namespace armarx
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
};
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index b4b4879b4ac4c0fd3246565c06eef9f83d50b35c..93b61e1d4f7338e888e55e0427e203a89843f731 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -94,7 +94,7 @@ namespace armarx
lastExecutionTime = now;
std::vector<float> vel(3, 0.0f);
{
- ScopedLock lock(currentPoseMutex);
+ std::unique_lock lock(currentPoseMutex);
switch (platformMode)
{
case ePositionControl:
@@ -177,7 +177,7 @@ namespace armarx
{
ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
{
- ScopedLock lock(currentPoseMutex);
+ std::unique_lock lock(currentPoseMutex);
platformMode = ePositionControl;
targetPositionX = targetPlatformPositionX;
targetPositionY = targetPlatformPositionY;
@@ -199,7 +199,7 @@ namespace armarx
void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
{
ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
- ScopedLock lock(currentPoseMutex);
+ std::unique_lock lock(currentPoseMutex);
platformMode = eVelocityControl;
linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
@@ -211,7 +211,7 @@ namespace armarx
{
float targetPositionX, targetPositionY, targetRotation;
{
- ScopedLock lock(currentPoseMutex);
+ std::unique_lock lock(currentPoseMutex);
targetPositionX = currentPositionX + targetPlatformOffsetX;
targetPositionY = currentPositionY + targetPlatformOffsetY;
targetRotation = currentRotation + targetPlatformOffsetRotation;
@@ -221,7 +221,7 @@ namespace armarx
void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
{
- ScopedLock lock(currentPoseMutex);
+ std::unique_lock lock(currentPoseMutex);
maxLinearVelocity = positionalVelocity;
maxAngularVelocity = orientaionalVelocity;
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 8c7743a7f940c74860597157a46beb16b4de6794..c6d9851b1345a8aad3bd296b99f7c06ea774fec6 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -99,7 +99,7 @@ namespace armarx
PropertyDefinitionsPtr createPropertyDefinitions() override;
protected:
- boost::mutex currentPoseMutex;
+ std::mutex currentPoseMutex;
IceUtil::Time lastExecutionTime;
int intervalMs;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index d5f45c229d9595643638eb7965354c2591afe952..28bc1e4819946d6734110fa3831e33f4bc1009b0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -7,6 +7,8 @@
#include <VirtualRobot/math/Helpers.h>
+#include <iomanip>
+
#include "NJointCartesianNaturalPositionController.h"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index b8b5c3c93a55497807556278ac357a2c2c5f7115..6c1b1930b58993ae49a58d487cc540503abbc0d5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -7,6 +7,8 @@
#include "NJointCartesianWaypointController.h"
+#include <iomanip>
+
namespace armarx
{
std::ostream& operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 7f73e77b62de92a209368a3f379e0c9e1d3da81e..e42b68f1d78db459b423fd93e2c6723fa1cdd39e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -80,7 +80,7 @@ namespace armarx
return d;
}
- boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
+ std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
{
ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
auto result = used;
@@ -90,13 +90,13 @@ namespace armarx
{
if (used.at(i))
{
- return boost::optional<std::vector<char>>();
+ return std::nullopt;
}
result.at(i) = true;
}
}
- return {true, std::move(result)};
+ return std::move(result);
}
NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
@@ -282,7 +282,7 @@ namespace armarx
onExitNJointController();
// make sure the destructor of the handles does not throw an exception and triggers a termination of the process
ARMARX_DEBUG << "Deleting thread handles";
- ScopedLock lock(threadHandlesMutex);
+ std::unique_lock lock(threadHandlesMutex);
for (auto& pair : threadHandles)
{
try
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 1f6e157696ee50f33a216cf6665d271b11be7939..88363b7242e9666883d22351adfb600735deb6f7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -28,14 +28,12 @@
#include <functional>
#include <unordered_set>
-#include <boost/optional.hpp>
-
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXGui/interface/WidgetDescription.h>
@@ -53,13 +51,8 @@
#include "../util/JointAndNJointControllers.h"
#include "../util/ControlThreadOutputBuffer.h"
-namespace VirtualRobot
-{
- class Robot;
- class RobotNode;
- using RobotNodePtr = boost::shared_ptr<RobotNode>;
- using RobotPtr = boost::shared_ptr<Robot>;
-}
+#include <VirtualRobot/VirtualRobot.h>
+#include <optional>
namespace armarx::RobotUnitModule
{
@@ -707,7 +700,7 @@ namespace armarx
template < typename Task >
void runTask(const std::string& taskName, Task&& task)
{
- ScopedLock lock(threadHandlesMutex);
+ std::unique_lock lock(threadHandlesMutex);
ARMARX_CHECK_EXPRESSION(!taskName.empty());
ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
@@ -717,7 +710,7 @@ namespace armarx
threadHandles[taskName] = handlePtr;
}
std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
- Mutex threadHandlesMutex;
+ std::mutex threadHandlesMutex;
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////// ice interface //////////////////////////////////// //
@@ -877,11 +870,11 @@ namespace armarx
const std::map<std::string, const JointController*>& getControlDevicesUsedJointController();
//check for conflict
- boost::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
- boost::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
+ std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
+ std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
template<class ItT>
- static boost::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
+ static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////////// publishing ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index bcefd5a83350721c67aff181034f5f0ef80fe8d6..0b89eaf5f18d12223a623d3b1eba13d672ddfd3b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -149,7 +149,7 @@ namespace armarx
return controlDeviceUsedJointController;
}
- inline boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
+ inline std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
@@ -273,11 +273,11 @@ namespace armarx
namespace armarx
{
template<class ItT>
- inline boost::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
+ inline std::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
- return {true, std::vector<char>{}};
+ return std::vector<char>{};
}
std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
std::vector<char> inuse(n, false);
@@ -288,10 +288,10 @@ namespace armarx
{
return r;
}
- inuse = std::move(r.get());
+ inuse = std::move(*r);
++first;
}
- return {true, std::move(inuse)};
+ return std::move(inuse);
}
}
namespace armarx::detail
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index cb80274fe3e8e0fb67715fcc887576afb9fba8a2..bc0c3d694843f9c207a38ce2b30c5f5d8ff75a52 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,6 +22,8 @@
#include "NJointTaskSpaceImpedanceController.h"
+#include <VirtualRobot/MathTools.h>
+
using namespace armarx;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 1cb5f37569f80c4d8ada490a901c6633c61877e1..8f5960857d371d51d9053113a3b032732f3bb905 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -364,7 +364,7 @@ namespace armarx::RobotUnitModule
ARMARX_ERROR << ss.str();
throw InvalidArgumentException {ss.str()};
}
- inuse = std::move(r.get());
+ inuse = std::move(*r);
}
ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
auto printInUse = ARMARX_STREAM_PRINTER
@@ -393,7 +393,7 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "keeping already requested NJointControllerBase '"
<< nJointCtrl->getInstanceName() << "' in list of requested controllers";
ctrlsToAct.insert(nJointCtrl);
- inuse = std::move(r.get());
+ inuse = std::move(*r);
}
else
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 526db6aa7e1771f8dc234a8bc890be2f3003e6be..219ba7815eba0cb2b03c4a5c93ccbe0e0973d175 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -24,6 +24,8 @@
#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleRobotData.h"
+#include <ArmarXCore/core/util/StringHelpers.h>
+
namespace armarx::RobotUnitModule
{
const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
@@ -679,17 +681,14 @@ namespace armarx::RobotUnitModule
controlDeviceHardwareControlModeGroupsStr.end(), ';'
) + 1;
ctrlModeGroups.groups.reserve(numGroups);
- std::vector<std::string> strGroups;
- strGroups.reserve(numGroups);
- boost::algorithm::split(strGroups, controlDeviceHardwareControlModeGroupsStr, boost::is_any_of(";"));
+ std::vector<std::string> strGroups = Split(controlDeviceHardwareControlModeGroupsStr, ";");
for (const auto& gstr : strGroups)
{
- std::vector<std::string> strElems;
- boost::algorithm::split(strElems, gstr, boost::is_any_of(","));
+ bool trimDeviceNames = true;
+ std::vector<std::string> strElems = Split(gstr, ",", trimDeviceNames);
std::set<std::string> group;
for (auto& device : strElems)
{
- boost::algorithm::trim(device);
ARMARX_CHECK_EXPRESSION_W_HINT(
!device.empty(),
"The ControlDeviceHardwareControlModeGroups property contains empty device names"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index f5d357ce82651f0f5aa52c94bf8015a158f85f75..70da5df0a915823ed517022cd287ce16b5eb057b 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -30,6 +30,8 @@
#include "../util/ControlThreadOutputBuffer.h"
+#include <boost/algorithm/string/predicate.hpp>
+
namespace armarx::RobotUnitModule
{
void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index b4aced7b9e0fb437985e14d8bf8a2fa1adfb9440..71c9f1caad2438d2c0869dc64e70f01440907de6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -92,6 +92,11 @@ namespace armarx::RobotUnitModule
}
void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
+ bool isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return false;
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index a75e7810451595119ef11af4d711c931e27f4b79..a1c4fb5941d1108f8d5626f7995d690b6733f8e1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -25,6 +25,8 @@
#include "../NJointControllers/NJointController.h"
#include "../Units/RobotUnitSubUnit.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx::RobotUnitModule
{
/**
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 12acad70bbf3e49c8d2aaa4ff6e69c299570fd8c..541147c28728402614fb24648376efd893615746 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -30,6 +30,10 @@
#include "RobotUnitModuleRobotData.h"
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/trim.hpp>
+
namespace armarx::RobotUnitModule
{
const std::string& RobotData::getRobotPlatformName() const
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index aeea0cfd4ac9bbd1f6c899c1d887d8749b1f92d8..1e0ce16e1da5103672893f829883630ec186eb6d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -29,6 +29,9 @@
#include "../NJointControllers/NJointController.h"
+#include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
#define FLOOR_OBJ_STR "FLOOR"
namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 65e8263cc44f4669dd43f0c4b084840f76b2b847..7913d2dd9a4a27c7b90ee4b6d4e5a7b9d4d13736 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -24,8 +24,6 @@
#include <mutex>
-#include <boost/algorithm/string.hpp>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/components/units/KinematicUnit.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index f67bb14d7a0df2743ee3d65410f58f768937f1fb..99ec57ddd276fe00bbc9ca9c76967d8e1d56ac11 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -21,6 +21,8 @@
*/
#include "PlatformSubUnit.h"
+#include <boost/algorithm/clamp.hpp>
+
void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
{
if (!getProxy())
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 7420f33e06d32adbbcc66d1b0ed09dc66effdfd5..7fb01dd2a3d2f49059a5906c7ea998ed8c95a4d9 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -24,8 +24,6 @@
#include <mutex>
-#include <boost/algorithm/clamp.hpp>
-
#include <Eigen/Core>
#include <VirtualRobot/MathTools.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index f978876e538d9b8420af57d8c35b8f8f8063a9bd..1c98f6b653589f3a6f6fd611d4695e1933111458 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -58,7 +58,7 @@ void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
std::string tcp;
if (tcpName.empty())
@@ -185,7 +185,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
bool TCPControllerSubUnit::isRequested(const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
for (auto& pair : tcpControllerNameMap)
{
auto ctrl = robotUnit->getNJointController(pair.second);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index b834d2b8adb16a0405bd3c60834693a3f7df4f57..cb7f6603057bdf2219d3f2aa92506bc6d76d2d53 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -27,6 +27,9 @@
#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointTCPController.h"
#include "../util.h"
+
+#include <mutex>
+
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
@@ -57,7 +60,7 @@ namespace armarx
// RobotUnitSubUnit interface
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
private:
- mutable Mutex dataMutex;
+ mutable std::mutex dataMutex;
RobotUnit* robotUnit = nullptr;
VirtualRobot::RobotPtr coordinateTransformationRobot;
std::map<std::string, std::string> tcpControllerNameMap;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 0a3669091152c2de2ec2caad4865a0e3d6b6c08d..4af60478ff5da6a4a57d055dd919473a64b1d1d5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -158,7 +158,7 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
{
ARMARX_DEBUG << "Loading Trajectory ...";
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
this->jointTraj = TrajectoryPtr::dynamicCast(jointTraj);
@@ -215,7 +215,7 @@ void TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& base
void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!controllerExists())
{
@@ -237,7 +237,7 @@ double TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!controllerExists())
{
@@ -251,7 +251,7 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
{
ARMARX_DEBUG << "Setting end-time ...";
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!jointTraj)
{
@@ -306,7 +306,7 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
{
ARMARX_DEBUG << "Setting joints in use ...";
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (controllerExists())
{
@@ -366,7 +366,7 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist)
{
- ScopedRecursiveLock lock(controllerMutex);
+ std::unique_lock lock(controllerMutex);
if (controllerExists() && deleteIfAlreadyExist)
{
@@ -413,7 +413,7 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
void TrajectoryControllerSubUnit::assureTrajectoryController()
{
- ScopedRecursiveLock lock(controllerMutex);
+ std::unique_lock lock(controllerMutex);
if (!controllerExists())
{
@@ -424,7 +424,7 @@ void TrajectoryControllerSubUnit::assureTrajectoryController()
bool TrajectoryControllerSubUnit::controllerExists()
{
- ScopedRecursiveLock lock(controllerMutex);
+ std::unique_lock lock(controllerMutex);
auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
NJointTrajectoryControllerPtr trajController;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index d50323bf97082b1fb89df256ddf63784c0b7c277..b0b65db9dad4ee9cd94742d27b542cb03719492a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -30,6 +30,8 @@
#include "KinematicSubUnit.h"
+#include <mutex>
+
namespace armarx
{
class TrajectoryControllerSubUnitPropertyDefinitions:
@@ -152,8 +154,8 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawer;
PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
- Mutex dataMutex;
- RecursiveMutex controllerMutex;
+ std::mutex dataMutex;
+ std::recursive_mutex controllerMutex;
bool recreateController = true;
// Component interface
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 0d0aa5b7616f7e88412d70decd10fbd545327139..98fe0b961192861a9fa7df1029afc178cd4db1ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -154,7 +154,7 @@ namespace armarx
}
}
- ControlThreadOutputBuffer::RtMessageLogEntryBase& ControlThreadOutputBuffer::RtMessageLogEntryBase::setLoggingLevel(MessageType lvl)
+ ControlThreadOutputBuffer::RtMessageLogEntryBase& ControlThreadOutputBuffer::RtMessageLogEntryBase::setLoggingLevel(MessageTypeT lvl)
{
printMsg = true;
loggingLevel = lvl;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index bca3c8214e327b59584bc091491289cc2f3dc690..6412aaba778c5137020f43ae7e9efadf3e69c640 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -29,6 +29,7 @@
#include <boost/format.hpp>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
@@ -75,7 +76,7 @@ namespace armarx::detail
{}
virtual ~RtMessageLogEntryBase() = default;
- RtMessageLogEntryBase& setLoggingLevel(MessageType lvl);
+ RtMessageLogEntryBase& setLoggingLevel(MessageTypeT lvl);
RtMessageLogEntryBase& deactivateSpam(float sec);
RtMessageLogEntryBase& deactivateSpamTag(std::uint64_t tag);
@@ -90,7 +91,7 @@ namespace armarx::detail
protected:
friend struct RtMessageLogBuffer;
private:
- MessageType loggingLevel {eUNDEFINED};
+ MessageTypeT loggingLevel {MessageTypeT::UNDEFINED};
float deactivateSpamSec {0};
bool printMsg {false};
std::uint64_t deactivateSpamTag_;
@@ -306,14 +307,14 @@ namespace armarx
->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
}())
-#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eDEBUG)
-#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eVERBOSE)
-#define ARMARX_RT_LOGF_INFO(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eINFO)
-#define ARMARX_RT_LOGF_IMPORTANT(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eIMPORTANT)
-#define ARMARX_RT_LOGF_WARNING(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eWARN)
-#define ARMARX_RT_LOGF_WARN(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eWARN)
-#define ARMARX_RT_LOGF_ERROR(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eERROR)
-#define ARMARX_RT_LOGF_FATAL(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eFATAL)
+#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
+#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
+#define ARMARX_RT_LOGF_INFO(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
+#define ARMARX_RT_LOGF_IMPORTANT(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
+#define ARMARX_RT_LOGF_WARNING(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_WARN(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_ERROR(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
+#define ARMARX_RT_LOGF_FATAL(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
//impl
namespace armarx::detail
diff --git a/source/RobotAPI/components/units/SensorActorUnit.cpp b/source/RobotAPI/components/units/SensorActorUnit.cpp
index 7cc36ca5e76d4315887bd5dbbf00102a15aa2db5..f93f684d72619a4ad402f8f56a5f6c285d00a866 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.cpp
+++ b/source/RobotAPI/components/units/SensorActorUnit.cpp
@@ -54,12 +54,12 @@ void SensorActorUnit::init(const Ice::Current& c)
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " is already initialized." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " is already initialized." << armarx::flush;
}
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be initialized while unrequested." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be initialized while unrequested." << armarx::flush;
}
}
@@ -79,12 +79,12 @@ void SensorActorUnit::start(const Ice::Current& c)
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be started." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " could not be started." << armarx::flush;
}
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be started while unrequested." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be started while unrequested." << armarx::flush;
}
}
@@ -104,12 +104,12 @@ void SensorActorUnit::stop(const Ice::Current& c)
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " could not be stopped." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " could not be stopped." << armarx::flush;
}
}
else
{
- ARMARX_LOG << armarx::eWARN << "Unit " << getName() << " can not be stopped while unrequested. " << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be stopped while unrequested. " << armarx::flush;
}
}
@@ -127,7 +127,7 @@ void SensorActorUnit::request(const Ice::Current& c)
// try to lock unit
if (!unitMutex.try_lock())
{
- ARMARX_LOG << armarx::eERROR << "Trying to request already owned unit " << getName() << "\n" << armarx::flush;
+ ARMARX_ERROR << "Trying to request already owned unit " << getName() << "\n";
throw ResourceUnavailableException("Trying to request already owned unit");
}
@@ -147,7 +147,7 @@ void SensorActorUnit::release(const Ice::Current& c)
if (!(ownerId == callerId))
{
- ARMARX_LOG << armarx::eERROR << "Trying to release unit owned by another component" << armarx::flush;
+ ARMARX_ERROR << "Trying to release unit owned by another component";
throw ResourceNotOwnedException("Trying to release unit owned by another component");
}
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index bef26a5e7c92e97d3ab2889e4081b59caee0daec..2c18c79e6bd840ed60f5660a858e104faecc3de6 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/interface/units/UnitInterface.h>
-#include <boost/thread/mutex.hpp>
+#include <mutex>
namespace armarx
{
@@ -125,7 +125,7 @@ namespace armarx
*/
virtual void onStop() {};
- boost::mutex unitMutex;
+ std::mutex unitMutex;
Ice::Identity ownerId;
UnitExecutionState executionState;
};
diff --git a/source/RobotAPI/components/units/SpeechObserver.cpp b/source/RobotAPI/components/units/SpeechObserver.cpp
index 26c215dbfa745af1213c3b1f52db4c5a7aa761d0..29c03c5eefe9ff3089c662adf61bee28cd8817e7 100644
--- a/source/RobotAPI/components/units/SpeechObserver.cpp
+++ b/source/RobotAPI/components/units/SpeechObserver.cpp
@@ -24,6 +24,8 @@
#include "SpeechObserver.h"
#include <ArmarXCore/util/json/JSONObject.h>
+#include <Ice/ObjectAdapter.h>
+
using namespace armarx;
SpeechObserver::SpeechObserver()
@@ -72,7 +74,7 @@ std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
reportStateCounter++;
setDataField("TextToSpeech", "State", Variant(SpeechStateToString(state)));
setDataField("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter));
@@ -81,7 +83,7 @@ void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current
void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
reportTextCounter++;
setDataField("TextToSpeech", "Text", Variant(text));
setDataField("TextToSpeech", "TextChangeCounter", Variant(reportTextCounter));
@@ -90,7 +92,7 @@ void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
void SpeechObserver::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
@@ -103,7 +105,7 @@ SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) :
void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
JSONObject json;
json.fromString(text);
obs->setDataField("SpeechToText", "RecognizedText", Variant(json.getString("text")));
@@ -111,6 +113,6 @@ void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::
void armarx::SpeechListenerImpl::reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
diff --git a/source/RobotAPI/components/units/SpeechObserver.h b/source/RobotAPI/components/units/SpeechObserver.h
index 43e3bbc6fc37511b052d590fde29a9e8a09fabd9..2cf428ace331d0afc54e5e711dcd60304b542172 100644
--- a/source/RobotAPI/components/units/SpeechObserver.h
+++ b/source/RobotAPI/components/units/SpeechObserver.h
@@ -26,6 +26,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/observers/SpeechObserverInterface.h>
+#include <mutex>
+
namespace armarx
{
class SpeechObserverPropertyDefinitions:
@@ -46,7 +48,7 @@ namespace armarx
SpeechListenerImpl(SpeechObserver* obs);
protected:
SpeechObserver* obs;
- Mutex dataMutex;
+ std::mutex dataMutex;
// TextListenerInterface interface
public:
void reportText(const std::string&, const Ice::Current&) override;
@@ -78,7 +80,7 @@ namespace armarx
static std::string SpeechStateToString(TextToSpeechStateType state);
private:
- Mutex dataMutex;
+ std::mutex dataMutex;
int reportTextCounter = 0;
int reportStateCounter = 0;
};
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 8f4bd0e84990ce14bdc686609e3fdbb317321247..37e1df461dc90453d02567bc65feb6deeb744daf 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -23,18 +23,21 @@
#include "TCPControlUnit.h"
#include <RobotAPI/libraries/core/LinkedPose.h>
-#include <boost/unordered_map.hpp>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/MathTools.h>
#include <Eigen/Core>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
#include <memory>
+#include <cfloat>
using namespace VirtualRobot;
using namespace Eigen;
@@ -66,7 +69,7 @@ namespace armarx
void TCPControlUnit::onConnectComponent()
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
debugObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
@@ -170,7 +173,7 @@ namespace armarx
void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
- ScopedLock lock(taskMutex);
+ std::unique_lock lock(taskMutex);
cycleTime = milliseconds;
if (execTask)
@@ -187,7 +190,7 @@ namespace armarx
request();
}
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
@@ -266,7 +269,7 @@ namespace armarx
}
ARMARX_IMPORTANT << "Requesting TCPControlUnit";
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
requested = true;
execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
@@ -278,7 +281,7 @@ namespace armarx
void TCPControlUnit::release(const Ice::Current& c)
{
ARMARX_IMPORTANT << "Releasing TCPControlUnit";
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
// while (calculationRunning)
// {
@@ -302,9 +305,9 @@ namespace armarx
void TCPControlUnit::periodicExec()
{
- ScopedTryLock lock(dataMutex);
+ std::unique_lock lock(dataMutex, std::defer_lock);
- if (lock.owns_lock())
+ if (lock.try_lock())
{
{
@@ -358,7 +361,7 @@ namespace armarx
localDIKMap[data.nodeSetName] = dIk;
}
- EDifferentialIKPtr dIk = boost::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
+ auto dIk = std::dynamic_pointer_cast<EDifferentialIK>(localDIKMap[data.nodeSetName]);
dIk->clearGoals();
}
@@ -476,7 +479,7 @@ namespace armarx
{
RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
- EDifferentialIKPtr dIK = boost::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
+ auto dIK = std::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
// dIK->setVerbose(true);
Eigen::VectorXf jointDelta;
@@ -656,7 +659,7 @@ namespace armarx
}
else
{
- VR_ERROR << "Internal error?!" << endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
@@ -752,9 +755,9 @@ namespace armarx
ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
- if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
+ if (std::isnan(jv[i]) || std::isinf(jv[i]))
{
- ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << endl;
+ ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << std::endl;
dofWeights = tempDOFWeights;
return false;
}
@@ -776,7 +779,7 @@ namespace armarx
{
if (verbose)
{
- ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << endl;
+ ARMARX_INFO << deactivateSpam(1) << "Tolerances ok, loop:" << step << std::endl;
}
break;
@@ -789,14 +792,14 @@ namespace armarx
if (dTheta.norm() < mininumChange)
{
// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
+ ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << std::endl;
break;
}
if (checkImprovement && posDist > lastDist)
{
// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << endl;
+ ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << std::endl;
robot->setJointValues(rns, jvBest);
break;
}
@@ -835,9 +838,9 @@ namespace armarx
if (step >= maxNStep && verbose)
{
- ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << endl;
- ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << endl;
- ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << endl;
+ ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << std::endl;
+ ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << std::endl;
+ ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << std::endl;
return false;
}
@@ -901,7 +904,7 @@ namespace armarx
}
else
{
- VR_ERROR << "Internal error?!" << endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
@@ -1051,7 +1054,7 @@ namespace armarx
}
else
{
- VR_ERROR << "Internal error?!" << endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
@@ -1221,7 +1224,7 @@ namespace armarx
}
Vector3f position = targets[tcp].block(0, 3, 3, 1) - tcp->getGlobalPose().block(0, 3, 3, 1);
- //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << endl;
+ //cout << "Error Position <" << tcp->getName() << ">: " << position.norm() << std::endl;
//cout << boost::format("Error Position < %1% >: %2%") % tcp->getName() % position.norm() << std::endl;
float result = 0.0f;
Eigen::VectorXf tcpWeight(3);
@@ -1297,7 +1300,7 @@ void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&,
void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&)
{
- ScopedLock lock(reportMutex);
+ std::unique_lock lock(reportMutex);
FramedPoseBaseMap tcpPoses;
std::string rootFrame = localReportRobot->getRootNode()->getName();
auto it = jointAngles.find("timestamp");
@@ -1330,7 +1333,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
}
lastTopicReportTime = TimeUtil::GetTime();
- ScopedLock lock(reportMutex);
+ std::unique_lock lock(reportMutex);
if (!localVelReportRobot)
{
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index cc984d4358f9e0b1dbec9455f6cb26283aa876bf..8b373d10178d0975bbf78831b08859c15781f9b6 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -25,13 +25,14 @@
#include <RobotAPI/interface/units/TCPControlUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/core/Component.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <memory>
+
namespace armarx
{
/**
@@ -211,9 +212,9 @@ namespace armarx
FramedPoseBaseMap lastTCPPoses;
IceUtil::Time lastTopicReportTime;
- Mutex dataMutex;
- Mutex taskMutex;
- Mutex reportMutex;
+ std::mutex dataMutex;
+ std::mutex taskMutex;
+ std::mutex reportMutex;
bool calculationRunning;
double syncTimeDelta;
@@ -276,7 +277,7 @@ namespace armarx
std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf> tcpWeights;
Eigen::VectorXf tcpWeightVec;
};
- using EDifferentialIKPtr = boost::shared_ptr<EDifferentialIK>;
+ using EDifferentialIKPtr = std::shared_ptr<EDifferentialIK>;
}
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
index 2c34c81588359653d6c2592568a0c822a8f7a1b1..8fda50da01097a9f7eb8ba77d056dfb19dda12e1 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
@@ -74,7 +74,7 @@ namespace armarx
void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
FramedPoseBaseMap::const_iterator it = poseMap.begin();
@@ -126,7 +126,7 @@ namespace armarx
void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
{
- ScopedLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
for (; it != tcpTranslationVelocities.end(); it++)
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index 88cdf43c30f840a00c3b82857bf51b059aba8ad0..92859f6e597b85231d02a6fd1b00b92970f642fa 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -24,6 +24,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <mutex>
+
namespace armarx
{
/**
@@ -76,7 +78,7 @@ namespace armarx
void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::emptyCurrent) override;
void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
- Mutex dataMutex;
+ std::mutex dataMutex;
};
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 38e01c30f7cb4cfdd13ae802c6e3c7dc31e714b2..31ed73e85ab61b61ed48ede725bc03447114e05f 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -44,7 +44,7 @@ void GamepadUnit::onConnectComponent()
sendTask = new SimplePeriodicTask<>([&]
{
ARMARX_TRACE;
- ScopedLock lock(mutex);
+ std::unique_lock lock(mutex);
if (!js.opened())
{
return;
@@ -114,7 +114,7 @@ void GamepadUnit::run()
}
}
ARMARX_TRACE;
- ScopedLock lock(mutex);
+ std::unique_lock lock(mutex);
IceUtil::Time now = IceUtil::Time::now();
dataTimestamp = new TimestampVariant(now);
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 85ac99dc262ec38a2b1e4e3b1464e97e4866f22e..109ce81a17a98c5154b396e09064d9943be6ff0a 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -31,7 +31,6 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
@@ -115,7 +114,7 @@ namespace armarx
SimplePeriodicTask<>::pointer_type sendTask;
void run();
- Mutex mutex;
+ std::mutex mutex;
std::string deviceName;
Joystick js;
GamepadData data;
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 9f1508cce0928aa91169c61cd36f5389ef6d75aa..af108e0e0839fe4b8d1d6c912163e67e0ffeaec9 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,9 +24,7 @@
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/replace.hpp>
-#include <boost/algorithm/string/classification.hpp>
#include <HokuyoLaserScannerDriver/urg_utils.h>
@@ -45,14 +43,12 @@ void HokuyoLaserUnit::onInitComponent()
angleOffset = getProperty<float>("AngleOffset").getValue();
std::string deviceStrings = getProperty<std::string>("Devices").getValue();
- std::vector<std::string> splitDeviceStrings;
- boost::split(splitDeviceStrings, deviceStrings, boost::is_any_of(";"));
+ std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
devices.clear();
devices.reserve(splitDeviceStrings.size());
for (std::string const& deviceString : splitDeviceStrings)
{
- std::vector<std::string> deviceInfo;
- boost::split(deviceInfo, deviceString, boost::is_any_of(","));
+ std::vector<std::string> deviceInfo = Split(deviceString, ",");
if (deviceInfo.size() != 3)
{
ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 0856bed5a856ca6114a3fa3c2c6ba13a833c0fae..61a75d997f2ef1871b12cffc78cde76cb797b3ca 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -5,6 +5,7 @@
#include <fcntl.h>
#include <math.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -316,8 +317,7 @@ void OrientedTactileSensorUnit::run()
OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::string line)
{
SensorData data;
- std::vector<std::string> splitValues;
- boost::split(splitValues, line, boost::is_any_of(" "));
+ std::vector<std::string> splitValues = Split(line, " ");
data.id = stoi(splitValues.at(0));
data.pressure = std::stof(splitValues.at(1));
data.qw = std::stof(splitValues.at(2));
@@ -358,8 +358,7 @@ bool OrientedTactileSensorUnit::loadCalibration()
bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
{
- std::vector<std::string> splitValues;
- boost::split(splitValues, line, boost::is_any_of(" "));
+ std::vector<std::string> splitValues = Split(line, " ");
calibration.accel_offset_x = stoi(splitValues.at(0));
calibration.accel_offset_y = stoi(splitValues.at(1));
calibration.accel_offset_z = stoi(splitValues.at(2));
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index d8a1c3496194a4b1fa3850a67e9b53159a683bc2..7e584629a24c0673cd3fb145a28ea3b771358974 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -9,7 +9,6 @@
#include <iostream>
#include <fstream>
#include <stdio.h>
-#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/Dense>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
index 8559064a772d5b9dc299df845b56481c7f031b68..b0a2f0b3705d9c00c88fff8e0470651f8f3f2f43 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -35,7 +35,6 @@
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <iostream>
-#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
static inline tcflag_t __bitrate_to_flag(unsigned int bitrate)
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 3f13ff1bcc6aa970425931d46afe02cf12657c1c..899c86434cea6757fb0cbb899203131452dea07d 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -123,7 +123,7 @@ std::string WeissHapticSensor::getDeviceName()
void WeissHapticSensor::scheduleSetDeviceTag(std::string tag)
{
- boost::mutex::scoped_lock lock(mutex);
+ std::unique_lock lock(mutex);
setDeviceTagValue = tag;
setDeviceTagScheduled = true;
}
@@ -206,7 +206,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
if (setDeviceTagScheduled)
{
- boost::mutex::scoped_lock lock(mutex);
+ std::unique_lock lock(mutex);
setDeviceTagScheduled = false;
ARMARX_INFO << "[" << device << "] Stopping periodic frame aquisition to set new device tag";
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
index 9e006ae3d9f367d041d7c501eb62142b02f62d1c..25191aab19608735c499bacfe3f9d7ae0fb1e401 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -34,7 +34,8 @@
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-#include <boost/thread/mutex.hpp>
+
+#include <mutex>
namespace armarx
{
@@ -73,7 +74,7 @@ namespace armarx
bool setDeviceTagScheduled;
std::string setDeviceTagValue;
- boost::mutex mutex;
+ std::mutex mutex;
int minimumReportIntervalMs;
};
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 6732b5b66028b2c9b1389088a36ead1ae1147093..37c660527f62b0e50549e6297ec9d31fc06ab350 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -85,12 +85,14 @@ namespace armarx
// Layer info tree.
connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
+#if 0
connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
&layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value());
layerInfoTree.setWidget(widget.layerInfoTree);
layerInfoTree.registerVisualizerCallbacks(visualizer);
+#endif
// We need a callback from the visualizer, when the layers have changed
@@ -289,6 +291,7 @@ namespace armarx
void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
{
+#if 0
(void) previous;
if (!current->parent())
@@ -309,6 +312,7 @@ namespace armarx
{
ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer.";
}
+#endif
}
void ArVizWidgetController::onCollapseAll(bool)
@@ -650,10 +654,17 @@ namespace armarx
{
updates[updateIter->update.name] = &updateIter->update;
}
+
+ auto layerIDsBefore = visualizer.getLayerIDs();
for (auto& pair : updates)
{
visualizer.apply(*pair.second);
}
+ auto layerIDsAfter = visualizer.getLayerIDs();
+ if (layerIDsAfter != layerIDsBefore)
+ {
+ visualizer.emitLayersChanged(layerIDsAfter);
+ }
return updateBegin->timestampInMicroseconds;
}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
index f133beec6a99782c94321d108f3972030c3e6d7f..383bfd544aff904db1afe911363613dfa2dcf269 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
@@ -98,7 +98,7 @@ namespace armarx
*/
static QString GetWidgetName()
{
- return "RobotControl.CartesianNaturalPositionController";
+ return "RobotControl.NJointControllers.CartesianNaturalPositionController";
}
void onInitComponent() override;
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
index 38578c06d1ad3cc784896d377a3986bbc73c3e6a..1b2caffb7f5f818c18e1ee068b6e9d7e0745eabc 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
@@ -17,12 +17,8 @@ set(GUI_MOC_HDRS ${HEADERS})
set(GUI_UIS CartesianWaypointControlGuiWidget.ui)
set(COMPONENT_LIBS
- VirtualRobot
-
SimpleConfigDialog
-
- RobotAPIInterfaces
- RobotAPICore
+ RobotAPIComponentPlugins
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index 5d2d12234ba03ac626e0579ed633b4699eae868d..5348bcaaa944c72d5b05f85afa0cd5253ca5ce9c 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -58,35 +58,27 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::loadSettings(QSettings* settings)
{
std::lock_guard g{_allMutex};
- _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
- _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
+ getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
}
void CartesianWaypointControlGuiWidgetController::saveSettings(QSettings* settings)
{
std::lock_guard g{_allMutex};
- settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
- settings->setValue("ru", QString::fromStdString(_robotUnitName));
+ settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
}
void CartesianWaypointControlGuiWidgetController::onInitComponent()
{
- std::lock_guard g{_allMutex};
- usingProxy(_robotStateComponentName);
- usingProxy(_robotUnitName);
}
void CartesianWaypointControlGuiWidgetController::onConnectComponent()
{
std::lock_guard g{_allMutex};
- //proxies
- {
- _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
- _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
- }
//robot
{
- _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
+ _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
}
//fill rns combo box
{
@@ -135,7 +127,7 @@ namespace armarx
ARMARX_IMPORTANT << "Creating " << njointControllerClassName << " '"
<< _controllerName << "'";
_controller = NJointCartesianWaypointControllerInterfacePrx::checkedCast(
- _robotUnit->createNJointController(
+ getRobotUnit()->createNJointController(
njointControllerClassName,
_controllerName,
cfg));
@@ -199,8 +191,8 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::configured()
{
std::lock_guard g{_allMutex};
- _robotStateComponentName = _dialog->getProxyName("rsc");
- _robotUnitName = _dialog->getProxyName("ru");
+ getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
+ getRobotUnitComponentPlugin().setRobotUnitName(_dialog->getProxyName("ru"));
}
void CartesianWaypointControlGuiWidgetController::triggerParsing()
@@ -283,11 +275,11 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::timerEvent(QTimerEvent*)
{
std::lock_guard g{_allMutex};
- if (!_robot || !_robotStateComponent)
+ if (!_robot)
{
return;
}
- RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
+ synchronizeLocalClone(_robot);
if (_controller)
{
ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
index 47eb81db0e3eb8e4c3c459ac9b50b83054f941cc..fc6b1936d3abe105a02c8fdf2ca72439dcf8d84f 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -34,6 +34,8 @@
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/gui-plugins/CartesianWaypointControlGui/ui_CartesianWaypointControlGuiWidget.h>
@@ -59,20 +61,20 @@ namespace armarx
*/
class ARMARXCOMPONENT_IMPORT_EXPORT
CartesianWaypointControlGuiWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < CartesianWaypointControlGuiWidgetController >
+ public armarx::ArmarXComponentWidgetControllerTemplate < CartesianWaypointControlGuiWidgetController >,
+ public virtual RobotUnitComponentPluginUser,
+ public virtual RobotStateComponentPluginUser
{
Q_OBJECT
public:
/// Controller Constructor
explicit CartesianWaypointControlGuiWidgetController();
-
/// Controller destructor
virtual ~CartesianWaypointControlGuiWidgetController();
/// @see ArmarXWidgetController::loadSettings()
void loadSettings(QSettings* settings) override;
-
/// @see ArmarXWidgetController::saveSettings()
void saveSettings(QSettings* settings) override;
@@ -82,7 +84,7 @@ namespace armarx
*/
static QString GetWidgetName()
{
- return "RobotControl.CartesianWaypointControlGui";
+ return "RobotControl.NJointControllers.CartesianWaypointControlGui";
}
void onInitComponent() override;
@@ -92,9 +94,6 @@ namespace armarx
QPointer<QDialog> getConfigDialog(QWidget* parent) override;
void configured() override;
- signals:
- /* QT signal declarations */
-
private slots:
void on_pushButtonStop_clicked();
void on_pushButtonExecute_clicked();
@@ -110,10 +109,6 @@ namespace armarx
void timerEvent(QTimerEvent*) override;
private:
- std::string _robotStateComponentName;
- std::string _robotUnitName;
- RobotStateComponentInterfacePrx _robotStateComponent;
- RobotUnitInterfacePrx _robotUnit;
Ui::CartesianWaypointControlGuiWidget _ui;
QPointer<SimpleConfigDialog> _dialog;
NJointCartesianWaypointControllerInterfacePrx _controller;
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index fa70bd0987bcf9b62ba26634bc863ec89b57d3f5..f4a9f5673d6d782a774fe27333f449fee5cb57b6 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -25,6 +25,8 @@
#include <QTimer>
+#include <boost/algorithm/string/join.hpp>
+
#include <string>
@@ -85,7 +87,7 @@ namespace armarx
m->addObject(debugDrawer, false);
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
rootVisu->addChild(debugDrawer->getVisualization());
}
enableMainWidgetAsync(true);
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
index 74fc9a145aba93485aba286ef7e66c90eb171028..1ed61c68cc05ded112d6a0515acae2008ab17602 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
@@ -25,6 +25,7 @@
#include <QDialog>
#include <ArmarXCore/core/IceManager.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 4ace604723047d0cb2688feaeedf286a3b944815..5c86b2dda49248a4148b5f3c83785272a94789f9 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -25,6 +25,7 @@
#include "HandUnitConfigDialog.h"
#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
// Qt headers
#include <Qt>
@@ -35,7 +36,7 @@
#include <QMessageBox>
#include <QTimer>
-#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+#include <cmath>
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
index 43b5ca8e42613ea6b6a2e9110329bf482aa87ef6..78b41fd88ccc8fbe9a397be585af03ce28f1d483 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
@@ -22,6 +22,8 @@
#include "HomogeneousMatrixCalculatorWidgetController.h"
+#include <Eigen/Dense>
+
#include <string>
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 76b9edea96974cce02ccb474e0a0e8452a5fe122..17ce02826bd5eb2ab427cfc88310292b82d2d6f3 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -131,7 +131,7 @@ void KinematicUnitWidgetController::onInitComponent()
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
debugLayerVisu = new SoSeparator();
debugLayerVisu->ref();
debugLayerVisu->addChild(debugDrawer->getVisualization());
@@ -182,13 +182,9 @@ void KinematicUnitWidgetController::onConnectComponent()
}
CMakePackageFinder project(projectName);
- Ice::StringSeq projectIncludePaths;
auto pathsString = project.getDataDir();
ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
@@ -262,14 +258,14 @@ void KinematicUnitWidgetController::onDisconnectComponent()
enableMainWidgetAsync(false);
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
robot.reset();
robotNodeSet.reset();
currentNode.reset();
}
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
robotVisu->removeAllChildren();
debugLayerVisu->removeAllChildren();
}
@@ -280,7 +276,7 @@ void KinematicUnitWidgetController::onExitComponent()
enableMainWidgetAsync(false);
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (robotVisu)
{
@@ -376,7 +372,7 @@ void KinematicUnitWidgetController::copyToClipboard()
{
NameValueMap values;
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (selectedControlMode == ePositionControl)
{
@@ -470,7 +466,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
NameValueMap jointAngles;
NameControlModeMap jointModes;
@@ -668,7 +664,7 @@ VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string
return robot;
}
-CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
+VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
{
CoinVisualizationPtr coinVisualization;
@@ -806,7 +802,7 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
void KinematicUnitWidgetController::selectJoint(int i)
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!robotNodeSet || i < 0 || i >= (int)robotNodeSet->getSize())
{
@@ -846,7 +842,7 @@ void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int colu
void KinematicUnitWidgetController::sliderValueChanged(int pos)
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!currentNode)
{
@@ -939,7 +935,7 @@ void KinematicUnitWidgetController::updateControlModesTable()
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
for (unsigned int i = 0; i < rn.size(); i++)
@@ -1011,7 +1007,7 @@ void KinematicUnitWidgetController::updateJointStatusesTable()
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
for (unsigned int i = 0; i < rn.size(); i++)
@@ -1085,7 +1081,7 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
float dirty_squaresum = 0;
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (jointAnglesUpdateFrequency && jointAnglesUpdateFrequency->getValue())
{
@@ -1145,7 +1141,7 @@ void KinematicUnitWidgetController::updateJointVelocitiesTable()
}
float dirty_squaresum = 0;
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!robotNodeSet)
{
return;
@@ -1185,7 +1181,7 @@ void KinematicUnitWidgetController::updateJointTorquesTable()
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
{
return;
@@ -1213,7 +1209,7 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
{
return;
@@ -1244,7 +1240,7 @@ void KinematicUnitWidgetController::updateMotorTemperaturesTable()
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
{
return;
@@ -1278,7 +1274,7 @@ void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointA
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
jointAnglesUpdateFrequency->update(timestamp, new Variant((double)(timestamp - lastJointAngleUpdateTimestamp) * 1e-6));
lastJointAngleUpdateTimestamp = timestamp;
mergeMaps(reportedJointAngles, jointAngles);
@@ -1300,7 +1296,7 @@ void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jo
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
mergeMaps(reportedJointVelocities, jointVelocities);
if (skipper.checkFrequency(c))
{
@@ -1315,7 +1311,7 @@ void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& joint
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
reportedJointTorques = jointTorques;
if (skipper.checkFrequency(c))
{
@@ -1332,7 +1328,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
{
// if(!aValueChanged && reportedJointControlModes.size() > 0)
// return;
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
mergeMaps(reportedJointControlModes, jointModes);
if (skipper.checkFrequency(c))
{
@@ -1342,7 +1338,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (aValueChanged && jointCurrents.size() > 0)
{
@@ -1371,7 +1367,7 @@ void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValue
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
mergeMaps(this->reportedJointTemperatures, jointMotorTemperatures);
if (skipper.checkFrequency(c))
{
@@ -1386,7 +1382,7 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
mergeMaps(reportedJointStatuses, jointStatuses);
if (skipper.checkFrequency(c))
{
@@ -1398,7 +1394,7 @@ void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& joi
void KinematicUnitWidgetController::updateModel()
{
// ARMARX_INFO << "updateModel()" << flush;
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
if (!robotNodeSet)
{
return;
@@ -1413,7 +1409,7 @@ void KinematicUnitWidgetController::highlightCriticalValues()
return;
}
- boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
@@ -1480,10 +1476,8 @@ void KinematicUnitWidgetController::highlightCriticalValues()
}
}
-void KinematicUnitWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
+void KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
- //ARMARX_IMPORTANT << "KinematicUnitWidgetController controller " << getInstanceName() << ": set mutex " << mutex3D.get();
-
this->mutex3D = mutex3D;
if (debugDrawer)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 047655ab2555949865fa9cc0b13b9dd4cda48642..e0d9428388657a0bc07dc871d8ca6a126af26d95 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -54,13 +54,12 @@
#include <QStyledItemDelegate>
#include <ArmarXCore/core/util/IceReportSkipper.h>
-#include <boost/shared_ptr.hpp>
-#include <boost/cstdint.hpp>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <mutex>
namespace armarx
{
- using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>;
-
class KinematicUnitConfigDialog;
/*!
@@ -176,7 +175,7 @@ namespace armarx
void updateGuiElements();
// overwrite setMutex, so that we can inform the debugdrawer
- void setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D) override;
+ void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
QPointer<QWidget> getCustomTitlebarWidget(QWidget* parent) override;
@@ -245,7 +244,7 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr robotNodeSet;
VirtualRobot::RobotNodePtr currentNode;
- CoinVisualizationPtr kinematicUnitVisualization;
+ VirtualRobot::CoinVisualizationPtr kinematicUnitVisualization;
SoNode* kinematicUnitNode;
SoSeparator* rootVisu;
SoSeparator* robotVisu;
@@ -273,10 +272,10 @@ namespace armarx
void on_pushButtonFromJson_clicked();
private:
- boost::recursive_mutex mutexNodeSet;
+ std::recursive_mutex mutexNodeSet;
// init stuff
VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
- CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
+ VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName);
bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet);
bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
index 028d5e2563ccc7015b0253ad3ab3711d710c205e..89b829f8cfa49734b420deb353fbbbf2a8c91c9e 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
@@ -24,6 +24,7 @@
#include <QDialog>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 4dc0dee2b359dc32e42785050c979031d0ed5063..68a21112e18da9b40ea282a899b5802d003279eb 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -36,6 +36,7 @@
//std
#include <memory>
+#include <cmath>
using namespace armarx;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 1c1c83237601ca2dda19101551d3c93295336c10..7df85b9141364c93721785486e19de74da364655 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -48,7 +48,7 @@ namespace armarx
path p = path {homeDir} / ".cmake" / "packages";
if (is_directory(p))
{
- for (const path& entry : boost::make_iterator_range(directory_iterator(p), {}))
+ for (const path& entry : directory_iterator(p))
{
const std::string pkg = entry.filename().string();
if (CMakePackageFinder {pkg, "", true} .packageFound())
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
index 3f2d74a09c04af7cdef406f37afd5f30feca3647..9141512a800d382cbe917659b1fd236b7598bae8 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
@@ -23,6 +23,8 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <thread>
+
namespace armarx
{
RobotUnitWidgetBase::RobotUnitWidgetBase(QString name, QWidget* parent) :
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 76479d44965d055fd707ea9e13eaa49ecf41e4e1..d4d399ecf19b96604c17a822f122ce8a4bf99870 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -31,7 +31,10 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/application/Application.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
// Qt headers
#include <Qt>
@@ -45,6 +48,7 @@
#include <Inventor/Qt/SoQt.h>
#include <ArmarXGui/libraries/StructuralJson/JsonWriter.h>
#include <ArmarXCore/util/json/JSONObject.h>
+
// System
#include <stdio.h>
#include <string>
@@ -139,7 +143,7 @@ void RobotViewerWidgetController::onInitComponent()
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
debugLayerVisu = new SoSeparator();
debugLayerVisu->ref();
debugLayerVisu->addChild(debugDrawer->getVisualization());
@@ -180,13 +184,9 @@ void RobotViewerWidgetController::onConnectComponent()
}
CMakePackageFinder project(projectName);
- Ice::StringSeq projectIncludePaths;
auto pathsString = project.getDataDir();
ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
+ Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
@@ -282,7 +282,7 @@ void RobotViewerWidgetController::onDisconnectComponent()
robotStateComponentPrx = nullptr;
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (robotVisu)
{
@@ -301,7 +301,7 @@ void RobotViewerWidgetController::onExitComponent()
enableMainWidgetAsync(false);
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (debugLayerVisu)
{
@@ -380,7 +380,7 @@ void RobotViewerWidgetController::setRobotVisu(bool colModel)
return;
}
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
VirtualRobot::SceneObject::VisualizationType v = VirtualRobot::SceneObject::Full;
@@ -444,7 +444,7 @@ void RobotViewerWidgetController::showRobot(bool show)
return;
}
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (show && rootVisu->findChild(robotVisu) < 0)
{
@@ -679,7 +679,7 @@ void RobotViewerWidgetController::onConfigurationChanged()
void RobotViewerWidgetController::updateRobotVisu()
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot)
{
@@ -718,7 +718,7 @@ void RobotViewerWidgetController::updateRobotVisu()
void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
for (auto& model : robot->getCollisionModels())
{
@@ -728,7 +728,7 @@ void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
setRobotVisu(true);
}
-void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
+void RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
//ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
this->mutex3D = mutex3D;
@@ -741,7 +741,7 @@ void RobotViewerWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_
void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&)
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot)
{
return;
@@ -756,7 +756,7 @@ void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const Framed
void armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles, Ice::Long, bool aValueChanged, const Ice::Current&)
{
- boost::recursive_mutex::scoped_lock lock(*mutex3D);
+ std::unique_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot || !aValueChanged)
{
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index 9292a3666978a4741ef8f6a75dd6da4021d8ef8d..48a161aa9eed86e54d45d7cf58a753b258880005 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -44,14 +44,10 @@
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-#include <boost/shared_ptr.hpp>
-
#include <Inventor/sensors/SoTimerSensor.h>
namespace armarx
{
- using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>;
-
class RobotViewerConfigDialog;
/**
@@ -117,7 +113,7 @@ namespace armarx
SoNode* getScene() override;
// overwrite setMutex, so that we can inform the debugdrawer
- void setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D) override;
+ void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
signals:
void robotStatusUpdated();
@@ -171,7 +167,7 @@ namespace armarx
// init stuff
VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
- CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
+ VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
QPointer<QWidget> __widget;
QPointer<SimpleConfigDialog> dialog;
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
index f5a2a86d8bad0ecbe32d21d34e96588a847495f4..010545fff9e39d192624d3f5fdb391aa89541c1c 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
@@ -25,6 +25,7 @@
#include <QDialog>
#include <ArmarXCore/core/IceManager.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index e72461ff8ebe4b7ca106dd59e7dfa8a71258fcbd..ffc177c8b38ff3f9e0569afba0ec2000070f7753 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -31,6 +31,7 @@ set(SLICE_FILES
observers/SpeechObserverInterface.ice
observers/GraspCandidateObserverInterface.ice
+ objectpose/types.ice
objectpose/ObjectPoseObserver.ice
objectpose/ObjectPoseProvider.ice
@@ -80,14 +81,15 @@ 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
components/ViewSelectionInterface.ice
+ components/CartesianPositionControlInterface.ice
+ components/NaturalIKInterface.ice
+
visualization/DebugDrawerInterface.ice
visualization/DebugDrawerToArViz.ice
@@ -97,6 +99,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/aron.ice b/source/RobotAPI/interface/aron.ice
index 5ad2c13812ee9ddf3e29a39cfa2f82a51669540f..3dbe06245692e4392a05e9d2b9997dfe1dac09bf 100644
--- a/source/RobotAPI/interface/aron.ice
+++ b/source/RobotAPI/interface/aron.ice
@@ -16,10 +16,12 @@ module aron
sequence<AronKeyValuePair> AronKeyValueList;
sequence<AronData> AronDataList;
sequence<byte> AronBlobValue;
+ sequence<int> AronIntSeq;
- class AronList extends AronData { AronDataList elements; };
- class AronObject extends AronData { AronKeyValueList elements; };
+ class AronList extends AronData { AronDataList elements; };
+ class AronDict extends AronData { AronKeyValueList elements; };
class AronNull extends AronData { };
+ class AronNDArray extends AronData { AronIntSeq dimensions; int itemSize; string type; AronBlobValue data; };
#define HANDLE_TYPE(cppType, upperType) \
class Aron##upperType extends AronData { cppType value; };
@@ -30,36 +32,10 @@ module aron
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
+
#undef HANDLE_TYPE
- //class AronString extends AronData { string value; };
- //class AronInt extends AronData { int value; };
- //class AronLong extends AronData { long value; };
- //class AronFloat extends AronData { float value; };
- //class AronDouble extends AronData { double value; };
- //class AronBool extends AronData { bool value; };
- //class AronBlob extends AronData { AronBlobValue value; };
- //class AronNull extends AronData { };
- //
- //class AronVector2f extends AronData { Eigen::Vector2f value; };
- //class AronVector3f extends AronData { Eigen::Vector3f value; };
- //class AronVector6f extends AronData { Eigen::Vector6f value; };
- //class AronVectorXf extends AronData { Eigen::VectorXf value; };
- //
- //class AronMatrix3f extends AronData { Eigen::Matrix3f value; };
- //class AronMatrix4f extends AronData { Eigen::Matrix4f value; };
- //class AronMatrixXf extends AronData { Eigen::MatrixXf value; };
- //class AronQuaternionf extends AronData { Eigen::Quaternionf value; };
module types
{
@@ -73,7 +49,7 @@ module aron
class ListType extends AbstractType {
AbstractType childtype;
};
- class ObjectType extends AbstractType {
+ class DictType extends AbstractType {
AbstractType childtype;
};
diff --git a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..13741869c90df9117d6f326036ae92b6e6a24ce0
--- /dev/null
+++ b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
@@ -0,0 +1,42 @@
+/*
+ * 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 Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+//#include <RobotAPI/interface/aron.h>
+
+module armarx
+{
+ interface CartesianPositionControlInterface
+ {
+ void setEnabled(string side, bool enabled);
+ void setTarget(string side, Eigen::Matrix4f target, bool setOrientation);
+ void setFTLimit(string side, float maxForce, float maxTorque);
+ void setCurrentFTAsOffset(string side);
+ void resetFTOffset(string side);
+ int getFTTresholdExceededCounter(string side);
+ void emulateFTTresholdExceeded(string side);
+
+ };
+};
diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a5627f61ef822deaa31e59eb065a2032a877ddcd
--- /dev/null
+++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice
@@ -0,0 +1,41 @@
+/*
+ * 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 Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/aron.ice>
+
+module armarx
+{
+ struct NaturalIKResult
+ {
+ bool reached;
+ Ice::FloatSeq jointValues;
+ };
+
+ interface NaturalIKInterface
+ {
+ NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::AronDict args);
+ };
+};
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..42ed92db3ba81fe2a0255bab1e9a98c9f386991a 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,14 @@ 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..d21a3c55b7574a459f0be3e2577870cf7640f5e3
--- /dev/null
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -0,0 +1,53 @@
+/*
+ * 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);
+
+ void wait_unitl_obstacles_are_ready();
+ };
+};
+
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();
+
+ };
+
+};
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 920395731ad73633b7d82d6a0a99ded551a3c5fe..7a8060dbabbb206be75243df6f579b90a203993d 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -26,6 +26,8 @@
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
+#include <RobotAPI/interface/objectpose/types.ice>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.ice>
@@ -34,33 +36,10 @@ module armarx
module objpose
{
- struct ObjectPose
- {
- ObjectTypeEnum objectType = AnyObject;
- ObjectID objectID;
-
- PoseBase objectPoseRobot;
- PoseBase objectPoseGlobal;
- PoseBase objectPoseOriginal;
- string objectPoseOriginalFrame;
-
- StringFloatDictionary robotConfig;
- PoseBase robotPose;
-
- /// Confidence in [0, 1] (1 = full, 0 = none).
- float confidence = 0;
- /// Source timestamp.
- long timestampMicroSeconds = -1;
-
- string providerName;
- };
- sequence<ObjectPose> ObjectPoseSeq;
-
-
interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
{
- ObjectPoseSeq getObjectPoses();
- ObjectPoseSeq getObjectPosesByProvider(string providerName);
+ data::ObjectPoseSeq getObjectPoses();
+ data::ObjectPoseSeq getObjectPosesByProvider(string providerName);
void requestObjects(ObjectIDSeq objectIDs, long relativeTimeoutMS);
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 487d8c983680c549235b779226bbc39064ca7265..4dcfb2d2f699107a2fbf95a4cf0baf996726a33b 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -24,9 +24,8 @@
#pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
-#include <ArmarXCore/interface/observers/RequestableService.ice>
+
+#include <RobotAPI/interface/objectpose/types.ice>
module armarx
@@ -34,37 +33,6 @@ module armarx
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
- enum ObjectTypeEnum
- {
- AnyObject, KnownObject, UnknownObject
- };
-
- struct ObjectID
- {
- string project; ///< e.g. "KIT", "YCB", "SecondHands", ...
- string name; ///< e.g. "Amicelli", "001_chips_can", ...
- };
- sequence<ObjectID> ObjectIDSeq;
-
- struct ProvidedObjectPose
- {
- ObjectTypeEnum objectType = AnyObject;
- ObjectID objectID;
-
- /// Pose in `objectPoseFrame`.
- PoseBase objectPose;
- string objectPoseFrame;
-
- /// Confidence in [0, 1] (1 = full, 0 = none).
- float confidence = 0;
- /// Source timestamp.
- long timestampMicroSeconds = -1;
-
- string providerName;
- };
- sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
-
-
struct ProviderInfo
{
ObjectTypeEnum objectType = AnyObject;
@@ -77,7 +45,7 @@ module armarx
{
/// Signal that a new provider is now available (and ready for `getProviderInfo()`.
void reportProviderAvailable(string providerName);
- void reportObjectPoses(string providerName, ProvidedObjectPoseSeq candidates);
+ void reportObjectPoses(string providerName, data::ProvidedObjectPoseSeq candidates);
};
interface ObjectPoseProvider
diff --git a/source/RobotAPI/interface/objectpose/types.ice b/source/RobotAPI/interface/objectpose/types.ice
new file mode 100644
index 0000000000000000000000000000000000000000..eaf7adf779b0ac6d51c952f343f08d75bd87d93c
--- /dev/null
+++ b/source/RobotAPI/interface/objectpose/types.ice
@@ -0,0 +1,121 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Rainer Kartmann
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+
+
+module armarx
+{
+ // A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
+ module objpose
+ {
+ enum ObjectTypeEnum
+ {
+ AnyObject, KnownObject, UnknownObject
+ };
+
+ struct ObjectID
+ {
+ string dataset; ///< e.g. "KIT", "YCB", "SecondHands", ...
+ string name; ///< e.g. "Amicelli", "001_chips_can", ...
+ };
+ sequence<ObjectID> ObjectIDSeq;
+
+
+ struct AABB
+ {
+ Vector3Base center;
+ Vector3Base extents;
+ };
+
+ struct Box
+ {
+ Vector3Base position;
+ QuaternionBase orientation;
+ Vector3Base extents;
+ };
+
+
+ module data
+ {
+ /// An object pose provided by an ObjectPoseProvider.
+ struct ProvidedObjectPose
+ {
+ /// Name of the providing component.
+ string providerName;
+ /// Known or unknown object.
+ ObjectTypeEnum objectType = AnyObject;
+
+ /// The object ID, i.e. dataset and name.
+ ObjectID objectID;
+
+ /// Pose in `objectPoseFrame`.
+ PoseBase objectPose;
+ string objectPoseFrame;
+
+ /// Confidence in [0, 1] (1 = full, 0 = none).
+ float confidence = 0;
+ /// Source timestamp.
+ long timestampMicroSeconds = -1;
+
+ /// Object bounding box in object's local coordinate frame.
+ Box localOOBB;
+ };
+ sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
+
+
+ /// An object pose as stored by the ObjectPoseObserver.
+ struct ObjectPose
+ {
+ /// Name of the providing component.
+ string providerName;
+ /// Known or unknown object.
+ ObjectTypeEnum objectType = AnyObject;
+
+ /// The object ID, i.e. dataset and name.
+ ObjectID objectID;
+
+ PoseBase objectPoseRobot;
+ PoseBase objectPoseGlobal;
+ PoseBase objectPoseOriginal;
+ string objectPoseOriginalFrame;
+
+ StringFloatDictionary robotConfig;
+ PoseBase robotPose;
+
+ /// Confidence in [0, 1] (1 = full, 0 = none).
+ float confidence = 0;
+ /// Source timestamp.
+ long timestampMicroSeconds = -1;
+
+ /// Object bounding box in object's local coordinate frame.
+ Box localOOBB;
+ };
+ sequence<ObjectPose> ObjectPoseSeq;
+ }
+
+ };
+};
+
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 8749be775e87d64a4538fcfae4e2e1079e478612..ac4da1da07d1ac2c75526a2f98c32c547aa3d88d 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -62,6 +62,7 @@ module armarx
ApertureType preshape = AnyAperture;
ApproachType approach = AnyApproach;
string graspTrajectoryName;
+ //string graspTrajectoryPackage;
};
class GraspCandidateReachabilityInfo
{
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 8f3237b5312f56a01c71acc49d3bd7bbad401337..361cf690a3498a2a9ffcb95527ef27005c54fe86 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -187,6 +187,7 @@ module armarx
{
//state
["cpp:const"] idempotent bool isRunning();
+ ["cpp:const"] idempotent bool isSimulation();
};
interface RobotUnitLoggingInterface
{
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
index ab2ad3915589358848b643d624dae4270eb035a8..0bbbf7011e67bcb59c3e1dacbc0e5fe5541f1221 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <boost/smart_ptr/shared_ptr.hpp>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
#include <RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h>
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
index f653b4205b8c3e2e4af3ff3a1472d65e90bac3a8..3e54ff4f9b66febc9f1403e62fa194262850a275 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
@@ -35,6 +35,7 @@ armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if (SOEM_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${SOEM_INCLUDE_DIR})
+ target_link_libraries("${LIB_NAME}" PUBLIC ${SOEM_LIBRARIES})
endif()
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index 41701aaf3f7953445affcfd0292a581120637905..ad1a33863adfa1148400a2166182790a64c6a53d 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -7,6 +7,8 @@
#include <ctime>
#include <chrono>
#include <thread>
+#include <iomanip>
+
#include <ArmarXCore/core/logging/Logging.h>
//#include <Armar6RT/units/Armar6Unit/FunctionalDevices/Joint.h>
//#include "Armar6RT/units/Armar6Unit/BusSlaves/SensorBoard.h"
@@ -1387,7 +1389,7 @@ bool EtherCAT::generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex,
ARMARX_WARNING << "Bus is not running no write";
return false;
}
- ScopedLock lock(etherCatMutex);
+ std::unique_lock lock(etherCatMutex);
IceUtil::Time start = IceUtil::Time::now();
int workCount = ec_SDOwrite(slave, index, subindex, (boolean) ca, buflen, buf, SDO_WRITE_TIMEOUT);
IceUtil::Time end = IceUtil::Time::now();
@@ -1431,7 +1433,7 @@ bool EtherCAT::generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex,
{
return false;
}
- ScopedLock lock(etherCatMutex);
+ std::unique_lock lock(etherCatMutex);
IceUtil::Time start = IceUtil::Time::now();
int workCount = ec_SDOread(slave, index, subindex, (boolean) ca, &buflen, buf, SDO_READ_TIMEOUT);
IceUtil::Time end = IceUtil::Time::now();
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
index e9bc549d71890e6345cd2ef8706b0d338baafcc6..f103df995c10cb2210071b2b58c8a368231d06d9 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
@@ -3,8 +3,6 @@
#include "AbstractFunctionalDevice.h"
-#include <ArmarXCore/core/system/Synchronization.h>
-
#include <IceUtil/Time.h>
#include <iostream>
@@ -13,6 +11,7 @@
#include <memory>
#include <sstream>
#include <atomic>
+#include <mutex>
/**
@@ -236,7 +235,7 @@ namespace armarx
RobotUnit* mainUnitPtr;
std::string ifname;
- mutable Mutex etherCatMutex;
+ mutable std::mutex etherCatMutex;
bool emergencyStopWasActivated = false;
IceUtil::Time busUpdateLastUpdateTime;
IceUtil::Time ermergencyStopRecoverStartpoint;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index c4dd0f79ab5d8b0b953f7933c2d8e9fcceddd89f..0fa7cc0b918228cc21398ca5ccd4825b2c78cfdb 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -40,6 +40,7 @@
#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
@@ -50,8 +51,9 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <sched.h>
#include <sys/mman.h>
-#include <syscall.h>
#include <sys/stat.h>
+#include <fcntl.h>
+#include <syscall.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
#include <RobotAPI/libraries/core/Pose.h>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5a3e68a75af25b715267a830dff6f726a250a93b
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::ArmarXObjects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ArmarXObjects.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h
new file mode 100644
index 0000000000000000000000000000000000000000..54315bcc8ebc727fdf55a6595a0683d6ccd1fea0
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ArmarXObjects.h
@@ -0,0 +1,31 @@
+/*
+ * 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::ArmarXObjects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "ObjectFinder.h"
+#include "ObjectInfo.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c819dd3e7e82293c031e5c6d19267cb9a2b1c533
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -0,0 +1,35 @@
+set(LIB_NAME ${PROJECT_NAME}ArmarXObjects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCoreInterfaces ArmarXCore
+)
+
+set(LIB_FILES
+ ArmarXObjects.cpp
+
+ ObjectFinder.cpp
+ ObjectInfo.cpp
+)
+set(LIB_HEADERS
+ ArmarXObjects.h
+
+ ObjectFinder.h
+ ObjectInfo.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#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(ArmarXObjects PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7a74d925c6fc005cd506fff1f5a28558106c752
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -0,0 +1,180 @@
+#include "ObjectFinder.h"
+
+#include <SimoxUtility/filesystem/list_directory.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
+
+namespace armarx
+{
+ namespace fs = std::filesystem;
+
+
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+ {
+ Logging::setTag("ObjectFinder");
+ }
+
+ void ObjectFinder::init() const
+ {
+ if (packageDataDir.empty())
+ {
+ CMakePackageFinder packageFinder(packageName);
+ packageDataDir = packageFinder.getDataDir();
+ if (packageDataDir.empty())
+ {
+ ARMARX_WARNING << "Could not find package '" << packageName << "'.";
+ throw LocalException() << "Could not find package '" << packageName << "'.";
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
+ }
+ }
+ }
+
+
+ std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
+ {
+ init();
+ if (!dataset.empty())
+ {
+ return ObjectInfo(packageName, packageDataDir, dataset, name);
+ }
+ // Search for object in datasets.
+ const auto& datasets = getDatasets();
+ for (const path& dataset : datasets)
+ {
+ if (fs::is_directory(_rootDirAbs() / dataset / name))
+ {
+ return ObjectInfo(packageName, packageDataDir, dataset, name);
+ }
+ }
+
+ std::stringstream ss;
+ ss << "Did not find object '" << name << "' in any of these datasets:\n";
+ for (const path& dataset : datasets)
+ {
+ ss << "- " << dataset << "\n";
+ }
+ ss << "Objects root directory: " << _rootDirAbs();
+ ARMARX_WARNING << ss.str();
+
+ return std::nullopt;
+ }
+
+ std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID) const
+ {
+ init();
+ if (nameOrID.find("/") != nameOrID.npos)
+ {
+ const std::vector<std::string> split = armarx::split(nameOrID, "/", true);
+ ARMARX_CHECK_EQUAL(split.size(), 2) << "Expected ID of format 'Dataset/Name', but got: '" << nameOrID
+ << "' (too many '/').";
+ return findObject(split[0], split[1]);
+ }
+ else
+ {
+ return findObject("", nameOrID);
+ }
+ }
+
+ std::vector<std::string> ObjectFinder::getDatasets() const
+ {
+ // init(); // Done by called methods.
+ std::vector<std::string> datasets;
+ for (const auto& dir : getDatasetDirectories())
+ {
+ datasets.push_back(dir.filename());
+ }
+ return datasets;
+ }
+
+ std::vector<ObjectFinder::path> ObjectFinder::getDatasetDirectories() const
+ {
+ init();
+ const bool local = false;
+ std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
+ std::vector<path> datasetDirs;
+ for (const auto& p : dirs)
+ {
+ if (std::filesystem::is_directory(p))
+ {
+ datasetDirs.push_back(p);
+ }
+ }
+ return datasetDirs;
+ }
+
+ std::vector<ObjectInfo> ObjectFinder::findAllObjects(bool checkPaths) const
+ {
+ init();
+ const bool local = true;
+ std::vector<ObjectInfo> objects;
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
+ {
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
+ {
+ std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
+ for (const auto& o : dataset)
+ {
+ objects.push_back(o);
+ }
+ }
+ }
+ return objects;
+ }
+
+ std::map<std::string, std::vector<ObjectInfo>>
+ ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
+ {
+ // init(); // Done by called methods.
+ std::map<std::string, std::vector<ObjectInfo>> objects;
+ for (const std::string& dataset : getDatasets())
+ {
+ objects[dataset] = findAllObjectsOfDataset(dataset, checkPaths);
+ }
+ return objects;
+ }
+
+ std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ {
+ init();
+ path datasetDir = _rootDirAbs() / dataset;
+ if (!fs::is_directory(datasetDir))
+ {
+ ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
+ << datasetDir;
+ return {};
+ }
+
+ std::vector<ObjectInfo> objects;
+ const bool local = true;
+ for (const path& dir : simox::fs::list_directory(datasetDir, local))
+ {
+ if (fs::is_directory(datasetDir / dir))
+ {
+ ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ if (!checkPaths || object.checkPaths())
+ {
+ objects.push_back(object);
+ }
+ }
+ }
+ return objects;
+ }
+
+ ObjectFinder::path ObjectFinder::_rootDirAbs() const
+ {
+ return packageDataDir / packageName;
+ }
+
+ ObjectFinder::path ObjectFinder::_rootDirRel() const
+ {
+ return packageName;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7f3170c1954e810a077c88c0ea2f3aa1cc9f898
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <filesystem>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "ObjectInfo.h"
+
+
+namespace armarx
+{
+ /**
+ * @brief Used to find objects in the ArmarX objects repository [1].
+ *
+ * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
+ */
+ class ObjectFinder : Logging
+ {
+ public:
+ using path = std::filesystem::path;
+
+ public:
+
+ ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
+
+ std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
+ std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
+
+
+ std::vector<std::string> getDatasets() const;
+ std::vector<path> getDatasetDirectories() const;
+
+ std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
+ std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
+ std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
+
+
+ private:
+
+ void init() const;
+
+ path _rootDirAbs() const;
+ path _rootDirRel() const;
+
+
+ private:
+
+ /// Name of package containing the object models (ArmarXObjects by default).
+ mutable std::string packageName;
+
+ /// Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
+ mutable path packageDataDir;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..feaf2f203aca7445081f2f91e3d511a7b5bbdbd5
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -0,0 +1,146 @@
+#include "ObjectInfo.h"
+
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx
+{
+ namespace fs = std::filesystem;
+
+
+ ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
+ const std::string& dataset, const std::string& name) :
+ _packageName(packageName), _packageDataDir(packageDataDir), _dataset(dataset), _name(name)
+ {
+ }
+
+ std::string ObjectInfo::package() const
+ {
+ return _packageName;
+ }
+
+ std::string ObjectInfo::dataset() const
+ {
+ return _dataset;
+ }
+
+ std::string ObjectInfo::name() const
+ {
+ return _name;
+ }
+
+ std::string ObjectInfo::id() const
+ {
+ return _dataset + "/" + _name;
+ }
+
+ ObjectInfo::path ObjectInfo::objectDirectory() const
+ {
+ return path(_packageName) / _dataset / _name;
+ }
+
+ PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
+ {
+ std::string extension = _extension;
+ if (extension.at(0) != '.')
+ {
+ extension = "." + extension;
+ }
+ std::string filename = _name + suffix + extension;
+
+ PackageFileLocation loc;
+ loc.package = _packageName;
+ loc.relativePath = objectDirectory() / filename;
+ loc.absolutePath = _packageDataDir / loc.relativePath;
+ return loc;
+ }
+
+ PackageFileLocation ObjectInfo::simoxXML() const
+ {
+ return file(".xml");
+ }
+
+ PackageFileLocation ObjectInfo::wavefrontObj() const
+ {
+ return file(".obj");
+ }
+
+ PackageFileLocation ObjectInfo::boundingBoxJson() const
+ {
+ return file(".json", "_bb");
+ }
+
+ simox::AxisAlignedBoundingBox ObjectInfo::aabb() const
+ {
+ nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+ nlohmann::json jaabb = j.at("aabb");
+
+ auto center = jaabb.at("center").get<Eigen::Vector3f>();
+ auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
+ auto min = jaabb.at("min").get<Eigen::Vector3f>();
+ auto max = jaabb.at("max").get<Eigen::Vector3f>();
+
+ simox::AxisAlignedBoundingBox aabb(min, max);
+
+ ARMARX_CHECK(aabb.center().isApprox(center)) << aabb.center().transpose() << "\n" << center.transpose();
+ ARMARX_CHECK(aabb.extents().isApprox(extents)) << aabb.extents().transpose() << "\n" << extents.transpose();
+ ARMARX_CHECK(aabb.min().isApprox(min)) << aabb.min().transpose() << "\n" << min.transpose();
+ ARMARX_CHECK(aabb.max().isApprox(max)) << aabb.max().transpose() << "\n" << max.transpose();
+
+ return aabb;
+ }
+
+ simox::OrientedBox<float> ObjectInfo::oobb() const
+ {
+ nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+ nlohmann::json joobb = j.at("oobb");
+ auto pos = joobb.at("pos").get<Eigen::Vector3f>();
+ auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
+ auto extents = joobb.at("extents").get<Eigen::Vector3f>();
+
+ Eigen::Vector3f corner = pos - ori * extents / 2;
+
+ simox::OrientedBox<float> oobb(corner,
+ ori.col(0) * extents(0),
+ ori.col(1) * extents(1),
+ ori.col(2) * extents(2));
+
+ ARMARX_CHECK(oobb.center().isApprox(pos)) << oobb.center().transpose() << "\n" << pos.transpose();
+ ARMARX_CHECK(oobb.rotation().isApprox(ori)) << oobb.rotation() << "\n" << ori;
+ ARMARX_CHECK(oobb.dimensions().isApprox(extents)) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+ return oobb;
+ }
+
+ bool ObjectInfo::checkPaths() const
+ {
+ namespace fs = std::filesystem;
+ bool result = true;
+
+ if (!fs::is_regular_file(simoxXML().absolutePath))
+ {
+ ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ result = false;
+ }
+ if (!fs::is_regular_file(wavefrontObj().absolutePath))
+ {
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ result = false;
+ }
+
+ return result;
+ }
+
+}
+
+namespace armarx
+{
+ std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
+ {
+ return os << "'" << rhs.id() << "'";
+ }
+}
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
similarity index 51%
rename from source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
rename to source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 548fb3a82b95d27244490e7227ccfcc7a1d04023..0fd1ef03216ba8fdbb620462306b9779baeabde9 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -1,65 +1,35 @@
#pragma once
#include <filesystem>
+#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
+namespace simox
+{
+ // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+ struct AxisAlignedBoundingBox;
+ // #include <SimoxUtility/shapes/OrientedBox.h>
+ template<class FloatT> class OrientedBox;
+}
namespace armarx
{
- class ObjectInfo;
-
-
- class ObjectFinder : Logging
- {
- public:
- using path = std::filesystem::path;
-
- public:
-
- ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
-
- std::optional<ObjectInfo> findObject(const std::string& project, const std::string& name);
- std::optional<ObjectInfo> findObject(const std::string& nameOrID);
-
-
- std::vector<std::string> getProjects();
- std::vector<path> getProjectDirectories();
-
- std::vector<ObjectInfo> findAllObjects();
- std::vector<ObjectInfo> findAllObjectsOfProject(const std::string& project);
-
-
- private:
-
- void init();
-
- path _rootDirAbs() const;
- path _rootDirRel() const;
-
- private:
-
-
- /// Name of package containing the object models (ArmarXObjects by default).
- std::string packageName;
-
- /// Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
- path packageDataDir;
-
- };
-
-
struct PackageFileLocation
{
/// Name of the ArmarX package.
std::string package;
+ /// Relative to the package's data directory.
std::string relativePath;
+ /// The absolute path (in the host's file system).
std::filesystem::path absolutePath;
};
+ /**
+ * @brief Accessor for the object files.
+ */
class ObjectInfo
{
public:
@@ -68,13 +38,16 @@ namespace armarx
public:
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& project, const std::string& name);
+ const std::string& dataset, const std::string& name);
+
+ virtual ~ObjectInfo() = default;
+
std::string package() const;
- std::string project() const;
+ std::string dataset() const;
std::string name() const;
- /// Return "project/name".
+ /// Return "dataset/name".
std::string id() const;
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
@@ -82,6 +55,12 @@ namespace armarx
PackageFileLocation simoxXML() const;
PackageFileLocation wavefrontObj() const;
+ PackageFileLocation boundingBoxJson() const;
+
+ simox::AxisAlignedBoundingBox aabb() const;
+ simox::OrientedBox<float> oobb() const;
+
+
/**
* @brief Checks the existence of expected files.
* If a file is does not exist, emits a warning returns false.
@@ -100,7 +79,7 @@ namespace armarx
std::string _packageName;
path _packageDataDir;
- std::string _project;
+ std::string _dataset;
std::string _name;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0af92b11dcd764b7ab376f07a5869cb1962e5f4f
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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::ArmarXObjects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ArmarXObjects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArmarXObjects.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2ad25f7ea27f28908cf50d0c950ba4170487fc85
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+
+armarx_add_test(ArmarXObjectsTest ArmarXObjectsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 049d3fd2e11370c975a87557043dd5c40a253029..90a62cd3087bd6e2462865fb26a21c4140e4863c 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,4 +1,5 @@
add_subdirectory(ArmarXEtherCAT)
+add_subdirectory(ArmarXObjects)
add_subdirectory(ControllerUIUtility)
add_subdirectory(core)
add_subdirectory(DebugDrawerConfigWidget)
@@ -12,8 +13,9 @@ add_subdirectory(RobotStatechartHelpers)
add_subdirectory(SimpleJsonLogger)
add_subdirectory(SimpleTrajectory)
add_subdirectory(widgets)
-add_subdirectory(natik)
-add_subdirectory(aron)
add_subdirectory(diffik)
+add_subdirectory(natik)
+
add_subdirectory(armem)
+add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index a040355c292794e446ed5ff6b7c8adf1228f57ba..9cb91dae9d98f8ee798851c37c0d60282a09414c 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -28,6 +28,7 @@
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/MathTools.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/core/Trajectory.h>
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
index 109cf497d8f2becb93953215df58f4070bb38131..87973287030bf4dac0f45a0a0af25c8fb930afe7 100644
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
@@ -21,7 +21,9 @@
*/
#include "DSJointCarryController.h"
+
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
using namespace armarx;
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
index 3cd1070441917ed0fc0fbbed77aecd9f63b5df3b..b27e2d95809ae52f73c5c6e341f123bef6e6b941 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -21,7 +21,9 @@
*/
#include "DSRTBimanualController.h"
+
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
index 42f3555d878961c5e5082d8e98ec5f21f18c2a32..53d89208654b540280009cc687fcc47194e2ee71 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
@@ -21,7 +21,9 @@
*/
#include "DSRTController.h"
+
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 1292e78fa38178ff7f555aae561465bbb4e27ac6..6567872ebd97ab3016b3af816f2048d473040464 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -13,11 +13,27 @@ set(LIB_FILES
RobotStateComponentPlugin.cpp
DebugDrawerHelperComponentPlugin.cpp
ArVizComponentPlugin.cpp
+ GraspCandidateObserverComponentPlugin.cpp
+ PlatformUnitComponentPlugin.cpp
+ RobotUnitComponentPlugin.cpp
+ RobotUnitObserverComponentPlugin.cpp
+ KinematicUnitComponentPlugin.cpp
+ TrajectoryPlayerComponentPlugin.cpp
+ CartesianPositionControlComponentPlugin.cpp
+ NaturalIKComponentPlugin.cpp
)
set(LIB_HEADERS
RobotStateComponentPlugin.h
DebugDrawerHelperComponentPlugin.h
ArVizComponentPlugin.h
+ GraspCandidateObserverComponentPlugin.h
+ PlatformUnitComponentPlugin.h
+ RobotUnitComponentPlugin.h
+ RobotUnitObserverComponentPlugin.h
+ KinematicUnitComponentPlugin.h
+ TrajectoryPlayerComponentPlugin.h
+ CartesianPositionControlComponentPlugin.h
+ NaturalIKComponentPlugin.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b403e45ccfea923d1ead7b8e73fa5e7dbcf0de2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
@@ -0,0 +1,53 @@
+#include "CartesianPositionControlComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void CartesianPositionControlComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void CartesianPositionControlComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_cartesianPositionControl, PROPERTY_NAME);
+ }
+
+ void CartesianPositionControlComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the CartesianPositionControl");
+ }
+ }
+
+ CartesianPositionControlInterfacePrx CartesianPositionControlComponentPlugin::getCartesianPositionControl()
+ {
+ return _cartesianPositionControl;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ CartesianPositionControlComponentPluginUser::CartesianPositionControlComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ CartesianPositionControlInterfacePrx CartesianPositionControlComponentPluginUser::getCartesianPositionControl()
+ {
+ return plugin->getCartesianPositionControl();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d53885cc44d3d0d42f4357c6c7b7cf0cfa7db5f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class CartesianPositionControlComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ CartesianPositionControlInterfacePrx getCartesianPositionControl();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "CartesianPositionControlName";
+ CartesianPositionControlInterfacePrx _cartesianPositionControl;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use CartesianPositionControl.
+ */
+ class CartesianPositionControlComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ CartesianPositionControlComponentPluginUser();
+ CartesianPositionControlInterfacePrx getCartesianPositionControl();
+
+ private:
+ armarx::plugins::CartesianPositionControlComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using CartesianPositionControlComponentPluginUser = armarx::CartesianPositionControlComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e99ccfb4eb2994373380b03415f0e96b39b0e14
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "GraspCandidateObserverComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void GraspCandidateObserverComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void GraspCandidateObserverComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_graspCandidateObserver, PROPERTY_NAME);
+ }
+
+ void GraspCandidateObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "GraspCandidateObserver",
+ "Name of the GraspCandidateObserver");
+ }
+ }
+
+ grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPlugin::getGraspCandidateObserver()
+ {
+ return _graspCandidateObserver;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ GraspCandidateObserverComponentPluginUser::GraspCandidateObserverComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver()
+ {
+ return plugin->getGraspCandidateObserver();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..e15688a68568b1591ff5aeedfbb6d20337b002a2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class GraspCandidateObserverComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "GraspCandidateObserverName";
+ grasping::GraspCandidateObserverInterfacePrx _graspCandidateObserver;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use GraspCandidateObserver.
+ */
+ class GraspCandidateObserverComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ GraspCandidateObserverComponentPluginUser();
+ grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver();
+
+ private:
+ armarx::plugins::GraspCandidateObserverComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using GraspCandidateObserverComponentPluginUser = armarx::GraspCandidateObserverComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba8ba16d24d40bbb2312690ef87b4dd9f5619cd5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
@@ -0,0 +1,56 @@
+#include "KinematicUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void KinematicUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void KinematicUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_kinematicUnit, PROPERTY_NAME);
+ }
+
+ void KinematicUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the KinematicUnit");
+ }
+ }
+
+ KinematicUnitInterfacePrx KinematicUnitComponentPlugin::getKinematicUnit()
+ {
+ return _kinematicUnit;
+ }
+
+
+ }
+
+}
+
+namespace armarx
+{
+
+ KinematicUnitComponentPluginUser::KinematicUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ KinematicUnitInterfacePrx KinematicUnitComponentPluginUser::getKinematicUnit()
+ {
+ return plugin->getKinematicUnit();
+ }
+
+
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..e3b4062b59500f26314a6ccdb6e5480dd7d34a2b
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class KinematicUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ KinematicUnitInterfacePrx getKinematicUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "KinematicUnitName";
+ KinematicUnitInterfacePrx _kinematicUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use KinematicUnit.
+ */
+ class KinematicUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ KinematicUnitComponentPluginUser();
+ KinematicUnitInterfacePrx getKinematicUnit();
+
+ private:
+ armarx::plugins::KinematicUnitComponentPlugin* plugin = nullptr;
+ };
+
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using KinematicUnitComponentPluginUser = armarx::KinematicUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..008d497652fbd0888ef82eb1e5b99650bda11a25
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "NaturalIKComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void NaturalIKComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void NaturalIKComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_naturalIK, PROPERTY_NAME);
+ }
+
+ void NaturalIKComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "NaturalIK",
+ "Name of the NaturalIK");
+ }
+ }
+
+ NaturalIKInterfacePrx NaturalIKComponentPlugin::getNaturalIK()
+ {
+ return _naturalIK;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ NaturalIKComponentPluginUser::NaturalIKComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ NaturalIKInterfacePrx NaturalIKComponentPluginUser::getNaturalIK()
+ {
+ return plugin->getNaturalIK();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..0532416d50ae164baf5645d0cbc30ff50f3e9f48
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/NaturalIKInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class NaturalIKComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ NaturalIKInterfacePrx getNaturalIK();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "NaturalIKName";
+ NaturalIKInterfacePrx _naturalIK;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use NaturalIK.
+ */
+ class NaturalIKComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ NaturalIKComponentPluginUser();
+ NaturalIKInterfacePrx getNaturalIK();
+
+ private:
+ armarx::plugins::NaturalIKComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using NaturalIKComponentPluginUser = armarx::NaturalIKComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f40b6cd34a5f5a8f028b714cc7a1517d590f2b3c
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
@@ -0,0 +1,53 @@
+#include "PlatformUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void PlatformUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void PlatformUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_platformUnit, PROPERTY_NAME);
+ }
+
+ void PlatformUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the PlatformUnit");
+ }
+ }
+
+ PlatformUnitInterfacePrx PlatformUnitComponentPlugin::getPlatformUnit()
+ {
+ return _platformUnit;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ PlatformUnitComponentPluginUser::PlatformUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ PlatformUnitInterfacePrx PlatformUnitComponentPluginUser::getPlatformUnit()
+ {
+ return plugin->getPlatformUnit();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..d2cec0f45b698f813d12a10920a122efbb63d00b
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class PlatformUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ PlatformUnitInterfacePrx getPlatformUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "PlatformUnitName";
+ PlatformUnitInterfacePrx _platformUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use PlatformUnit.
+ */
+ class PlatformUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ PlatformUnitComponentPluginUser();
+ PlatformUnitInterfacePrx getPlatformUnit();
+
+ private:
+ armarx::plugins::PlatformUnitComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using PlatformUnitComponentPluginUser = armarx::PlatformUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index e41d1f2aca5bfec043059be72033e9f35d5706a6..3f28f11c839e988564882fe679eaec47b74cb649 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -130,6 +130,11 @@ namespace armarx::plugins
}
}
+ RobotStateComponentInterfacePrx RobotStateComponentPlugin::getRobotStateComponent()
+ {
+ return _robotStateComponent;
+ }
+
bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return RemoteRobot::synchronizeLocalClone(robot, _robotStateComponent);
@@ -194,9 +199,14 @@ namespace armarx::plugins
void RobotStateComponentPlugin::preOnInitComponent()
{
+ if (!_robotStateComponent && _robotStateComponentName.empty())
+ {
+ parent<Component>().getProperty(_robotStateComponentName, makePropertyName(_propertyName));
+ }
+
if (!_robotStateComponent)
{
- parent<Component>().usingProxyFromProperty(makePropertyName(_propertyName));
+ parent<Component>().usingProxy(_robotStateComponentName);
}
}
@@ -204,7 +214,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().getProxyFromProperty(_robotStateComponent, makePropertyName(_propertyName));
+ parent<Component>().getProxy(_robotStateComponent, _robotStateComponentName);
}
}
@@ -237,6 +247,18 @@ namespace armarx::plugins
ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot ";
return SimpleDiffIK::CalculateReachability(targets, initialJV, rns, node, params);
}
+
+ void RobotStateComponentPlugin::setRobotStateComponentName(const std::string& name)
+ {
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotStateComponentName);
+ _robotStateComponentName = name;
+ }
+
+ const std::string& RobotStateComponentPlugin::getRobotStateComponentName() const
+ {
+ return _robotStateComponentName;
+ }
}
namespace armarx
@@ -300,6 +322,11 @@ namespace armarx
getRobotStateComponentPlugin().setRobotRNSAndNode(id, rnsName, nodeName);
}
+ RobotStateComponentInterfacePrx RobotStateComponentPluginUser::getRobotStateComponent()
+ {
+ return getRobotStateComponentPlugin().getRobotStateComponent();
+ }
+
bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(robot);
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index fbbbae26afc6571885dac07c9ef064c89b3698f2..25ca57fffc2155f8fc88114de62c0836b206c02e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -22,13 +22,14 @@
#pragma once
+#include <mutex>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/core/RobotState.h>
-
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx::plugins
@@ -52,6 +53,8 @@ namespace armarx::plugins
struct RobotData;
using ComponentPlugin::ComponentPlugin;
+ void setRobotStateComponentName(const std::string& name);
+ const std::string& getRobotStateComponentName() const;
//get / add
public:
bool hasRobot(const std::string& id) const;
@@ -82,6 +85,8 @@ namespace armarx::plugins
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+
+ RobotStateComponentInterfacePrx getRobotStateComponent();
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
@@ -134,6 +139,7 @@ namespace armarx::plugins
//data
public:
static constexpr auto _propertyName = "RemoteStateComponentName";
+ std::string _robotStateComponentName;
RobotStateComponentInterfacePrx _robotStateComponent;
mutable std::recursive_mutex _robotsMutex;
std::map<std::string, RobotData> _robots;
@@ -186,6 +192,9 @@ namespace armarx
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+
+ RobotStateComponentInterfacePrx getRobotStateComponent();
+
//sync
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e94a395814376eb08b20e45f5d043f21fa6dbb4
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -0,0 +1,64 @@
+#include "RobotUnitComponentPlugin.h"
+
+namespace armarx::plugins
+{
+ void RobotUnitComponentPlugin::preOnInitComponent()
+ {
+ if (_robotUnitName.empty())
+ {
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ }
+ parent<Component>().usingProxy(_robotUnitName);
+ }
+
+ void RobotUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ }
+
+ void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the RobotUnit");
+ }
+ }
+
+ RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit() const
+ {
+ return _robotUnit;
+ }
+
+ void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
+ {
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
+ }
+ const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
+ {
+ return _robotUnitName;
+ }
+}
+
+namespace armarx
+{
+ RobotUnitComponentPluginUser::RobotUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit() const
+ {
+ return plugin->getRobotUnit();
+ }
+
+ plugins::RobotUnitComponentPlugin& RobotUnitComponentPluginUser::getRobotUnitComponentPlugin()
+ {
+ return *plugin;
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3cbf9a3b1e43d262370fc3b6082d07612a9c7cc
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class RobotUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ RobotUnitInterfacePrx getRobotUnit() const;
+
+ void setRobotUnitName(const std::string& name);
+ const std::string& getRobotUnitName() const;
+ private:
+ static constexpr const char* PROPERTY_NAME = "RobotUnitName";
+ RobotUnitInterfacePrx _robotUnit;
+ std::string _robotUnitName;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use RobotUnit.
+ */
+ class RobotUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ RobotUnitComponentPluginUser();
+ RobotUnitInterfacePrx getRobotUnit() const;
+ plugins::RobotUnitComponentPlugin& getRobotUnitComponentPlugin();
+ private:
+ armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using RobotUnitComponentPluginUser = armarx::RobotUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..284533cf16e62834d85ff33e73d36b158943acef
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "RobotUnitObserverComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void RobotUnitObserverComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void RobotUnitObserverComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_robotUnitObserver, PROPERTY_NAME);
+ }
+
+ void RobotUnitObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "RobotUnitObserver",
+ "Name of the RobotUnitObserver");
+ }
+ }
+
+ ObserverInterfacePrx RobotUnitObserverComponentPlugin::getRobotUnitObserver()
+ {
+ return _robotUnitObserver;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ RobotUnitObserverComponentPluginUser::RobotUnitObserverComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ ObserverInterfacePrx RobotUnitObserverComponentPluginUser::getRobotUnitObserver()
+ {
+ return plugin->getRobotUnitObserver();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ebeff910700102871aa6229259e05f2efec866d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class RobotUnitObserverComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ ObserverInterfacePrx getRobotUnitObserver();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "RobotUnitObserverName";
+ ObserverInterfacePrx _robotUnitObserver;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use RobotUnitObserver.
+ */
+ class RobotUnitObserverComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ RobotUnitObserverComponentPluginUser();
+ ObserverInterfacePrx getRobotUnitObserver();
+
+ private:
+ armarx::plugins::RobotUnitObserverComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using RobotUnitObserverComponentPluginUser = armarx::RobotUnitObserverComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1de30ae07c9f8218666121c28350f32c1ca1f5dc
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "TrajectoryPlayerComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void TrajectoryPlayerComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void TrajectoryPlayerComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_trajectoryPlayer, PROPERTY_NAME);
+ }
+
+ void TrajectoryPlayerComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "TrajectoryPlayer",
+ "Name of the TrajectoryPlayer");
+ }
+ }
+
+ TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPlugin::getTrajectoryPlayer()
+ {
+ return _trajectoryPlayer;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ TrajectoryPlayerComponentPluginUser::TrajectoryPlayerComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPluginUser::getTrajectoryPlayer()
+ {
+ return plugin->getTrajectoryPlayer();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fc52df857f7a7225eacb25103dbd7fa16de4af5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class TrajectoryPlayerComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ TrajectoryPlayerInterfacePrx getTrajectoryPlayer();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "TrajectoryPlayerName";
+ TrajectoryPlayerInterfacePrx _trajectoryPlayer;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use TrajectoryPlayer.
+ */
+ class TrajectoryPlayerComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ TrajectoryPlayerComponentPluginUser();
+ TrajectoryPlayerInterfacePrx getTrajectoryPlayer();
+
+ private:
+ armarx::plugins::TrajectoryPlayerComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using TrajectoryPlayerComponentPluginUser = armarx::TrajectoryPlayerComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
index bee6f061d2051bc0307b172ee57d01fc8c209aef..4130b956b90dbae919c281d7ec9eacea13a3880c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
@@ -1,6 +1,7 @@
#include "NJointBimanualForceController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 1baf81f98b87b0ec18f66bf66ca778c7b4c955c1..ff23afd40d30462e240a05e173d122b2f2b21849 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -1,6 +1,7 @@
#include "NJointBimanualObjLevelController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
index 19b87eb3d8c2402ab6aacb1f48026e565e2ba918..630e11b7630c28fa15807a364a30e867fdf881f2 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
@@ -1,5 +1,7 @@
#include "NJointAdaptiveWipingController.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx
{
NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index 730062df308da449c88d308edfc8547fbbf142aa..843a8704145eef3c80b133bd2f1e225e012f5fd9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -1,5 +1,7 @@
#include "NJointAnomalyDetectionAdaptiveWipingController.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx
{
NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController> registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
index cfd77287685f0719fe865819dc5a94eadcb760df..f62e71798edaaac3e00ddd5e9b243f362415cbd6 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
@@ -1,6 +1,7 @@
#include "NJointBimanualCCDMPVelocityController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index 3e57047ea1d09a8b5d469e1f067489ddb5dea3e5..11d0637a53ea10892ddf40f82b3f3cb64b6d61fd 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -1,6 +1,8 @@
#include "NJointJSDMPController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 2742cd9944d5b5bc74cbc6d1a76b344aae51d182..b8e560bb9dd0f341cde6a8ae57bf65ae58ff196a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -1,5 +1,7 @@
#include "NJointPeriodicTSDMPCompliantController.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx
{
NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
index 8d892d2c7ddf40dbef0b3bace0835d6890b848b7..24b98a2ea15d0919be10196f834482d385983dcb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
@@ -1,5 +1,7 @@
#include "NJointPeriodicTSDMPForwardVelController.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx
{
NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController> registrationControllerNJointPeriodicTSDMPForwardVelController("NJointPeriodicTSDMPForwardVelController");
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index b05288e5c1d00dd8d5de429e7b5a3ed2ef5d8dcc..b63554a7dc4ae326a6d1e80af6210581590c6594 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -1,4 +1,7 @@
#include "NJointTSDMPController.h"
+
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 724ad162a736bf3b1e55990381e3ff6fe2fb307c..e284a6c0866fa35d34baa580ede3976b0049afef 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -1,5 +1,7 @@
#include "NJointTaskSpaceImpedanceDMPController.h"
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+
namespace armarx
{
NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController("NJointTaskSpaceImpedanceDMPController");
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 3cfdc7cac8d3b756479dd3166b0caf93bb318b94..becf5e2ec7f6d035fb87b59b6615d04b04ed16f2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -28,6 +28,7 @@
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index b4d16c5e2e3bff8fdbcf2d32469076825306bfff..10cb284d673b1eb358a289a043044271a99552b2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -22,7 +22,11 @@
*/
#include "RobotNameHelper.h"
+
#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
+
+#include <Eigen/Dense>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index bd95565df108c2b30086bb83aae8a2588cb9cc86..eca096cc5e62727236ad8c04e482d7ab4f5cdfec 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -25,15 +25,12 @@
#include <RobotAPI/interface/core/RobotState.h>
-#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
-
-#include <boost/optional.hpp>
-
#include <VirtualRobot/Robot.h>
namespace armarx
{
using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
+ using StatechartProfilePtr = std::shared_ptr<class StatechartProfile>;
class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
{
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp
index b24b7f8dcf085c74f3767df01ab6ee42abebe99f..9c5f9118e3893d412b98fcef044ea37abb9d795b 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/AronNavigator.cpp
@@ -23,7 +23,7 @@
#include "AronNavigator.h"
#include <ArmarXCore/core/exceptions/Exception.h>
-
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
@@ -31,140 +31,66 @@ namespace armarx
{
template <typename Tin, typename Tout>
- Tout cast_and_check(const AronDataPtr& data, const std::string& path, const std::string& type)
+ Tout cast_and_check(const AronDataPtr& data, const AronPath& path, const std::string& type)
{
Tin tdata = Tin::dynamicCast(data);
if (!tdata)
{
- throw LocalException("value cannot be cast to ") << type << ". path: " << path;
+ throw LocalException("value cannot be cast to ") << type << ". path: " << path.toString();
}
return tdata->value;
}
AronNavigator::AronNavigator(const AronDataPtr& data)
- : data(data), path(""), key(""), index(-1)
+ : data(data)
{ }
- AronNavigator::AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index)
- : data(data), path(path), key(key), index(index)
+ AronNavigator::AronNavigator(const AronDataPtr& data, const AronPath& path)
+ : data(data), path(path)
{ }
- AronNavigator AronNavigator::atIndex(size_t index)
+ ListNavigator AronNavigator::asList()
{
AronListPtr list = AronListPtr::dynamicCast(data);
if (!list)
{
- throw LocalException("value is not a list. path: ") << path;
+ throw LocalException("value is not a list. path: ") << path.toString();
}
- return AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index);
+ return ListNavigator(list, path);
}
- AronNavigator AronNavigator::atKey(const std::string& key)
+ DictNavigator AronNavigator::asDict()
{
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (!obj)
- {
- throw LocalException("value is not an object. path: ") << path;
- }
- for (const AronKeyValuePair& pair : obj->elements)
+ AronDictPtr dict = AronDictPtr::dynamicCast(data);
+ if (!dict)
{
- if (pair.key == key)
- {
- return AronNavigator(pair.value, path + "/" + key, key, 0);
- }
+ throw LocalException("value is not a dict. path: ") << path.toString();
}
- throw LocalException("key '") << key << "'not found. path: " << path;
+ return DictNavigator(dict, path);
}
- std::vector<AronNavigator> AronNavigator::children()
- {
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (list)
- {
- std::vector<AronNavigator> result;
- for (size_t i = 0; i < list->elements.size(); i++)
- {
- result.emplace_back(AronNavigator(list->elements.at(i), path + "/" + std::to_string(i), "", i));
- }
- return result;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (obj)
- {
- std::vector<AronNavigator> result;
- for (const AronKeyValuePair& pair : obj->elements)
- {
- result.emplace_back(AronNavigator(pair.value, path + "/" + pair.key, pair.key, -1));
- }
- return result;
- }
- throw LocalException("value is not a list nor an object. path: ") << path;
- }
-
- void AronNavigator::append(const AronDataPtr& value)
+ Eigen::Vector3f AronNavigator::asVector3f()
{
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (!list)
- {
- throw LocalException("value is not a list. path: ") << path;
- }
- list->elements.push_back(value);
+ return Eigen::Vector3f(checkReinterpretNDArray<float>(3));
}
- bool AronNavigator::append(const std::string& key, const AronDataPtr& value)
+ Eigen::Matrix3f AronNavigator::asMatrix3f()
{
- int index = keyIndex(key);
- if (index >= 0)
- {
- return false;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.push_back({key, value});
- return true;
+ return Eigen::Matrix3f(checkReinterpretNDArray<float>(3, 3));
}
- void AronNavigator::set(const std::string& key, const AronDataPtr& value)
+ Eigen::Matrix4f AronNavigator::asMatrix4f()
{
- int index = keyIndex(key);
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.at(index).value = value;
+ return Eigen::Matrix4f(checkReinterpretNDArray<float>(4, 4));
}
- bool AronNavigator::hasKey(const std::string& key)
- {
- return keyIndex(key) >= 0;
- }
- int AronNavigator::keyIndex(const std::string& key)
- {
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- if (!obj)
- {
- throw LocalException("value is not an object. path: ") << path;
- }
- for (size_t i = 0; i < obj->elements.size(); i++)
- {
- if (obj->elements.at(i).key == key)
- {
- return i;
- }
- }
- return -1;
- }
- bool AronNavigator::deleteKey(const std::string& key)
- {
- int index = keyIndex(key);
- if (index < 0)
- {
- return false;
- }
- AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
- obj->elements.erase(obj->elements.begin() + index);
- return true;
- }
+
+
+
#define HANDLE_TYPE(cppType, upperType) \
cppType AronNavigator::as##upperType() \
@@ -174,7 +100,7 @@ namespace armarx
bool AronNavigator::is##upperType() \
{ \
return Aron##upperType##Ptr::dynamicCast(data); \
- } \
+ } /*\
void AronNavigator::append##upperType(const cppType& value) \
{ \
append(new Aron##upperType(value)); \
@@ -182,7 +108,7 @@ namespace armarx
bool AronNavigator::append##upperType(const std::string& key, const cppType& value) \
{ \
return append(key, new Aron##upperType(value)); \
- }
+}*/
HANDLE_TYPE(std::string, String)
HANDLE_TYPE(int, Int)
@@ -190,15 +116,6 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
@@ -209,31 +126,7 @@ namespace armarx
return AronNullPtr::dynamicCast(data);
}
- bool AronNavigator::appendVec(const std::string& key, const std::vector<float>& vec)
- {
- AronListPtr list = new AronList();
- for (float f : vec)
- {
- list->elements.push_back(new AronFloat(f));
- }
- return append(key, list);
- }
- std::vector<float> armarx::aron::AronNavigator::AronNavigator::asVecFloat()
- {
- AronListPtr list = AronListPtr::dynamicCast(data);
- if (!list)
- {
- throw LocalException("value is not a list. path: ") << path;
- }
- std::vector<float> result;
- for (size_t index = 0; index < list->elements.size(); index++)
- {
- float f = AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index).asFloat();
- result.push_back(f);
- }
- return result;
- }
void AronNavigator::writeXml(RapidXmlWriterNode parent)
{
@@ -285,14 +178,211 @@ namespace armarx
}*/
}
+
+
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size)
+ {
+ AronNDArrayPtr arr = AronNDArrayPtr::dynamicCast(data);
+ if (!arr)
+ {
+ throw LocalException("value is not an ndarray. path: ") << path.toString();
+ }
+ if (arr->itemSize != (int)size)
+ {
+ throw LocalException("item size mismatch for ndarray. itemSize: ") << arr->itemSize << ", requested: " << size << ", path: " << path.toString();
+ }
+ return arr;
+ }
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(size);
+ ARMARX_CHECK_W_HINT(arr->dimensions.size() == 1, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString());
+ return arr;
+ }
+
+ AronNDArrayPtr AronNavigator::checkCastToNDArray(size_t size, size_t dim1, size_t dim2)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(size);
+ ARMARX_CHECK_W_HINT(arr->dimensions.size() == 2, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(0) == (int)dim1, path.toString());
+ ARMARX_CHECK_W_HINT(arr->dimensions.at(1) == (int)dim2, path.toString());
+ return arr;
+ }
+
bool AronNavigator::isList()
{
return AronListPtr::dynamicCast(data);
}
- bool AronNavigator::isObject()
+ bool AronNavigator::isDict()
+ {
+ return AronDictPtr::dynamicCast(data);
+ }
+
+ ListNavigator::ListNavigator(const AronListPtr& list, const AronPath& path)
+ : list(list), path(path)
+ {
+ }
+
+ AronNavigator ListNavigator::at(size_t index)
{
- return AronObjectPtr::dynamicCast(data);
+ return AronNavigator(list->elements.at(index), path.appendIndex(index));
}
+ std::vector<AronNavigator> ListNavigator::children()
+ {
+ std::vector<AronNavigator> result;
+ for (size_t i = 0; i < list->elements.size(); i++)
+ {
+ result.emplace_back(AronNavigator(list->elements.at(i), path.appendIndex(i)));
+ }
+ return result;
+ }
+
+ void ListNavigator::append(const AronDataPtr& value)
+ {
+ list->elements.push_back(value);
+ }
+
+ std::vector<AronDataPtr>& ListNavigator::get()
+ {
+ return list->elements;
+ }
+
+
+
+ std::vector<float> ListNavigator::asVecFloat()
+ {
+ std::vector<float> result;
+ for (size_t index = 0; index < list->elements.size(); index++)
+ {
+ float f = AronNavigator(list->elements.at(index), path.appendIndex(index)).asFloat();
+ result.push_back(f);
+ }
+ return result;
+ }
+
+ AronPath AronPath::appendIndex(size_t index) const
+ {
+ AronPath p;
+ p.path = path;
+ p.path.emplace_back(AronPathItem(index));
+ return p;
+ }
+
+ AronPath AronPath::appendKey(const std::string& key) const
+ {
+ AronPath p;
+ p.path = path;
+ p.path.emplace_back(AronPathItem(key));
+ return p;
+ }
+
+ std::string AronPath::toString() const
+ {
+ std::stringstream ss;
+ bool first = true;
+ for (const AronPathItem& i : path)
+ {
+ if (!first)
+ {
+ ss << "/";
+ }
+ first = false;
+ if (i.index >= 0)
+ {
+ ss << i.index;
+ }
+ else
+ {
+ ss << i.key;
+ }
+ }
+ return ss.str();
+ }
+
+ DictNavigator::DictNavigator(const AronDictPtr& dict, const AronPath& path)
+ : dict(dict), path(path)
+ {
+ }
+
+ AronNavigator DictNavigator::at(const std::string& key)
+ {
+ for (const AronKeyValuePair& pair : dict->elements)
+ {
+ if (pair.key == key)
+ {
+ return AronNavigator(pair.value, path.appendKey(key));
+ }
+ }
+ throw LocalException("key '") << key << "'not found. path: " << path.toString();
+ }
+
+ std::vector<AronNavigator> DictNavigator::children()
+ {
+ std::vector<AronNavigator> result;
+ for (const AronKeyValuePair& pair : dict->elements)
+ {
+ result.emplace_back(AronNavigator(pair.value, path.appendKey(pair.key)));
+ }
+ return result;
+ }
+
+ bool DictNavigator::append(const std::string& key, const AronDataPtr& value)
+ {
+ int index = indexOf(key);
+ if (index >= 0)
+ {
+ return false;
+ }
+ dict->elements.push_back({key, value});
+ return true;
+ }
+
+ bool DictNavigator::appendVec(const std::string& key, const std::vector<float>& vec)
+ {
+ AronListPtr list = new AronList();
+ for (float f : vec)
+ {
+ list->elements.push_back(new AronFloat(f));
+ }
+ return append(key, list);
+ }
+
+ void DictNavigator::set(const std::string& key, const AronDataPtr& value)
+ {
+ int index = indexOf(key);
+ dict->elements.at(index).value = value;
+ }
+
+ bool DictNavigator::has(const std::string& key)
+ {
+ return indexOf(key) >= 0;
+ }
+
+ int DictNavigator::indexOf(const std::string& key)
+ {
+ for (size_t i = 0; i < dict->elements.size(); i++)
+ {
+ if (dict->elements.at(i).key == key)
+ {
+ return i;
+ }
+ }
+ return -1;
+ }
+
+ bool DictNavigator::remove(const std::string& key)
+ {
+ int index = indexOf(key);
+ if (index < 0)
+ {
+ return false;
+ }
+ dict->elements.erase(dict->elements.begin() + index);
+ return true;
+ }
+
+
}
}
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h
index ae39728626f92f277c908c7b5bf8d176c7e7eb83..9c568530d13e22dc3f663beaa21e98fd5184481b 100644
--- a/source/RobotAPI/libraries/aron/AronNavigator.h
+++ b/source/RobotAPI/libraries/aron/AronNavigator.h
@@ -37,30 +37,54 @@ namespace armarx
enum { value = false };
};
+ struct AronPathItem
+ {
+ AronPathItem(int index): index(index), key("") {}
+ AronPathItem(const std::string& key): index(-1), key(key) {}
+ int index = -1;
+ std::string key = "";
+ };
+ struct AronPath
+ {
+ std::vector<AronPathItem> path;
+ AronPath appendIndex(size_t index) const;
+ AronPath appendKey(const std::string& key) const;
+ std::string toString() const;
+ };
+ class ListNavigator;
+ class DictNavigator;
+ template <typename T>
+ class NDArrayNavigator;
+
class AronNavigator
{
public:
AronNavigator(const AronDataPtr& data);
- AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index);
+ AronNavigator(const AronDataPtr& data, const AronPath& path);
- AronNavigator atIndex(size_t index);
- AronNavigator atKey(const std::string& key);
- std::vector<AronNavigator> children();
- void append(const AronDataPtr& value);
- bool append(const std::string& key, const AronDataPtr& value);
- void set(const std::string& key, const AronDataPtr& value);
- bool hasKey(const std::string& key);
- int keyIndex(const std::string& key);
- bool deleteKey(const std::string& key);
+ ListNavigator asList();
+ DictNavigator asDict();
+ template <typename T>
+ NDArrayNavigator<T> asNDArray()
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T));
+ return NDArrayNavigator<T>(arr, path);
+ }
+
+ Eigen::Vector3f asVector3f();
+ Eigen::Matrix3f asMatrix3f();
+ Eigen::Matrix4f asMatrix4f();
bool isList();
- bool isObject();
+ bool isDict();
bool isNull();
+
+
#define HANDLE_TYPE(cppType, upperType) \
cppType as##upperType(); \
bool is##upperType(); \
@@ -73,15 +97,6 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
@@ -91,21 +106,33 @@ namespace armarx
template <typename T>
std::vector<T> asStdVector();
- //TODO
- bool appendVec(const std::string& key, const std::vector<float>& vec);
- std::vector<float> asVecFloat();
+
template <typename T>
std::map<std::string, T> asStdMap();
void writeXml(RapidXmlWriterNode parent);
+ private:
+ AronNDArrayPtr checkCastToNDArray(size_t size);
+ AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1);
+ AronNDArrayPtr checkCastToNDArray(size_t size, size_t dim1, size_t dim2);
+ template <typename T>
+ T* checkReinterpretNDArray(size_t dim1)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1);
+ return reinterpret_cast<T*>(arr->data.data());
+ }
+ template <typename T>
+ T* checkReinterpretNDArray(size_t dim1, size_t dim2)
+ {
+ AronNDArrayPtr arr = checkCastToNDArray(sizeof(T), dim1, dim2);
+ return reinterpret_cast<T*>(arr->data.data());
+ }
private:
AronDataPtr data;
- std::string path;
- std::string key;
- int index;
+ AronPath path;
};
template<typename T>
@@ -126,18 +153,84 @@ namespace armarx
HANDLE_TYPE(float, Float)
HANDLE_TYPE(double, Double)
HANDLE_TYPE(bool, Bool)
- HANDLE_TYPE(AronBlobValue, Blob)
-
- HANDLE_TYPE(Eigen::Vector2f, Vector2f)
- HANDLE_TYPE(Eigen::Vector3f, Vector3f)
- HANDLE_TYPE(Eigen::Vector6f, Vector6f)
- HANDLE_TYPE(Eigen::VectorXf, VectorXf)
- HANDLE_TYPE(Eigen::Matrix3f, Matrix3f)
- HANDLE_TYPE(Eigen::Matrix4f, Matrix4f)
- HANDLE_TYPE(Eigen::MatrixXf, MatrixXf)
#undef HANDLE_TYPE
+ class ListNavigator
+ {
+ public:
+ ListNavigator(const AronListPtr& list, const AronPath& path);
+ AronNavigator at(size_t index);
+ std::vector<AronNavigator> children();
+ void append(const AronDataPtr& value);
+ std::vector<AronDataPtr>& get();
+
+ //TODO
+ std::vector<float> asVecFloat();
+
+ private:
+ AronListPtr list;
+ AronPath path;
+ };
+
+ class DictNavigator
+ {
+ public:
+ DictNavigator(const AronDictPtr& dict, const AronPath& path);
+ AronNavigator at(const std::string& key);
+ std::vector<AronNavigator> children();
+ bool append(const std::string& key, const AronDataPtr& value);
+ bool appendVec(const std::string& key, const std::vector<float>& vec);
+ void set(const std::string& key, const AronDataPtr& value);
+ bool has(const std::string& key);
+ int indexOf(const std::string& key);
+ bool remove(const std::string& key);
+ private:
+ AronDictPtr dict;
+ AronPath path;
+ };
+
+ template <typename T>
+ class NDArrayNavigator
+ {
+ public:
+ NDArrayNavigator(const AronNDArrayPtr& arr, const AronPath& path)
+ : arr(arr), path(path) {}
+ size_t dimensions()
+ {
+ return arr->dimensions.size();
+ }
+ int size(size_t dim)
+ {
+ return arr->dimensions.at(dim);
+ }
+
+ T& at(size_t i)
+ {
+ ARMARX_CHECK(dimensions() == 1);
+ return reinterpret_cast<T&>(_at(i));
+ }
+ T& at(size_t i, size_t j)
+ {
+ ARMARX_CHECK(dimensions() == 2);
+ return reinterpret_cast<T&>(_at(i * arr->dimensions.at(1) + j));
+ }
+ T* data()
+ {
+ return reinterpret_cast<T*>(arr->data.data());
+ }
+ private:
+ Ice::Byte& _at(size_t i)
+ {
+
+ return arr->data.at(arr->itemSize * i);
+ }
+
+ private:
+ AronNDArrayPtr arr;
+ AronPath path;
+ };
+
}
}
diff --git a/source/RobotAPI/libraries/aron/types/TypesMap.cpp b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
index 91b29d5b8d8a7723856ec1017160aeae57b493f6..2b3ca35316b4be12d4242d5a8168f931522dac1f 100644
--- a/source/RobotAPI/libraries/aron/types/TypesMap.cpp
+++ b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
@@ -37,7 +37,7 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type)
{
SingleTypePtr single = SingleTypePtr::dynamicCast(type);
ListTypePtr list = ListTypePtr::dynamicCast(type);
- ObjectTypePtr obj = ObjectTypePtr::dynamicCast(type);
+ DictTypePtr dict = DictTypePtr::dynamicCast(type);
if (single)
{
return getCppType(single->namespaces, single->typeName);
@@ -46,9 +46,9 @@ std::string TypesMap::getCppType(const types::AbstractTypePtr& type)
{
return "std::vector<" + getCppType(list->childtype) + ">";
}
- if (obj)
+ if (dict)
{
- return "std::map<std::string, " + getCppType(obj->childtype) + ">";
+ return "std::map<std::string, " + getCppType(dict->childtype) + ">";
}
throw LocalException("Unsupported type");
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index c759e1f768e1b766ee5ae77bcd670ab3ff15f448..b1f6850fbf3cd8067a0903c8a6973561f2418c9d 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -21,7 +21,6 @@
* GNU General Public License
*/
-#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -30,6 +29,9 @@
#include "CartesianNaturalPositionController.h"
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+
namespace armarx
{
CartesianNaturalPositionController::CartesianNaturalPositionController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& elbow,
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index af8d731a99eb24bc7082a7c3d7cb9f08278492cb..122d5b4d82d99194b3ac451be8e24343373d8742 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -21,7 +21,6 @@
* GNU General Public License
*/
-#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -30,6 +29,10 @@
#include "CartesianWaypointController.h"
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+#include <cfloat>
+
namespace armarx
{
CartesianWaypointController::CartesianWaypointController(
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index e7a796a6eb65e02cb59ac06751278d73912ca8cc..3e987d97dacfa905903e8ac6f86cbf3c8afb87fb 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -33,20 +33,14 @@
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/LinkedCoordinate.h>
+
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <boost/shared_ptr.hpp>
-
#include <sstream>
-namespace VirtualRobot
-{
- class Robot;
- using RobotPtr = boost::shared_ptr<Robot>;
- class LinkedCoordinate;
-}
-
namespace armarx::VariantType
{
// variant types
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 56ff6d28cd17522453014f9b880d8e88294ea3dc..f25048c8fec61c0923c8da06532f1a05570fba87 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -28,10 +28,11 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <Eigen/Core>
+#include <mutex>
+
namespace armarx
{
template <int dimensions = Eigen::Dynamic>
@@ -218,7 +219,7 @@ namespace armarx
double maxControlValue;
double maxDerivation;
bool firstRun;
- mutable RecursiveMutex mutex;
+ mutable std::recursive_mutex mutex;
bool threadSafe = true;
std::vector<bool> limitless;
private:
@@ -231,6 +232,8 @@ namespace armarx
PIDVectorX zeroVec;
} stackAllocations;
+ using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+ using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
ScopedRecursiveLockPtr getLock() const
{
if (threadSafe)
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 34c5e6c30469a5ef7d38d19a90a0bf5d67f57b5f..02f198137de13f4687bb212753b204b9ea49aaca 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -68,11 +68,11 @@ namespace armarx
}
}
- ScopedRecursiveLockPtr PIDController::getLock() const
+ auto PIDController::getLock() const -> ScopedRecursiveLockPtr
{
if (threadSafe)
{
- return std::make_shared<ScopedRecursiveLock>(mutex);
+ return std::make_unique<ScopedRecursiveLock>(mutex);
}
else
{
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index b8631b35b9154e28962149e3c1ca5688447d59ac..320f2e6146f018b9803d34dac4a4418dd22aebcc 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -28,10 +28,12 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <Eigen/Core>
+#include <memory>
+#include <mutex>
+
namespace armarx
{
@@ -73,8 +75,10 @@ namespace armarx
rtfilters::RTFilterBasePtr differentialFilter;
rtfilters::RTFilterBasePtr pdOutputFilter;
private:
+ using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+ using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
ScopedRecursiveLockPtr getLock() const;
- mutable RecursiveMutex mutex;
+ mutable std::recursive_mutex mutex;
};
using PIDControllerPtr = std::shared_ptr<PIDController>;
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 154cfb2890c9fb2f0f5e6a8b2867bf82ea68d775..2b9444a4d594a6483c435638fad3385f9b544f57 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -43,7 +43,7 @@ namespace armarx
VirtualRobot::RobotPtr RobotPool::getRobot(int inflation)
{
- ScopedLock lock(mutex);
+ std::scoped_lock lock(mutex);
for (auto& r : robots[inflation])
{
if (r.use_count() == 1)
@@ -61,7 +61,7 @@ namespace armarx
size_t RobotPool::getPoolSize() const
{
- ScopedLock lock(mutex);
+ std::scoped_lock lock(mutex);
size_t size = 0;
for (auto& pair : robots)
{
@@ -72,7 +72,7 @@ namespace armarx
size_t RobotPool::getRobotsInUseCount() const
{
- ScopedLock lock(mutex);
+ std::scoped_lock lock(mutex);
size_t count = 0;
for (auto& pair : robots)
{
@@ -89,7 +89,7 @@ namespace armarx
size_t RobotPool::clean()
{
- ScopedLock lock(mutex);
+ std::scoped_lock lock(mutex);
size_t count = 0;
for (auto& pair : robots)
{
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
index 6aa0ccb24c917a6e76c2d2a0e1d06f9cc8ae16be..4d6db6fc1ed2ba14a1f3c9bffe2b7557deaf0508 100644
--- a/source/RobotAPI/libraries/core/RobotPool.h
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -22,8 +22,11 @@
* GNU General Public License
*/
#pragma once
+
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <mutex>
+
namespace armarx
{
@@ -53,7 +56,7 @@ namespace armarx
private:
std::map<int, std::vector<VirtualRobot::RobotPtr>> robots;
VirtualRobot::RobotPtr baseRobot;
- mutable Mutex mutex;
+ mutable std::mutex mutex;
};
using RobotPoolPtr = std::shared_ptr<RobotPool>;
}
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
index 6e4988445a86ee0a2e42efc1ddcc4da2600d3609..bef441657c7653bfb75458789516a13074576a9d 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
@@ -28,6 +28,10 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/statechart/Statechart.h>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/trim.hpp>
+
namespace armarx
{
// ****************************************************************
@@ -36,7 +40,7 @@ namespace armarx
void RobotStatechartContext::onInitStatechartContext()
{
// StatechartContext::onInitStatechart();
- ARMARX_LOG << eINFO << "Init RobotStatechartContext" << flush;
+ ARMARX_INFO << "Init RobotStatechartContext" << flush;
kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
@@ -71,7 +75,7 @@ namespace armarx
void RobotStatechartContext::onConnectStatechartContext()
{
// StatechartContext::onConnectStatechart();
- ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
+ ARMARX_INFO << "Starting RobotStatechartContext" << flush;
// retrieve proxies
std::string rbStateName = getProperty<std::string>("RobotStateComponentName").getValue();
@@ -80,12 +84,12 @@ namespace armarx
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinUnitName);
kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
- ARMARX_LOG << eINFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
+ ARMARX_INFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
if (!headIKUnitName.empty())
{
headIKUnitPrx = getProxy<HeadIKUnitInterfacePrx>(headIKUnitName);
- ARMARX_LOG << eINFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName << flush;
+ ARMARX_INFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName << flush;
}
@@ -100,13 +104,13 @@ namespace armarx
boost::algorithm::trim(handUnitList.at(i));
HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
handUnits[handUnitList.at(i)] = handPrx;
- ARMARX_LOG << eINFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
+ ARMARX_INFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
}
}
// initialize remote robot
remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
- ARMARX_LOG << eINFO << "Created remote robot" << flush;
+ ARMARX_INFO << "Created remote robot" << flush;
}
PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
@@ -119,16 +123,16 @@ namespace armarx
{
if (handUnits.find(handUnitName) != handUnits.end())
{
- ARMARX_LOG << eINFO << "Found proxy of hand unit with name " << handUnitName << flush;
+ ARMARX_INFO << "Found proxy of hand unit with name " << handUnitName << flush;
return handUnits[handUnitName];
}
- ARMARX_LOG << eINFO << "Do not know proxy of hand unit with name " << handUnitName << flush;
+ ARMARX_INFO << "Do not know proxy of hand unit with name " << handUnitName << flush;
std::map<std::string, HandUnitInterfacePrx>::iterator it = handUnits.begin();
while (it != handUnits.end())
{
- ARMARX_LOG << eINFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
+ ARMARX_INFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
it++;
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index cea227464abe29898488605e6dcb38f5e50fb4a5..90c68c71c1fd0ca963f8f4eb1bc05c1d926be1f7 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -1971,6 +1971,36 @@ namespace armarx
return getDeriv(dim, 0);
}
+ Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const
+ {
+ if (!trajectory)
+ {
+ throw LocalException("Ptr to trajectory is NULL");
+ }
+ size_t numDim = trajectory->dim();
+ Eigen::VectorXf result(numDim);
+ for (std::size_t i = 0; i < numDim; ++i)
+ {
+ result(i) = getPosition(i);
+ }
+ return result;
+ }
+
+ Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const
+ {
+ if (!trajectory)
+ {
+ throw LocalException("Ptr to trajectory is NULL");
+ }
+ size_t numDim = trajectory->dim();
+ Eigen::VectorXd result(numDim);
+ for (std::size_t i = 0; i < numDim; ++i)
+ {
+ result(i) = getPosition(i);
+ }
+ return result;
+ }
+
double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
{
if (!trajectory)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 2feaea611590e10f71b227bcb845c46dd37f59ce..990de647c0bb74092d96a7774830cd1fb6f96f98 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -128,6 +128,9 @@ namespace armarx
{
return getPositionsAs<double>();
}
+ Eigen::VectorXf getPositionsAsVectorXf() const;
+ Eigen::VectorXd getPositionsAsVectorXd() const;
+
template<class T>
void writePositionsToNameValueMap(std::map<std::string, T>& map) const
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index cdb7046dbf34e051339eabf8bfe4342c4c5d00f8..2e330626166b264497319869534e99a2f093896e 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -42,7 +42,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
@@ -73,7 +73,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
@@ -104,7 +104,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
@@ -140,7 +140,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
@@ -199,7 +199,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
@@ -248,7 +248,7 @@ namespace armarx::filters
}
VariantBasePtr calculate(const Ice::Current&) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
index 42c89f07eadf0ec85339654db7f5baeee4a6743b..eaf21147840e8ec1c2e95fa7eedbb218f5518ed0 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
@@ -39,7 +39,7 @@ armarx::filters::MedianDerivativeFilterV3::MedianDerivativeFilterV3(std::size_t
armarx::VariantBasePtr armarx::filters::MedianDerivativeFilterV3::calculate(const Ice::Current& c) const
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index d161d4ca39078718e8b37653e60e536dcbf0d0f3..68b2cc80905a7747ab73c04f9d19c873b4e3fb77 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -55,7 +55,7 @@ namespace armarx::filters
VariantBasePtr calculate(const Ice::Current& c = Ice::emptyCurrent) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
VariantPtr newVariant;
@@ -135,7 +135,7 @@ namespace armarx::filters
if (firstRun)
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
index 5b0cf4dbe22bedcaee456006c02c83c08566631d..aa3b14347927cbbcd068f0164f2c92b86012fabd 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -28,7 +28,7 @@ namespace armarx::filters
VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index 8b43dd5487849e96fb9bdc34de95c5ce96a039c7..33bbed17812e9c8d06336da6ce37ca9b7e73703e 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -50,7 +50,7 @@ namespace armarx::filters
public:
VariantBasePtr calculate(const Ice::Current& c) const override
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
index 42bdc1c19577e18c7f157885549a39c7055dbceb..e4d4a1444065ad83bbc681fa83832bca5ccabb29 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -37,7 +37,7 @@ armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
{
- ScopedLock lock(historyMutex);
+ std::unique_lock lock(historyMutex);
if (dataHistory.size() == 0)
{
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index dfc84e32aa84f0bba6e323322cd78cab998473be..99495ecee5a9b0ce1402f6fb182eaa63341f80a4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -22,23 +22,30 @@
* GNU General Public License
*/
#include "RemoteRobot.h"
+
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-#include <Eigen/Geometry>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/core/logging/Logging.h>
-//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj << endl;
+#include <Eigen/Geometry>
+
+//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj;
#define DMES(Obj) ;
using namespace VirtualRobot;
@@ -47,7 +54,7 @@ using namespace Eigen;
namespace armarx
{
- boost::recursive_mutex RemoteRobot::m;
+ std::recursive_mutex RemoteRobot::m;
RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
Robot(),
@@ -199,12 +206,12 @@ namespace armarx
VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo)
{
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::scoped_lock cloneLock(m);
static int nonameCounter = 0;
if (!remoteNode || !robo)
{
- ARMARX_ERROR_S << " NULL data " << endl;
+ ARMARX_ERROR_S << " NULL data ";
return VirtualRobot::RobotNodePtr();
}
@@ -217,7 +224,7 @@ namespace armarx
if (name.empty())
{
- ARMARX_LOG_S << "Node without name!!!" << endl;
+ ARMARX_LOG_S << "Node without name!!!";
std::stringstream ss;
ss << "robot_node_" << nonameCounter;
nonameCounter++;
@@ -285,7 +292,7 @@ namespace armarx
break;
default:
- ARMARX_ERROR_S << "JointType nyi..." << endl;
+ ARMARX_ERROR_S << "JointType nyi...";
return VirtualRobot::RobotNodePtr();
break;
}
@@ -308,14 +315,14 @@ namespace armarx
/* boost::shared_ptr< RemoteRobotNode<VirtualRobotNodeType> > rnRemote = boost::dynamic_pointer_cast<RemoteRobotNode>(rnRemoteBase);
if (!rnRemote)
{
- ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i] << endl;
+ ARMARX_ERROR_S << "RemoteRobot does not know robot node with name " << children[i];
continue;
}*/
if (!localNode)
{
- ARMARX_ERROR_S << "Could not create local node: " << children[i] << endl;
+ ARMARX_ERROR_S << "Could not create local node: " << children[i];
continue;
}
@@ -329,8 +336,7 @@ namespace armarx
VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
{
- //ARMARX_IMPORTANT_S << "RemoteRobot local clone" << flush;
- boost::recursive_mutex::scoped_lock cloneLock(m);
+ std::scoped_lock cloneLock(m);
std::string robotType = getName();
std::string robotName = getName();
VirtualRobot::RobotPtr robo(new VirtualRobot::LocalRobot(robotName, robotType));
@@ -347,7 +353,7 @@ namespace armarx
if (!res)
{
- ARMARX_ERROR_S << "Failed to initialize local robot..." << endl;
+ ARMARX_ERROR_S << "Failed to initialize local robot...";
return VirtualRobot::RobotPtr();
}
@@ -375,24 +381,24 @@ namespace armarx
{
RobotPtr result;
- boost::recursive_mutex::scoped_lock cloneLock(m);
- ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
+ std::scoped_lock cloneLock(m);
+ ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")";
if (!sharedRobotPrx)
{
- ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting..." << endl;
+ ARMARX_ERROR_S << "NULL sharedRobotPrx. Aborting...";
return result;
}
if (filename.empty())
{
- RemoteRobotPtr rob(new RemoteRobot(sharedRobotPrx));
+ std::shared_ptr<RemoteRobot> rob(new RemoteRobot(sharedRobotPrx));
result = rob->createLocalClone();
if (!result)
{
- ARMARX_ERROR_S << "Could not create local clone. Aborting..." << endl;
+ ARMARX_ERROR_S << "Could not create local clone. Aborting...";
return result;
}
}
@@ -429,7 +435,7 @@ namespace armarx
if (!result)
{
- ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
+ ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting...";
return result;
}
}
@@ -460,12 +466,12 @@ namespace armarx
{
if (!robot)
{
- ARMARX_ERROR << "Robot is NULL! Aborting..." << endl;
+ ARMARX_ERROR << "Robot is NULL! Aborting...";
return false;
}
if (!sharedRobotPrx)
{
- ARMARX_ERROR_S << "shared robot prx is NULL! Aborting..." << endl;
+ ARMARX_ERROR_S << "shared robot prx is NULL! Aborting...";
return false;
}
PoseBasePtr globalPose;
@@ -480,7 +486,7 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
}
}
@@ -518,7 +524,7 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
}
}
@@ -559,5 +565,10 @@ namespace armarx
}
}
+ CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker()
+ {
+ return VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+ }
+
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 815d81841a7b09abb3650da0ce98fcd8b357a700..42970b7e78c7f62fbc7d5ab42ec4a5107a6e6487 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -22,32 +22,18 @@
* GNU General Public License
*/
#pragma once
-#ifndef _ARMARX_REMOTE_ROBOT_H__
-#define _ARMARX_REMOTE_ROBOT_H__
-
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-// boost
-#include <boost/thread/mutex.hpp>
-#include <boost/utility/enable_if.hpp>
-#include <boost/type_traits/is_base_of.hpp>
-
-//namespace VirtualRobot
-//{
-// class RobotNodeRevolute;
-// class RobotNodePrismatic;
-// class RobotNodeFixed;
-//}
+
+#include <mutex>
namespace armarx
{
@@ -73,6 +59,8 @@ namespace armarx
template<>
void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
+ VirtualRobot::CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker();
+
/** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
* @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
@@ -94,7 +82,7 @@ namespace armarx
_node->getJointValueOffest();
setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
- this->collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+ this->collisionChecker = RemoteRobotNode_getGlobalCollisionChecker();
RemoteRobotNodeInitializer<VirtualRobotNodeType>::initialize(this);
}
@@ -150,13 +138,10 @@ namespace armarx
*/
class RemoteRobot : public VirtualRobot::Robot
{
- template<class T>
- friend void boost::checked_delete(T*);
- protected:
- ~RemoteRobot() override;
public:
RemoteRobot(SharedRobotInterfacePrx robot);
+ ~RemoteRobot() override;
VirtualRobot::RobotNodePtr getRootNode() const override;
@@ -270,12 +255,11 @@ namespace armarx
mutable std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
mutable VirtualRobot::RobotNodePtr _root;
- static boost::recursive_mutex m;
+ static std::recursive_mutex m;
static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
};
- using RemoteRobotPtr = boost::shared_ptr<RemoteRobot>;
+ using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
}
-#endif
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index b230a3bb08cc49535081c714b7926ba817ada50d..2c259eafc983fc5b78cdffb42c5471578f2ac3c8 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -26,6 +26,8 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <ArmarXCore/interface/core/BasicTypes.h>
namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 9a2dbb98e6026f602741b6020b50e0d1fd1a9ad4..08fbb612d15a9734c86d1043aee74088a330171a 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -29,6 +29,7 @@
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotNodeSet.h>
@@ -39,6 +40,8 @@
#include <VirtualRobot/IK/DifferentialIK.h>
#include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/classification.hpp>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
@@ -92,7 +95,7 @@ void RobotStateObserver::updatePoses()
return;
}
- ScopedRecursiveLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
ReadLockPtr lock2 = robot->getReadLock();
FramedPoseBaseMap tcpPoses;
std::string rootFrame = robot->getRootNode()->getName();
@@ -181,7 +184,7 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
return;
}
- ScopedRecursiveLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
if (!robot)
{
@@ -316,7 +319,7 @@ PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
void RobotStateObserver::setRobot(RobotPtr robot)
{
- ScopedRecursiveLock lock(dataMutex);
+ std::unique_lock lock(dataMutex);
this->robot = robot;
std::vector< VirtualRobot::RobotNodeSetPtr > robotNodes;
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index e8c1146821c153ab5767805431e2a8c8a9d53163..e3acfd611dc6487744decc1c4d3183e33f74c383 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -31,15 +31,9 @@
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
-#include <string>
+#include <VirtualRobot/VirtualRobot.h>
-namespace VirtualRobot
-{
- class Robot;
- using RobotPtr = boost::shared_ptr<Robot>;
- class RobotNode;
- using RobotNodePtr = boost::shared_ptr<RobotNode>;
-}
+#include <string>
namespace armarx
{
@@ -100,7 +94,7 @@ namespace armarx
RobotStateComponentInterfacePrx server;
VirtualRobot::RobotPtr robot, velocityReportRobot;
std::vector<std::pair<VirtualRobot::RobotNodePtr, std::string> > nodesToReport;
- RecursiveMutex dataMutex;
+ std::recursive_mutex dataMutex;
IceUtil::Time lastVelocityUpdate;
// RobotStateObserverInterface interface
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index 9699d191373f2210ec6677e05a8af1b6315ca71d..05a777679813c9f2411dbe6cce56141ab75dae77 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -6,15 +6,22 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCore
RobotAPICore
+ StructuralJson
+ SimpleJsonLogger
)
set(LIB_FILES
NaturalDiffIK.cpp
SimpleDiffIK.cpp
+ RobotPlacement.cpp
+ GraspTrajectory.cpp
)
set(LIB_HEADERS
NaturalDiffIK.h
SimpleDiffIK.h
+ DiffIKProvider.h
+ RobotPlacement.h
+ GraspTrajectory.h
)
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
new file mode 100644
index 0000000000000000000000000000000000000000..a75434af401538fc92697010ee6fc3d6b43e9071
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
+
+ struct DiffIKResult
+ {
+ bool reachable;
+ float posError;
+ float oriError;
+ Eigen::VectorXf jointValues;
+
+ };
+ class DiffIKProvider
+ {
+ public:
+ virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
+ virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d76464e94326b8e864ace29ec294d10aff65402
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -0,0 +1,461 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "GraspTrajectory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+using namespace armarx;
+
+
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
+ feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0),
+ feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows()))
+{ }
+
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget,
+ float dt, const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+{ }
+
+Eigen::Vector3f GraspTrajectory::Keypoint::getTargetPosition() const
+{
+ return ::math::Helpers::GetPosition(tcpTarget);
+}
+
+Eigen::Matrix3f GraspTrajectory::Keypoint::getTargetOrientation() const
+{
+ return ::math::Helpers::GetOrientation(tcpTarget);
+}
+
+Eigen::Matrix4f GraspTrajectory::Keypoint::getTargetPose() const
+{
+ return tcpTarget;
+}
+
+void GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev, float dt)
+{
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
+ Eigen::VectorXf hnd0 = prev->handJointsTarget;
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
+ Eigen::VectorXf hnd1 = handJointsTarget;
+
+ Eigen::Vector3f dpos = pos1 - pos0;
+ Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
+ Eigen::VectorXf dhnd = hnd1 - hnd0;
+
+ this->dt = dt;
+ feedForwardPosVelocity = dpos / dt;
+ feedForwardOriVelocity = dori / dt;
+ feedForwardHandJointsVelocity = dhnd / dt;
+}
+
+GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart)
+{
+ KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
+ keypointMap[0] = keypoints.size();
+ keypoints.push_back(keypoint);
+}
+
+void GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ KeypointPtr prev = lastKeypoint();
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ float t = getDuration() + dt;
+ keypointMap[t] = keypoints.size();
+ keypoints.push_back(keypoint);
+}
+
+size_t GraspTrajectory::getKeypointCount() const
+{
+ return keypoints.size();
+}
+
+void GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ if (index <= 0 || index > keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ if (index < keypoints.size())
+ {
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(keypoint, next->dt);
+ }
+ keypoints.insert(keypoints.begin() + index, keypoint);
+ updateKeypointMap();
+}
+
+void GraspTrajectory::removeKeypoint(size_t index)
+{
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ keypoints.erase(keypoints.begin() + index);
+ if (index < keypoints.size())
+ {
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(prev, next->dt);
+ }
+ updateKeypointMap();
+}
+
+void GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+{
+ ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ keypoints.at(index) = keypoint;
+ updateKeypointMap();
+}
+
+void GraspTrajectory::setKeypointDt(size_t index, float dt)
+{
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr& keypoint = keypoints.at(index);
+ keypoint->updateVelocities(prev, dt);
+ updateKeypointMap();
+}
+
+GraspTrajectory::KeypointPtr& GraspTrajectory::lastKeypoint()
+{
+ return keypoints.at(keypoints.size() - 1);
+}
+
+GraspTrajectory::KeypointPtr& GraspTrajectory::getKeypoint(int i)
+{
+ return keypoints.at(i);
+}
+
+Eigen::Matrix4f GraspTrajectory::getStartPose()
+{
+ return getKeypoint(0)->getTargetPose();
+}
+
+void GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+{
+ if (t <= 0)
+ {
+ i1 = 0;
+ i2 = 0;
+ f = 0;
+ return;
+ }
+ std::map<float, size_t>::const_iterator it, prev;
+ it = keypointMap.upper_bound(t);
+ if (it == keypointMap.end())
+ {
+ i1 = keypoints.size() - 1;
+ i2 = keypoints.size() - 1;
+ f = 0;
+ return;
+ }
+ prev = std::prev(it);
+ i1 = prev->second;
+ i2 = it->second;
+ f = ::math::Helpers::ILerp(prev->first, it->first, t);
+}
+
+Eigen::Vector3f GraspTrajectory::GetPosition(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+}
+
+Eigen::Matrix3f GraspTrajectory::GetOrientation(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ Eigen::Vector3f oriVel = GetOrientationDerivative(t);
+ if (oriVel.squaredNorm() == 0)
+ {
+ return getKeypoint(i1)->getTargetOrientation();
+ }
+ Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized());
+ aa.angle() = aa.angle() * f * getKeypoint(i2)->dt;
+ return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation();
+}
+
+Eigen::Matrix4f GraspTrajectory::GetPose(float t)
+{
+ return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
+}
+
+std::vector<Eigen::Matrix4f> GraspTrajectory::getAllKeypointPoses()
+{
+ std::vector<Eigen::Matrix4f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPose());
+ }
+ return res;
+}
+
+std::vector<Eigen::Vector3f> GraspTrajectory::getAllKeypointPositions()
+{
+ std::vector<Eigen::Vector3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPosition());
+ }
+ return res;
+}
+
+std::vector<Eigen::Matrix3f> GraspTrajectory::getAllKeypointOrientations()
+{
+ std::vector<Eigen::Matrix3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetOrientation());
+ }
+ return res;
+}
+
+Eigen::VectorXf GraspTrajectory::GetHandValues(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+
+ return getKeypoint(i1)->handJointsTarget * (1 - f) + getKeypoint(i2)->handJointsTarget * f;
+}
+
+Eigen::Vector3f GraspTrajectory::GetPositionDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardPosVelocity;
+}
+
+Eigen::Vector3f GraspTrajectory::GetOrientationDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardOriVelocity;
+}
+
+Eigen::Vector6f GraspTrajectory::GetTcpDerivative(float t)
+{
+ Eigen::Vector6f ffVel;
+ ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
+ ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
+ return ffVel;
+}
+
+Eigen::VectorXf GraspTrajectory::GetHandJointsDerivative(float t)
+{
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardHandJointsVelocity;
+}
+
+float GraspTrajectory::getDuration() const
+{
+ return keypointMap.rbegin()->first;
+}
+
+GraspTrajectory::Length GraspTrajectory::calculateLength() const
+{
+ Length l;
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr k0 = keypoints.at(i - 1);
+ KeypointPtr k1 = keypoints.at(i);
+
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(k0->tcpTarget);
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(k1->tcpTarget);
+
+ l.pos += (pos1 - pos0).norm();
+ l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).angle());
+ }
+ return l;
+}
+
+int GraspTrajectory::GetHandJointCount() const
+{
+ return keypoints.at(0)->handJointsTarget.rows();
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation)
+{
+ GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+{
+ GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::getClone()
+{
+ return getTransformed(Eigen::Matrix4f::Identity());
+}
+
+GraspTrajectoryPtr GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward)
+{
+ Eigen::Matrix4f startPose = getStartPose();
+ Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
+ Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ Eigen::Vector3f up(0, 0, 1);
+
+ float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
+ Eigen::AngleAxisf aa(angle, up);
+
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ return getTransformed(transform);
+}
+
+SimpleDiffIK::Reachability GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+{
+ return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+}
+
+void GraspTrajectory::writeToFile(const std::string& filename)
+{
+ RapidXmlWriter writer;
+ RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ SimpleJsonLoggerEntry e;
+ e.Add("dt", keypoint->dt);
+ e.AddAsArr("Pose", keypoint->tcpTarget);
+ e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
+ }
+ writer.saveToFile(filename, true);
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
+{
+ std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage;
+ armarx::CMakePackageFinder finder(packageName);
+ std::string dataDir = finder.getDataDir() + "/" + packageName;
+ return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
+{
+ RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
+ GraspTrajectoryPtr traj;
+ for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
+ {
+ StructuralJsonParser p(kpNode.value());
+ p.parse();
+ JPathNavigator nav(p.parsedJson);
+ float dt = nav.selectSingleNode("dt").asFloat();
+
+ Eigen::Matrix4f pose;
+ std::vector<JPathNavigator> rows = nav.select("Pose/*");
+ for (int i = 0; i < 4; i++)
+ {
+ std::vector<JPathNavigator> cells = rows.at(i).select("*");
+ for (int j = 0; j < 4; j++)
+ {
+ pose(i, j) = cells.at(j).asFloat();
+ }
+ }
+
+ Eigen::Vector3f handValues;
+ std::vector<JPathNavigator> cells = nav.select("HandValues/*");
+ for (int j = 0; j < 3; j++)
+ {
+ handValues(j) = cells.at(j).asFloat();
+ }
+
+ if (!traj)
+ {
+ traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ }
+ else
+ {
+ traj->addKeypoint(pose, handValues, dt);
+ }
+ }
+ return traj;
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const std::string& filename)
+{
+ return ReadFromReader(RapidXmlReader::FromFile(filename));
+}
+
+GraspTrajectoryPtr GraspTrajectory::ReadFromString(const std::string& xml)
+{
+ return ReadFromReader(RapidXmlReader::FromXmlString(xml));
+}
+
+void GraspTrajectory::updateKeypointMap()
+{
+ keypointMap.clear();
+ float t = 0;
+ for (size_t i = 0; i < keypoints.size(); i++)
+ {
+ t += getKeypoint(i)->dt;
+ keypointMap[t] = i;
+ }
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e5633d59e9a2f2c8ad533735370a1f008694089
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
@@ -0,0 +1,151 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <map>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/interface/serialization/Eigen.h>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr;
+
+ class GraspTrajectory
+ {
+ public:
+ class Keypoint;
+ typedef boost::shared_ptr<Keypoint> KeypointPtr;
+
+ class Keypoint
+ {
+ public:
+ Eigen::Matrix4f tcpTarget;
+ Eigen::VectorXf handJointsTarget;
+ float dt;
+ Eigen::Vector3f feedForwardPosVelocity;
+ Eigen::Vector3f feedForwardOriVelocity;
+ Eigen::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity);
+ Eigen::Vector3f getTargetPosition() const;
+ Eigen::Matrix3f getTargetOrientation() const;
+ Eigen::Matrix4f getTargetPose() const;
+ void updateVelocities(const KeypointPtr& prev, float dt);
+ };
+
+ struct Length
+ {
+ float pos = 0;
+ float ori = 0;
+ };
+
+
+ public:
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart);
+
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ size_t getKeypointCount() const;
+
+ void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ void removeKeypoint(size_t index);
+
+ void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ void setKeypointDt(size_t index, float dt);
+
+ KeypointPtr& lastKeypoint();
+ KeypointPtr& getKeypoint(int i);
+ Eigen::Matrix4f getStartPose();
+
+ void getIndex(float t, int& i1, int& i2, float& f);
+
+ Eigen::Vector3f GetPosition(float t);
+
+ Eigen::Matrix3f GetOrientation(float t);
+
+ Eigen::Matrix4f GetPose(float t);
+
+ std::vector<Eigen::Matrix4f> getAllKeypointPoses();
+ std::vector<Eigen::Vector3f> getAllKeypointPositions();
+ std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
+
+ Eigen::VectorXf GetHandValues(float t);
+
+ Eigen::Vector3f GetPositionDerivative(float t);
+
+ Eigen::Vector3f GetOrientationDerivative(float t);
+
+ Eigen::Vector6f GetTcpDerivative(float t);
+
+ Eigen::VectorXf GetHandJointsDerivative(float t);
+
+ float getDuration() const;
+
+ Length calculateLength() const;
+ int GetHandJointCount() const;
+
+
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
+
+ GraspTrajectoryPtr getClone();
+
+ GraspTrajectoryPtr getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+
+ SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+
+ void writeToFile(const std::string& filename);
+
+ static GraspTrajectoryPtr ReadFromFile(const grasping::GraspCandidatePtr& cnd);
+
+ static GraspTrajectoryPtr ReadFromReader(const RapidXmlReaderPtr& reader);
+ static GraspTrajectoryPtr ReadFromFile(const std::string& filename);
+ static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+
+ private:
+
+ void updateKeypointMap();
+
+ private:
+ std::vector<KeypointPtr> keypoints;
+ std::map<float, size_t> keypointMap;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index f9a6e2a1022e46cae08520a4313a503fc52e022c..0f98f30dc1b8fa0b6408abe5786f6f7ee3647db2 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -27,6 +27,8 @@
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <cfloat>
+
namespace armarx
{
Eigen::VectorXf NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index f4c9a5af180f2ee62426584c3cbd4e0e06befc05..0bdafb506e4cf6a22203b9b88cde2eccacba32d6 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -23,8 +23,6 @@
#pragma once
-#include <boost/shared_ptr.hpp>
-
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.cpp b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4075c9aae309b66ce51d7125ede732b4869ba88d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
@@ -0,0 +1,68 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotPlacement.h"
+
+#include <VirtualRobot/math/Helpers.h>
+
+using namespace armarx;
+
+RobotPlacement::RobotPlacement(const DiffIKProviderPtr& ikProvider)
+{
+}
+
+std::vector<Eigen::Matrix4f> RobotPlacement::CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa)
+{
+ std::vector<Eigen::Matrix4f> r;
+ for (int x = minx; x <= maxx; x++)
+ for (int y = miny; y <= maxy; y++)
+ for (int a = mina; a <= maxa; a++)
+ {
+ r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
+ }
+ return r;
+}
+
+std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params)
+{
+ std::vector<RobotPlacement::Result> r;
+ std::vector<Eigen::Matrix4f> tcpTargets;
+ for (const Eigen::Matrix4f& pp : params.prePoses)
+ {
+ tcpTargets.emplace_back(pp);
+ }
+ std::vector<Eigen::Matrix4f> grasPoses = params.graspTrajectory->getAllKeypointPoses();
+
+ for (const Eigen::Matrix4f& placement : robotPlacements)
+ {
+ Eigen::Matrix4f invPlacement = placement.inverse();
+ std::vector<Eigen::Matrix4f> localPoses;
+ for (const Eigen::Matrix4f& tcpPose : tcpTargets)
+ {
+ localPoses.emplace_back(invPlacement * tcpPose);
+ }
+ DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0));
+ throw LocalException("not implemented");
+ }
+ return {};
+}
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h
new file mode 100644
index 0000000000000000000000000000000000000000..a677a7a1181b1a88dc731da6f4859c188453f5bf
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DiffIKProvider.h"
+#include "GraspTrajectory.h"
+
+#include <boost/shared_ptr.hpp>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class RobotPlacement> RobotPlacementPtr;
+
+ class RobotPlacement
+ {
+ public:
+ struct Result
+ {
+ DiffIKResult ikResult;
+ };
+ struct PlacementParams
+ {
+ std::vector<Eigen::Matrix4f> prePoses;
+ GraspTrajectoryPtr graspTrajectory;
+ };
+ public:
+ RobotPlacement(const DiffIKProviderPtr& ikProvider);
+
+ static std::vector<Eigen::Matrix4f> CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa);
+
+ std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const PlacementParams& params);
+
+
+ private:
+ DiffIKProviderPtr ikProvider;
+ bool returnOnlyReachable = true;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index 6d0021aec8bab8f99e5fe64ddf86f969b4bdcf80..c3f1a32dbaa1001d06762522080ef5367c072b70 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -26,6 +26,8 @@
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <cfloat>
+
namespace armarx
{
SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
@@ -138,4 +140,35 @@ namespace armarx
return r;
}
+ SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+ : rns(rns), tcp(tcp), params(params)
+ {
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+ {
+ params.resetRnsValues = true;
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+ {
+ params.resetRnsValues = false;
+ rns->setJointValues(startJointValues);
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+ }
+
}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index 4ae4e22fa9ddf740455e9eec2cb561f365a7ca53..e51be99211a5c8c71f612ab920be6a65bfabdcab 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -23,14 +23,16 @@
#pragma once
-#include <boost/shared_ptr.hpp>
+#include "DiffIKProvider.h"
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <memory>
+
namespace armarx
{
- typedef boost::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
+ typedef std::shared_ptr<class SimpleDiffIK> SimpleDiffIKPtr;
class SimpleDiffIK
{
@@ -109,4 +111,19 @@ namespace armarx
///@brief Use this to check a trajectory of waypoints
static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
};
+
+ class SimpleDiffIKProvider :
+ public DiffIKProvider
+ {
+ public:
+ SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+ DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+
+ private:
+ VirtualRobot::RobotNodeSetPtr rns;
+ VirtualRobot::RobotNodePtr tcp;
+ SimpleDiffIK::Parameters params;
+ };
}
+
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index ea98170c468c82a599e5dd1f22a73f2a76ffaaa5..557e31e5eb860f0e170a684aa6e7afa0b00487d4 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -22,15 +22,18 @@
*/
#include "CartesianNaturalPositionControllerProxy.h"
-#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/aron/AronNavigator.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/MathTools.h>
+
using namespace armarx;
+
CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
: _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
{
@@ -85,7 +88,10 @@ void CartesianNaturalPositionControllerProxy::init()
_controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
_controllerCreated = true;
}
- _controller->activateController();
+ if (_activateControllerOnInit)
+ {
+ _controller->activateController();
+ }
}
bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight)
@@ -347,6 +353,11 @@ std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::
return result;
}
+void CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
+{
+ _activateControllerOnInit = value;
+}
+
void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
{
VelocityBaseSettings& v = _velocityBaseSettings;
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index d87c65c4bf5136cf2ea3812059242ed6710c87b5..c2ccc06f5ce8b1a43106443ff3c169d0e12aa4e3 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -25,11 +25,12 @@
#include "NaturalIK.h"
-#include <boost/shared_ptr.hpp>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/aron.h>
+#include <memory>
+
namespace armarx
{
typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy> CartesianNaturalPositionControllerProxyPtr;
@@ -113,7 +114,6 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr rns;
};
-
CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
@@ -174,6 +174,8 @@ namespace armarx
DynamicKp getDynamicKp();
static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
+ void setActivateControllerOnInit(bool value);
+
private:
void updateDynamicKp();
void updateNullspaceTargets();
@@ -207,5 +209,6 @@ namespace armarx
std::map<std::string, WaypointConfig> _defaultWaypointConfigs;
bool _waypointChanged = false;
FTSensorValue _ftOffset;
+ bool _activateControllerOnInit = false;
};
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 88c57b35768281fee04794b07cf789318203f140..f0ad1b7309e7f33c84764363ca5cd150a370b18b 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -213,3 +213,33 @@ void NaturalIK::setLowerArmLength(float value)
{
lowerArmLength = value;
}
+
+NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params)
+ : natik(natik), arm(arm), setOri(setOri), params(params)
+{
+}
+
+DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+{
+ params.diffIKparams.resetRnsValues = true;
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
+
+DiffIKResult NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+{
+ params.diffIKparams.resetRnsValues = false;
+ arm.rns->setJointValues(startJointValues);
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 6db3aa965d5860f4bb5bc1e2bca48901b08e8b04..d6315096a58084776797e6ce5551b65651ff4026 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -27,6 +27,7 @@
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/libraries/diffik/DiffIKProvider.h>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <optional>
@@ -134,4 +135,19 @@ namespace armarx
float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH;
float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
};
+
+ class NaturalIKProvider
+ : public DiffIKProvider
+ {
+ public:
+ NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params = NaturalIK::Parameters());
+ DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+
+ private:
+ NaturalIK natik;
+ NaturalIK::ArmJoints arm;
+ NaturalDiffIK::Mode setOri;
+ NaturalIK::Parameters params;
+ };
}