diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index 0a11b09754f6a47f7b52e3fe0958f66aeeb034bd..2bb041a7b577bf9a9443dcb4f405949e840a636e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -121,10 +121,14 @@ namespace armarx
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
-
- if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
+ if (sensorGlobalPositionCorrection == nullptr)
+ {
+ ARMARX_ERROR << "The global position correction sensor is not available.";
+ return;
+ }
+ if (sensorRelativePosition == nullptr)
{
- ARMARX_ERROR << "one of the sensors is not available";
+ ARMARX_ERROR << "The relative position sensor is not available.";
return;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 697b087cc7f0b53edc3196b499c0f76dd3dec312..08870cfcc27981ee9e0e97f096563a35a1f51507 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -129,8 +129,8 @@ namespace armarx
}
//visu
{
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
+ _tripRt2NonRtRobotGP.commitWrite();
}
}
@@ -281,6 +281,9 @@ namespace armarx
}
}
_tripRt2NonRt.commitWrite();
+
+ _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
+ _tripRt2NonRtRobotGP.commitWrite();
}
void
@@ -412,17 +415,13 @@ namespace armarx
NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
const Ice::Current&)
{
- std::lock_guard g{_tripFakeRobotGPWriteMutex};
- _tripFakeRobotGP.getWriteBuffer() = p;
- _tripFakeRobotGP.commitWrite();
+ ; // No longer used ...
}
void
NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
- std::lock_guard g{_tripFakeRobotGPWriteMutex};
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ ; // No longer used ...
}
void
@@ -477,7 +476,7 @@ namespace armarx
std::lock_guard g{_tripRt2NonRtMutex};
const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
- const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.getUpToDateReadBuffer();
+ const Eigen::Matrix4f fakeGP = _tripRt2NonRtRobotGP.getUpToDateReadBuffer();
const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;
if (buf.tcp != buf.tcpTarg)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index dce6aed44f8bb931baf2a598c06f127cb9b20d66..59734617b4bb75cd3599f0d2c198beb7e7694bc0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -146,8 +146,7 @@ namespace armarx
mutable std::recursive_mutex _tripRt2NonRtMutex;
TripleBuffer<RtToNonRtData> _tripRt2NonRt;
- mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
- TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+ TripleBuffer<Eigen::Matrix4f> _tripRt2NonRtRobotGP;
//publish data
std::atomic_size_t _publishWpsNum{0};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index e9aea0a9858d85e24b9e49ee088c18405af0668a..61d0d6c3a63f6a82f981eed63a61d6cabd57105e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -722,7 +722,7 @@ namespace armarx::RobotUnitModule
for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
{
const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
- if (node->isRotationalJoint() || node->isTranslationalJoint())
+ if (node->isJoint())
{
const auto& name = node->getName();
if (sensorDevices.has(name))
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 8cb11c331040c5dfd12fea456c0d2391a36c9f2b..ee8522d21984cbf6936bd1b2c259e37850398d7f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -494,10 +494,13 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
- if (!rtDataStreamingEntry.count(receiver))
+
+ if (rtDataStreamingEntry.count(receiver) == 0u)
{
- throw InvalidArgumentException{"stopDataStreaming called for a nonexistent log"};
+ ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
+ return;
}
+
ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
rtDataStreamingEntry.at(receiver).stopStreaming = true;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index a7083b662e54b0a0c02ad847c3e0f197bce74d58..cc1e89e592642c8adbd137c071023a472284e4ec 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -33,6 +33,44 @@
namespace armarx::RobotUnitModule
{
+
+ RobotDataPropertyDefinitions::RobotDataPropertyDefinitions(std::string prefix) :
+ ModuleBasePropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("RobotFileName",
+ "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject",
+ "",
+ "Project in which the robot filename is located "
+ "(if robot is loaded from an external project)");
+
+ defineOptionalProperty<std::string>(
+ "RobotName",
+ "",
+ "Override robot name if you want to load multiple robots of the same type");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName",
+ "Robot",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineOptionalProperty<std::string>(
+ "PlatformName",
+ "Platform",
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
+ true,
+ "Enable or disable the platform and localization units.");
+ defineOptionalProperty<std::string>("PlatformInstanceName",
+ "Platform",
+ "Name of the platform instance (will publish "
+ "values on PlatformInstanceName + 'State')");
+ }
+
+ bool
+ RobotData::arePlatformAndLocalizationUnitsEnabled() const
+ {
+ return _arePlatformAndLocalizationUnitsEnabled;
+ }
+
const std::string&
RobotData::getRobotPlatformName() const
{
@@ -112,6 +150,8 @@ namespace armarx::RobotUnitModule
robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
robotFileName = getProperty<std::string>("RobotFileName").getValue();
robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ _arePlatformAndLocalizationUnitsEnabled =
+ getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
@@ -160,4 +200,5 @@ namespace armarx::RobotUnitModule
robotPool.reset(new RobotPool(robot, 10));
}
}
+
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 7b5defcbfbec169a83e7185f688a5016851c24ab..c6e94bd515bea5a6f88597c935eb277c234a33d2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -40,32 +40,7 @@ namespace armarx::RobotUnitModule
class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- RobotDataPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
- {
- defineRequiredProperty<std::string>("RobotFileName",
- "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotFileNameProject",
- "",
- "Project in which the robot filename is located "
- "(if robot is loaded from an external project)");
-
- defineOptionalProperty<std::string>(
- "RobotName",
- "",
- "Override robot name if you want to load multiple robots of the same type");
- defineOptionalProperty<std::string>(
- "RobotNodeSetName",
- "Robot",
- "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineOptionalProperty<std::string>(
- "PlatformName",
- "Platform",
- "Name of the platform needs to correspond to a node in the virtual robot.");
- defineOptionalProperty<std::string>("PlatformInstanceName",
- "Platform",
- "Name of the platform instance (will publish "
- "values on PlatformInstanceName + 'State')");
- }
+ RobotDataPropertyDefinitions(std::string prefix);
};
/**
@@ -99,21 +74,26 @@ namespace armarx::RobotUnitModule
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
+ bool arePlatformAndLocalizationUnitsEnabled() const;
+
/**
* @brief Returns the name of the robot's platform
* @return The name of the robot's platform
*/
const std::string& getRobotPlatformName() const;
+
/**
* @brief Returns the name of the robot's RobotNodeSet
* @return The name of the robot's RobotNodeSet
*/
const std::string& getRobotNodetSeName() const;
+
/**
* @brief Returns the name of the project containing the robot's model
* @return The name of the project containing the robot's model
*/
const std::string& getRobotProjectName() const;
+
/**
* @brief Returns the file name of the robot's model
* @return The file name of the robot's model
@@ -150,6 +130,8 @@ namespace armarx::RobotUnitModule
std::string robotFileName;
/// @brief The name of the robot's platform
std::string robotPlatformName;
+ /// @brief Indicates whether the robot platform unit is enabled.
+ bool _arePlatformAndLocalizationUnitsEnabled = false;
/// @brief The name of the robot's platform instance
std::string robotPlatformInstanceName;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index a4d9867488a2a2f8e9cb176158ba18fc01a8c538..698df2657899773a1a111bd02ce561abd66308ef 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -21,12 +21,17 @@
*/
#include "RobotUnitModuleSelfCollisionChecker.h"
+#include <algorithm>
+#include <cstddef>
+#include <string>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
@@ -219,6 +224,58 @@ namespace armarx::RobotUnitModule
node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
}
}
+
+ // Remove / filter collision pairs according to robot model (XML: Physics/IgnoreCollision)
+ {
+ ARMARX_VERBOSE << "Removing ignored collision pairs";
+ // introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
+ std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(namePairsToCheck.begin(), namePairsToCheck.end());
+
+ const auto isCollisionIgnored = [this](const std::string& a, const std::string& b) -> bool {
+
+ if(a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
+ {
+ return false;
+ }
+
+ const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
+ const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
+
+ if(nodeA == nullptr or nodeB == nullptr)
+ {
+ return false;
+ }
+
+ const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
+ const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
+
+ if(std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) != nodesIgnoredByA.end())
+ {
+ ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
+ return true;
+ }
+
+ if(std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) != nodesIgnoredByB.end())
+ {
+ ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
+ return true;
+ }
+
+ return false;
+
+ };
+
+ validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(), validNamePairsToCheck.end(), [&isCollisionIgnored](const auto& p) -> bool {
+ const auto& [a, b] = p;
+ return isCollisionIgnored(a, b);
+ }), validNamePairsToCheck.end());
+
+ ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size()) << " collision pairs.";
+
+ // copy over name pairs which should not be ignored
+ namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
+ }
+
//collect pairs
for (const auto& pair : namePairsToCheck)
{
@@ -227,14 +284,18 @@ namespace armarx::RobotUnitModule
? floor->getSceneObject(0)
: selfCollisionAvoidanceRobot->getRobotNode(pair.first);
+ ARMARX_CHECK_NOT_NULL(first) << pair.first;
+
VirtualRobot::SceneObjectPtr second =
(pair.second == FLOOR_OBJ_STR)
? floor->getSceneObject(0)
: selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+ ARMARX_CHECK_NOT_NULL(second) << pair.second;
+
nodePairsToCheck.emplace_back(first, second);
}
- ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+ ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), namePairsToCheck.size());
}
void
@@ -303,21 +364,23 @@ namespace armarx::RobotUnitModule
};
while (true)
{
- const auto startT = std::chrono::high_resolution_clock::now();
+ const auto freq = checkFrequency.load();
+
+ core::time::Metronome metronome(Frequency::Hertz(freq));
+
//done
if (isShuttingDown())
{
return;
}
- const auto freq = checkFrequency.load();
const bool inEmergencyStop =
_module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
if (inEmergencyStop || freq == 0)
{
- ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check "
+ ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: skipping check "
<< VAROUT(freq) << " " << VAROUT(inEmergencyStop);
//currently wait
- std::this_thread::sleep_for(std::chrono::microseconds{1000});
+ std::this_thread::sleep_for(std::chrono::microseconds{1'000});
continue;
}
//update robot + check
@@ -332,6 +395,7 @@ namespace armarx::RobotUnitModule
bool allJoints0 = true;
for (const auto& node : selfCollisionAvoidanceRobotNodes)
{
+ ARMARX_CHECK_NOT_NULL(node);
if (0 != node->getJointValue())
{
allJoints0 = false;
@@ -348,6 +412,10 @@ namespace armarx::RobotUnitModule
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
const auto& pair = nodePairsToCheck.at(idx);
+
+ ARMARX_CHECK_NOT_NULL(pair.first);
+ ARMARX_CHECK_NOT_NULL(pair.second);
+
if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
pair.first, pair.second))
{
@@ -370,9 +438,16 @@ namespace armarx::RobotUnitModule
<< nodePairsToCheck.size() << " pairs";
}
}
+
//sleep remaining
- std::this_thread::sleep_until(
- startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
+ const auto duration = metronome.waitForNextTick();
+
+ if(not duration.isPositive())
+ {
+ ARMARX_WARNING << deactivateSpam(10) <<
+ "Self collision checking took too long. "
+ "Exceeding time budget by " << duration.toMilliSecondsDouble() << "ms.";
+ }
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index ad3cc0f97680eabc2519b58598823294c770733f..bd92c93f31a9007c338b9ae4bcd52d9ebe1c4d50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -163,29 +163,54 @@ namespace armarx::RobotUnitModule
void
Units::initializeDefaultUnits()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
{
+ ARMARX_TRACE;
auto guard = getGuard();
+
+ ARMARX_TRACE;
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
ARMARX_INFO << "initializing default units";
+
+ ARMARX_TRACE;
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
- ARMARX_DEBUG << "initializing LocalizationUnit";
- initializeLocalizationUnit();
- ARMARX_DEBUG << "LocalizationUnit initialized";
+ ARMARX_TRACE;
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing LocalizationUnit";
+ initializeLocalizationUnit();
+ ARMARX_DEBUG << "LocalizationUnit initialized";
+ }
+ ARMARX_TRACE;
+
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing PlatformUnit";
+ initializePlatformUnit();
+ ARMARX_DEBUG << "PlatformUnit initialized";
+ }
- initializePlatformUnit();
- ARMARX_DEBUG << "PlatformUnit initialized";
+ ARMARX_TRACE;
initializeForceTorqueUnit();
ARMARX_DEBUG << "ForceTorqueUnit initialized";
+
+ ARMARX_TRACE;
initializeInertialMeasurementUnit();
ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+
+ ARMARX_TRACE;
initializeTrajectoryControllerUnit();
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+
+ ARMARX_TRACE;
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
+
+ ARMARX_TRACE;
}
ARMARX_INFO << "initializing default units...done! "
<< (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
@@ -371,6 +396,8 @@ namespace armarx::RobotUnitModule
using UnitT = PlatformSubUnit;
using IfaceT = PlatformUnitInterface;
+ ARMARX_TRACE;
+
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -378,12 +405,14 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
//is there a platform dev?
if (_module<RobotData>().getRobotPlatformName().empty())
{
ARMARX_INFO << "no platform unit created since no platform name was given";
return;
}
+ ARMARX_TRACE;
if (!_module<Devices>().getControlDevices().has(
_module<RobotData>().getRobotPlatformName()))
{
@@ -392,16 +421,19 @@ namespace armarx::RobotUnitModule
<< _module<RobotData>().getRobotPlatformName() << "' was not found";
return;
}
+ ARMARX_TRACE;
const ControlDevicePtr& controlDevice =
_module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
const SensorDevicePtr& sensorDevice =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
JointController* jointVel =
controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(jointVel);
ARMARX_CHECK_EXPRESSION(
sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
+ ARMARX_TRACE;
const std::string configName = getProperty<std::string>("PlatformUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -411,9 +443,11 @@ namespace armarx::RobotUnitModule
_module<RobotData>().getRobotPlatformInstanceName());
ARMARX_DEBUG << "creating unit " << configName
<< " using these properties: " << properties->getPropertiesForPrefix("");
+ ARMARX_TRACE;
IceInternal::Handle<UnitT> unit =
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
+ ARMARX_TRACE;
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
@@ -427,8 +461,10 @@ namespace armarx::RobotUnitModule
config,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
+ ARMARX_TRACE;
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -457,9 +493,11 @@ namespace armarx::RobotUnitModule
configGlobalPositionCtrlCfg,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
+ ARMARX_TRACE;
unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
_module<RobotData>().getRobotPlatformName());
@@ -470,12 +508,14 @@ namespace armarx::RobotUnitModule
void
Units::initializeLocalizationUnit()
{
+ ARMARX_TRACE;
ARMARX_DEBUG << "initializeLocalizationUnit";
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = LocalizationSubUnit;
using IfaceT = LocalizationUnitInterface;
+ ARMARX_TRACE;
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -483,13 +523,18 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
+ ARMARX_CHECK(
+ _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
+ << _module<RobotData>().getRobotPlatformName();
const SensorDevicePtr& sensorDeviceRelativePosition =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
->asA<SensorValueHolonomicPlatformRelativePosition>());
+ ARMARX_TRACE;
// const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
// ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index e1c717c58e00be94df3035916b5d3d4f0a50a589..bedfc430f0dee9f2030ac495d2ba6dd74bfa1350 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -82,8 +82,8 @@ namespace armarx
ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(isInitialized);
- ARMARX_VERBOSE << VAROUT(entries.size());
+ ARMARX_CHECK(isInitialized);
+ ARMARX_DEBUG << VAROUT(entries.size());
const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
if (writePosition_local - onePastLoggingReadPosition >= numEntries)
{
@@ -107,7 +107,7 @@ namespace armarx
}
//consume all
const std::size_t num = writePosition_local - onePastLoggingReadPosition;
- ARMARX_VERBOSE << num << " new entries to be treated";
+ ARMARX_DEBUG << num << " new entries to be treated";
for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
++onePastLoggingReadPosition, ++offset)
{
@@ -276,10 +276,8 @@ namespace armarx
}
}
- const auto PotentiallyMinimizeMember = [](auto & member, bool minimize) {
- return minimize ? 0 : member;
- };
-
+ const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
+ { return minimize ? 0 : member; };
detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
bool minimize) :
@@ -301,13 +299,13 @@ namespace armarx
maxAlign{1}
{
ARMARX_DEBUG << "copying RtMessageLogBuffer with minimize = " << minimize << " "
- << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
- << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
- << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
- << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace) << " "
- << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost) << " "
- << VAROUT(maxAlign);
+ << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
+ << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
+ << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
+ << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace)
+ << " " << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost)
+ << " " << VAROUT(maxAlign);
for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
{
const RtMessageLogEntryBase* entry = other.entries.at(idx);
@@ -361,10 +359,12 @@ namespace armarx
deleteAll();
if (requiredAdditionalEntries || entries.size() < numEntries)
{
- const auto numExcessEntries = std::max(requiredAdditionalEntries, numEntries - entries.size());
+ const auto numExcessEntries =
+ std::max(requiredAdditionalEntries, numEntries - entries.size());
const auto requiredSize = entries.size() + numExcessEntries;
ARMARX_WARNING << "Iteration " << iterationCount << " required "
- << requiredAdditionalEntries << " | " << numExcessEntries << " additional message entries. \n"
+ << requiredAdditionalEntries << " | " << numExcessEntries
+ << " additional message entries. \n"
<< "The requested total number of entries is " << requiredSize << ". \n"
<< "The current number of entries is " << entries.size() << ". \n"
<< "The maximal number of entries is "
@@ -514,10 +514,10 @@ namespace armarx
{
ARMARX_TRACE;
ARMARX_DEBUG << "Copy ControlThreadOutputBufferEntry with parameters: " << VAROUT(minimize)
- << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
- << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
- << VAROUT(messages.entries.size());
+ << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
+ << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
+ << VAROUT(messages.entries.size());
void* place = buffer.data();
std::size_t space = buffer.size();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index b247f12167424f45a2e87c690461d5aaab5516d0..129771f55fec581f806de5e09d8c7a7d928f2ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -114,7 +114,7 @@ namespace armarx::introspection
const std::string& name)
{
ARMARX_TRACE;
- ARMARX_CHECK_EQUAL(0, entries.count(name));
+ ARMARX_CHECK_EQUAL(entries.count(name), 0) << name;
entries.add(name, Entry(name, ptr));
return entries.at(name);
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 1431b8b5219ef4622d21024132397690a81d9526..1e7dacc348c3101705f4ae650ab978b031cd23d7 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -33,6 +33,7 @@ void GamepadUnit::onInitComponent()
ARMARX_TRACE;
offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
+ deviceEventName = getProperty<std::string>("GamepadForceFeedbackName").getValue();
readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
}
@@ -69,15 +70,23 @@ void GamepadUnit::onConnectComponent()
ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " << getProperty<int>("PublishTimeout").getValue() << " ms";
}
}, 30);
+
sendTask->start();
ARMARX_TRACE;
openGamepadConnection();
}
+void GamepadUnit::vibrate(const ::Ice::Current&)
+{
+ ARMARX_INFO << "vibration!";
+ js.executeEffect();
+}
+
bool GamepadUnit::openGamepadConnection()
{
- if (js.open(deviceName))
+ if (js.open(deviceName, deviceEventName))
{
+
ARMARX_TRACE;
ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
@@ -185,4 +194,3 @@ armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
getConfigIdentifier()));
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 109ce81a17a98c5154b396e09064d9943be6ff0a..a2008600dc1f50432bbbf3f00c646153c7d7e83e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -53,6 +53,7 @@ namespace armarx
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+ defineOptionalProperty<std::string>("GamepadForceFeedbackName", "", "device that will be used for force feedback, leave empty to disable. See RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
}
};
@@ -69,7 +70,8 @@ namespace armarx
* Detailed description of class GamepadUnit.
*/
class GamepadUnit :
- virtual public armarx::Component
+ virtual public armarx::Component,
+ virtual public GamepadUnitInterface
{
public:
/**
@@ -108,6 +110,8 @@ namespace armarx
bool openGamepadConnection();
+ void vibrate(const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
private:
GamepadUnitListenerPrx topicPrx;
RunningTask<GamepadUnit>::pointer_type readTask;
@@ -116,9 +120,9 @@ namespace armarx
void run();
std::mutex mutex;
std::string deviceName;
+ std::string deviceEventName;
Joystick js;
GamepadData data;
TimestampVariantPtr dataTimestamp;
};
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
index 1f39074b7a1449f9d58b643d42be68f2c123c445..8befbe9d5c08bd6d13220cd7c709884c906afc7b 100644
--- a/source/RobotAPI/drivers/GamepadUnit/Joystick.h
+++ b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
@@ -22,12 +22,17 @@
#pragma once
-#include<linux/joystick.h>
-#include<sys/stat.h>
-#include<fcntl.h>
+#include <cstdint>
+
+#include <fcntl.h>
+#include <sys/poll.h>
+#include <sys/stat.h>
+#include <unistd.h>
#include <ArmarXCore/core/Component.h>
+#include <linux/joystick.h>
+
namespace armarx
{
@@ -36,21 +41,39 @@ namespace armarx
private:
int fd = -1;
+ int fdEvent = -1;
js_event event;
public:
-
std::vector<int16_t> axis;
std::vector<bool> buttonsPressed;
int numberOfAxis;
int numberOfButtons;
std::string name;
- bool open(std::string const& deviceName)
+ bool
+ open(std::string const& deviceName, std::string const& deviceEventName)
{
+
fd = ::open(deviceName.c_str(), O_RDONLY);
+
+ if (!deviceEventName.empty())
+ {
+ ARMARX_INFO << "Force feedback enabled";
+ fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
+
+ ARMARX_CHECK(fdEvent != -1);
+ } else {
+ ARMARX_INFO << "Force feedback disabled";
+ }
+
if (fd != -1)
{
+ // ARMARX_INFO << "before";
+ // executeEffect();
+ // ARMARX_INFO << "after";
+
+
ioctl(fd, JSIOCGAXES, &numberOfAxis);
ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
name.resize(255);
@@ -59,15 +82,21 @@ namespace armarx
name = name.c_str();
buttonsPressed.resize(numberOfButtons, false);
}
+
+ ARMARX_INFO << "execute effect";
+ executeEffect();
+
return fd != -1;
}
- bool opened() const
+ bool
+ opened() const
{
return fd != -1;
}
- bool pollEvent()
+ bool
+ pollEvent()
{
int bytes = read(fd, &event, sizeof(event));
@@ -89,10 +118,165 @@ namespace armarx
return true;
}
- void close()
+ void
+ executeEffect(int gain = 100, const int nTimes = 1)
+ {
+ // this feature is disabled
+ if(fdEvent < 0) return;
+
+ // see https://docs.kernel.org/input/ff.html
+
+
+ // https://xnux.eu/devices/feature/vibrator.html
+
+ int ret;
+ // pollfd pfds[1];
+ int effects;
+
+ // fd = open_event_dev("vibrator", O_RDWR | O_CLOEXEC);
+ // syscall_error(fd < 0, "Can't open vibrator event device");
+
+ ret = ioctl(fdEvent, EVIOCGEFFECTS, &effects);
+ ARMARX_CHECK(ret >= 0);
+ // syscall_error(ret < 0, "EVIOCGEFFECTS failed");
+
+ // ARMARX_CHECK(effects & FF_RUMBLE);
+
+ // Set the gain of the device
+ {
+ // int gain; between 0 and 100
+ struct input_event ie; // structure used to communicate with the driver
+
+ ie.type = EV_FF;
+ ie.code = FF_GAIN;
+ ie.value = 0xFFFFUL * gain / 100;
+
+ if (write(fdEvent, &ie, sizeof(ie)) == -1)
+ {
+ perror("set gain");
+ }
+ }
+
+
+ ff_effect e;
+ // e.type = FF_RUMBLE;
+ // e.id = -1;
+ // e.replay.length = 5000;
+ // e.replay.delay = 500;
+ // e.u.rumble.strong_magnitude = 1;
+
+ e.type = FF_PERIODIC;
+ e.id = -1;
+ e.replay.length = 5000;
+ e.replay.delay = 500;
+ e.u.periodic.waveform = FF_SQUARE;
+ e.u.periodic.period = 1000;
+ e.u.periodic.magnitude = 0xFF;
+ e.u.periodic.offset = 0xFF;
+
+ ret = ioctl(fdEvent, EVIOCSFF, &e);
+ ARMARX_CHECK(ret >= 0);
+
+ // syscall_error(ret < 0, "EVIOCSFF failed");
+
+ ARMARX_INFO << VAROUT(e.id);
+
+ input_event play;
+ play.type = EV_FF;
+ play.code = static_cast<std::uint16_t>(e.id);
+ play.value = 1;
+
+ ret = write(fdEvent, &play, sizeof(play));
+ ARMARX_CHECK(ret >= 0);
+
+ ARMARX_INFO << "Executing effect";
+
+
+ for (int i = 0; i < 5; i++)
+ {
+
+ input_event statusIe; // structure used to communicate with the driver
+ statusIe.type = EV_FF_STATUS;
+ statusIe.code = e.id;
+ // statusIe.value = 0;
+
+ ret = write(fdEvent, &statusIe, sizeof(statusIe));
+
+ // ARMARX_CHECK()
+ // ret should be FF_STATUS_PLAYING
+ ARMARX_INFO << VAROUT(ret);
+
+ sleep(1);
+ }
+
+
+ // syscall_error(ret < 0, "write failed");
+
+ ARMARX_INFO << "Executing effect";
+ // sleep(6);
+
+
+ input_event stop;
+ stop.type = EV_FF;
+ stop.code = e.id;
+ stop.value = 0;
+ const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+
+
+ ret = ioctl(fdEvent, EVIOCRMFF, e.id);
+ ARMARX_CHECK(ret >= 0);
+
+ // syscall_error(ret < 0, "EVIOCRMFF failed");
+
+ // close(fdEvent);
+
+
+ /**/
+ // Set the gain of the device
+ // {
+ // // int gain; between 0 and 100
+ // struct input_event ie; // structure used to communicate with the driver
+
+ // ie.type = EV_FF;
+ // ie.code = FF_GAIN;
+ // ie.value = 0xFFFFUL * gain / 100;
+
+ // if (write(fd, &ie, sizeof(ie)) == -1)
+ // {
+ // perror("set gain");
+ // }
+ // }
+
+
+ // struct input_event play;
+ // struct input_event stop;
+
+ // // upload request to device
+ // ff_effect effect;
+ // const auto uploadStatus = ioctl(fd, EVIOCSFF, &effect);
+
+ // // Play n times
+ // play.type = EV_FF;
+ // play.code = effect.id;
+ // play.value = nTimes;
+
+ // const int playStatus = write(fd, static_cast<const void*>(&play), sizeof(play));
+
+ // // Stop an effect
+ // stop.type = EV_FF;
+ // stop.code = effect.id;
+ // stop.value = 0;
+ // const int stopStatus = write(fd, static_cast<const void*>(&stop), sizeof(stop));
+
+ // // remove effect
+ // ioctl(fd, EVIOCRMFF, effect.id);
+ }
+
+ void
+ close()
{
::close(fd);
fd = -1;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
index 2e4c5851c2417c67158785cb31226b03fff7c269..55776817eee69b3f783cda883871b88ec9679439 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
@@ -36,11 +36,11 @@ armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget* parent) :
proxyFinderLeftHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
- proxyFinderLeftHand->setSearchMask("*Unit");
+ proxyFinderLeftHand->setSearchMask("*LeftHandUnit");
ui->proxyFinderContainerLeftHand->addWidget(proxyFinderLeftHand, 0, 0, 1, 1);
proxyFinderRightHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
- proxyFinderRightHand->setSearchMask("*Unit");
+ proxyFinderRightHand->setSearchMask("*RightHandUnit");
ui->proxyFinderContainerRightHand->addWidget(proxyFinderRightHand, 0, 0, 1, 1);
}
diff --git a/source/RobotAPI/interface/units/GamepadUnit.ice b/source/RobotAPI/interface/units/GamepadUnit.ice
index 671e1de9b99591f0d4e23ab923feac8a5e1d535d..c63ddcfdad36942cfcca6c65155c8e96c6847f79 100644
--- a/source/RobotAPI/interface/units/GamepadUnit.ice
+++ b/source/RobotAPI/interface/units/GamepadUnit.ice
@@ -62,8 +62,9 @@ module armarx
bool rightStickButton;
};
- interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
+ interface GamepadUnitInterface // extends armarx::SensorActorUnitInterface
{
+ void vibrate();
};
interface GamepadUnitListener
@@ -78,4 +79,3 @@ module armarx
};
};
-