diff --git a/etc/doxygen/pages/GuiPlugins.dox b/etc/doxygen/pages/GuiPlugins.dox
index ac33f858293e16fbf8a4fae4c146523d67162851..b8922c0ea799be41f3526fe942e8d268aa592e7d 100644
--- a/etc/doxygen/pages/GuiPlugins.dox
+++ b/etc/doxygen/pages/GuiPlugins.dox
@@ -24,4 +24,6 @@ The following Gui Plugins are available:
\subpage ArmarXGui-GuiPlugins-HomogeneousMatrixCalculator
\subpage ArmarXGui-GuiPlugins-RobotUnitPlugin
+
+\subpage ArmarXGui-GuiPlugins-GuiHealthClient
*/
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 4709de0cff7c7ce0fae9eafb740daac19b0dbe87..6d45e0068862377bf515dc52d04b77d07d7f7bcf 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -22,6 +22,8 @@
#include "RobotHealth.h"
+#include <boost/regex.h>
+
using namespace armarx;
@@ -32,6 +34,8 @@ void RobotHealth::onInitComponent()
defaultMaximumCycleTimeWarn = getProperty<int>("MaximumCycleTimeWarnMS").getValue();
defaultMaximumCycleTimeErr = getProperty<int>("MaximumCycleTimeErrMS").getValue();
usingTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
+ reportErrorsWithSpeech = getProperty<bool>("ReportErrorsWithSpeech").getValue();
+ speechMinimumReportInterval = getProperty<int>("SpeechMinimumReportInterval").getValue();
//robotUnitRequired = getProperty<bool>("RobotUnitRequired").getValue();
/*if(robotUnitRequired)
@@ -50,6 +54,8 @@ void RobotHealth::onConnectComponent()
emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(getProperty<std::string>("EmergencyStopTopicName").getValue());
//remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
aggregatedRobotHealthTopicPrx = getTopic<AggregatedRobotHealthInterfacePrx>(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
+ textToSpeechTopic = getTopic<TextListenerInterfacePrx>(getProperty<std::string>("TextToSpeechTopicName").getValue());
+ lastSpeechOutput = TimeUtil::GetTime();
/*if(robotUnitRequired)
{
@@ -86,6 +92,28 @@ void RobotHealth::monitorHealthTaskClb()
if (e.isRunning)
{
ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name << " has died.";
+ if (reportErrorsWithSpeech && (TimeUtil::GetTime() - lastSpeechOutput).toSecondsDouble() > speechMinimumReportInterval)
+ {
+ lastSpeechOutput = TimeUtil::GetTime();
+
+ std::string name;
+
+ boost::smatch m;
+ boost::regex regex("^[a-zA-Z]+");
+ if (boost::regex_search(e.name, m, regex))
+ {
+ if (m.empty())
+ {
+ name = "Whatever";
+ }
+ else
+ {
+ name = m[0].str();
+ }
+ }
+
+ textToSpeechTopic->reportText("Oh no! Component " + name + " is no longer running.");
+ }
hasNewErr = true;
e.isRunning = false;
}
@@ -151,10 +179,12 @@ RobotHealth::Entry& RobotHealth::findOrCreateEntry(const std::string& name)
-void armarx::RobotHealth::heartbeat(const std::string& componentName, const Ice::Current&)
+void RobotHealth::heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&)
{
Entry& e = findOrCreateEntry(componentName);
long now = TimeUtil::GetTime().toMicroSeconds();
+ e.maximumCycleTimeWarn = args.maximumCycleTimeWarningMS * 1000;
+ e.maximumCycleTimeErr = args.maximumCycleTimeErrorMS * 1000;
if (!e.isRunning)
{
ARMARX_INFO << "'" << componentName << "' is now alive and running";
@@ -166,16 +196,13 @@ void armarx::RobotHealth::heartbeat(const std::string& componentName, const Ice:
}
e.lastUpdate = now;
e.isRunning = true;
+ e.enabled = true;
+ {
+ ScopedLock lock(e.messageMutex);
+ e.message = args.message;
+ }
}
-void armarx::RobotHealth::reportState(const std::string& componentName, RobotHealthState state, const std::string& message, const Ice::Current&)
-{
- Entry& e = findOrCreateEntry(componentName);
- e.state = state;
- e.message = message;
-}
-
-
void armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
{
Entry& e = findOrCreateEntry(componentName);
@@ -201,21 +228,29 @@ std::string RobotHealth::getSummary(const Ice::Current&)
if (!e.required && !e.enabled)
{
- ss << ": disabled\n";
+ ss << ": disabled";
}
else if (delta > e.maximumCycleTimeErr)
{
- ss << ": ERROR (" << (delta / 1000) << "ms)\n";
+ ss << ": ERROR (" << (delta / 1000) << "ms)";
}
else if (delta > e.maximumCycleTimeWarn)
{
- ss << ": warning (" << (delta / 1000) << "ms)\n";
+ ss << ": warning (" << (delta / 1000) << "ms)";
}
else
{
- ss << ": ok (" << (delta / 1000) << "ms)\n";
+ ss << ": ok (" << (delta / 1000) << "ms)";
+ }
+
+ ScopedLock lock(e.messageMutex);
+ if (e.message.size())
+ {
+ ss << " - " << e.message;
}
+ ss << "\n";
+
}
return ss.str();
}
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index b9d8ae4a16185e3148280275490f09a72ab8781d..fa27d305b974322ea94d9e1d51d15d1899577a5f 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -30,6 +30,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <atomic>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
namespace armarx
{
@@ -46,11 +47,15 @@ namespace armarx
{
defineOptionalProperty<std::string>("EmergencyStopTopicName", "EmergencyStop", "The name of the topic over which changes of the emergencyStopState are sent.");
defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
+ defineOptionalProperty<bool>("ReportErrorsWithSpeech", true, "");
+ defineOptionalProperty<std::string>("TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeech topic");
defineOptionalProperty<int>("MaximumCycleTimeWarnMS", 50, "Default value of the maximum cycle time for warnings");
defineOptionalProperty<int>("MaximumCycleTimeErrMS", 100, "Default value of the maximum cycle time for error");
defineOptionalProperty<std::string>("AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
defineOptionalProperty<std::string>("RequiredComponents", "", "Comma separated list of required components");
+ defineOptionalProperty<int>("SpeechMinimumReportInterval", 60, "Time that has to pass between reported messages in seconds.");
+
//defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote GUI provider");
//defineOptionalProperty<std::string>("RobotUnitName", "Armar6Unit", "Name of the RobotUnit");
//defineOptionalProperty<bool>("RobotUnitRequired", true, "Wait for RobotUnit");
@@ -102,6 +107,7 @@ namespace armarx
bool isRunning = false;
bool required = false;
bool enabled = true;
+ Mutex messageMutex;
};
protected:
@@ -135,7 +141,7 @@ namespace armarx
Mutex mutex;
std::deque<Entry> entries;
- std::atomic_size_t validEntries{0};
+ std::atomic_size_t validEntries {0};
PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
int defaultMaximumCycleTimeWarn;
int defaultMaximumCycleTimeErr;
@@ -144,11 +150,14 @@ namespace armarx
//bool robotUnitRequired;
RemoteGuiInterfacePrx remoteGuiPrx;
AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
+ TextListenerInterfacePrx textToSpeechTopic;
+ bool reportErrorsWithSpeech;
+ int speechMinimumReportInterval;
+ IceUtil::Time lastSpeechOutput;
// RobotHealthInterface interface
public:
- void heartbeat(const std::string& componentName, const Ice::Current&) override;
- void reportState(const std::string& componentName, RobotHealthState state, const std::string& message, const Ice::Current&) override;
+ void heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&) override;
void unregister(const std::string& componentName, const Ice::Current&) override;
std::string getSummary(const Ice::Current&) override;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index 5a211566d869d2214821172b6cc3c6b6ffad7392..07137983032ecd6e69839959dd0e76d448ebb278 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -44,7 +44,7 @@ void RobotHealthDummy::runTask()
while (!dummyTask->isStopped())
{
//ARMARX_INFO << "send heartbeat";
- robotHealthTopicPrx->heartbeat(getName());
+ robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
TimeUtil::SleepMS(10);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index b478f9e5b2564b13a2e0e107dfae21fd7145a980..5abecbc624674ca7b139339eb7aef3ab6be58215 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -210,11 +210,11 @@ namespace armarx
rtSwitchControllerSetupChangedControllers = false;
- // a missing hearbeat (if required by the config) is interpreted as emergencyStop == true
+ // a missing heartbeat (if required by the config) is interpreted as emergencyStop == true
if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
{
rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because hearbeat is missing!");
+ ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!");
}
// !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 8748a36239c3d84cfac49cbd864727e8e8a94a15..d5db6382a36175e8fade6c1b66dca9b3ca5f6944 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -95,6 +95,7 @@ namespace armarx
{
throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
}
+ rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
auto ptr = rtLoggingEntries[pb.getPath()];
CSVLoggingEntry& e = *ptr;
e.filename = pb.getPath();
@@ -251,6 +252,7 @@ namespace armarx
}
//write message data
{
+ bool atLeastOneMessage = false;
for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
{
if (!msg)
@@ -260,11 +262,15 @@ namespace armarx
outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
<< iteration.iteration << ":\n"
<< msg->format() << std::endl;
+ atLeastOneMessage = true;
+ }
+ if (atLeastOneMessage)
+ {
+ outMsg << "\nmessages lost: " << iteration.messages.messagesLost
+ << " (required additional "
+ << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
+ << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
}
- outMsg << "\nmessages lost: " << iteration.messages.messagesLost
- << " (required additional "
- << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
- << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
}
}
}
@@ -329,18 +335,19 @@ namespace armarx
}
//sens
{
+ //sensors
for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
{
const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : rtLoggingEntries)
+ for (auto& [_, entry] : rtLoggingEntries)
{
- CSVLoggingEntry& e = *pair.second;
- if (e.loggedSensorDeviceValues.at(idxDev).at(idxField))
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- pair.second->stream << ";" << str;
+ entry->stream << ";" << str;
}
}
}
@@ -348,21 +355,23 @@ namespace armarx
}
//ctrl
{
+ //joint controllers
for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
{
const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : rtLoggingEntries)
+ for (auto& [_, entry] : rtLoggingEntries)
{
- CSVLoggingEntry& e = *pair.second;
- if (e.loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- pair.second->stream << ";" << str;
+ entry->stream << ";" << str;
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index efd3b13af170fdac610037c5960315e21552045d..df4a0f5503a21b05f1df52686ca34c42c90ae076 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -517,6 +517,8 @@ namespace armarx
additionalMap["SoftwareEmergencyStopState" ] = new TimedVariant {_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() == EmergencyStopState::eEmergencyStopActive ? "EmergencyStopActive" : "EmergencyStopInactive",
lastControlThreadTimestamp
};
+
+
//update
if (haveSensorAndControlValsChanged)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index dea0cefc31e5558e73fa18bf13d16d0fb670af61..0d0aa5b7616f7e88412d70decd10fbd545327139 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -420,12 +420,14 @@ namespace armarx
return resultPlace;
};
+ //copy sensor values
sensors.reserve(other.sensors.size());
for (const SensorValueBase* sv : other.sensors)
{
- sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
+ sensors.emplace_back(sv->_placementCopyConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
+ //copy control targets
control.reserve(other.control.size());
for (const auto& cdctargs : other.control)
{
@@ -434,8 +436,7 @@ namespace armarx
ctargs.reserve(cdctargs.size());
for (const ControlTargetBase* ct : cdctargs)
{
- ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
- ctargs.back()->reset();
+ ctargs.emplace_back(ct->_placementCopyConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
}
}
}
diff --git a/source/RobotAPI/components/units/SpeechObserver.cpp b/source/RobotAPI/components/units/SpeechObserver.cpp
index dec46e56aa59676076607cf8fa7a280b9742737c..15b2766b4f07989833590f8a3e2a0aa3bbdd134a 100644
--- a/source/RobotAPI/components/units/SpeechObserver.cpp
+++ b/source/RobotAPI/components/units/SpeechObserver.cpp
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -23,8 +23,6 @@
#include "SpeechObserver.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
using namespace armarx;
SpeechObserver::SpeechObserver()
@@ -35,9 +33,6 @@ void SpeechObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
usingTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
- auto proxy = getObjectAdapter()->addWithUUID(new SpeechListenerImpl(this));
- getIceManager()->subscribeTopic(proxy, "Speech_Commands");
-
}
void SpeechObserver::onConnectObserver()
@@ -47,16 +42,12 @@ void SpeechObserver::onConnectObserver()
offerDataFieldWithDefault("TextToSpeech", "TextChangeCounter", Variant(reportTextCounter), "Counter for text updates");
offerDataFieldWithDefault("TextToSpeech", "State", Variant(""), "Current TTS state");
- offerDataFieldWithDefault("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter), "Counter for state updates");
-
- offerChannel("SpeechToText", "SpeechToText channel");
- offerDataFieldWithDefault("TextToSpeech", "RecognizedText", Variant(std::string()), "Text recognized by the SpeechRecogntion");
-
+ offerDataFieldWithDefault("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter), "Counter for text updates");
}
std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
{
- switch (state)
+ switch(state)
{
case eIdle:
return "Idle";
@@ -79,7 +70,7 @@ void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current
updateChannel("TextToSpeech");
}
-void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
+void SpeechObserver::reportText(const std::string& text, const Ice::Current&)
{
ScopedLock lock(dataMutex);
reportTextCounter++;
@@ -93,24 +84,3 @@ void SpeechObserver::reportTextWithParams(const std::string& text, const Ice::St
ScopedLock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
-
-SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) :
- obs(obs)
-{
-
-}
-
-
-void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- JSONObject json;
- json.fromString(text);
- obs->setDataField("SpeechToText", "RecognizedText", Variant(json.getString("text")));
-}
-
-void armarx::SpeechListenerImpl::reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&)
-{
- ScopedLock lock(dataMutex);
- ARMARX_WARNING << deactivateSpam(100) << "reportTextWithParams is not implemented";
-}
diff --git a/source/RobotAPI/components/units/SpeechObserver.h b/source/RobotAPI/components/units/SpeechObserver.h
index 2323a6d06b646368b4854f3d0a6cdbb481c76be0..769677e2759f0f7934ced448a3db51ef48db82e1 100644
--- a/source/RobotAPI/components/units/SpeechObserver.h
+++ b/source/RobotAPI/components/units/SpeechObserver.h
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -39,25 +39,11 @@ namespace armarx
defineOptionalProperty<std::string>("TextToSpeechStateTopicName", "TextToSpeechState", "Name of the TextToSpeechStateTopic");
}
};
- class SpeechObserver;
- class SpeechListenerImpl : public TextListenerInterface
- {
- public:
- SpeechListenerImpl(SpeechObserver* obs);
- protected:
- SpeechObserver* obs;
- Mutex dataMutex;
- // TextListenerInterface interface
- public:
- void reportText(const std::string&, const Ice::Current&) override;
- void reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
- };
class SpeechObserver :
- virtual public Observer,
- virtual public SpeechObserverInterface
+ virtual public Observer,
+ virtual public SpeechObserverInterface
{
- friend class SpeechListenerImpl;
public:
SpeechObserver();
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 4ee7f8ec607a5754a93a30c1a1cb49c040932b69..c257210ebe454a3cbe88c405bb90aab5e0af1582 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -32,6 +32,8 @@ using namespace armarx;
void HokuyoLaserUnit::onInitComponent()
{
+ offeringTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
+
topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
offeringTopic(topicName);
ARMARX_INFO << "Going to report on topic " << topicName;
@@ -75,6 +77,7 @@ void HokuyoLaserUnit::onInitComponent()
void HokuyoLaserUnit::onConnectComponent()
{
+ robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
if (task)
@@ -165,6 +168,27 @@ void HokuyoLaserUnit::updateScanData()
TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
for (HokuyoLaserScanDevice& device : devices)
{
+ if (device.errorCounter > 10)
+ {
+ ARMARX_ERROR << "Device " << device.ip << " has too many consecutive errors!";
+ // assume dead
+ device.connected = false;
+
+ ARMARX_WARNING << "Reconnecting to " << device.ip << ":" << device.port;
+ int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
+ device.connected = (ret == 0);
+ if (device.connected)
+ {
+ ARMARX_IMPORTANT << "Reconnected succesffully to " << device.ip << ":" << device.port;
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
+ << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+ continue;
+ }
+
+ }
if (device.connected)
{
int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
@@ -172,6 +196,7 @@ void HokuyoLaserUnit::updateScanData()
{
ARMARX_WARNING << deactivateSpam(1) << "Could not start measurement for laser scanner (IP: "
<< device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+ device.errorCounter++;
continue;
}
@@ -180,6 +205,7 @@ void HokuyoLaserUnit::updateScanData()
{
ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
<< device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
+ device.errorCounter++;
continue;
}
@@ -198,7 +224,10 @@ void HokuyoLaserUnit::updateScanData()
}
}
+ device.errorCounter = 0;
+
topic->reportSensorValues(device.ip, device.frame, scan, now);
+ robotHealthTopic->heartbeat(getName() + "_" + device.ip, RobotHealthHeartbeatArgs());
}
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 78feddf97de34cf24143eddec3c311a81c5c6a69..9aedab7bee4f27784cc02bd9f5e6d7a5562aa0cd 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <HokuyoLaserScannerDriver/urg_sensor.h>
#include <vector>
@@ -47,6 +48,7 @@ namespace armarx
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
+ defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
}
};
@@ -58,6 +60,7 @@ namespace armarx
bool connected = false;
urg_t urg;
std::vector<long> lengthData;
+ int errorCounter = 0;
};
/**
@@ -125,6 +128,7 @@ namespace armarx
std::vector<HokuyoLaserScanDevice> devices;
PeriodicTask<HokuyoLaserUnit>::pointer_type task;
LaserScannerInfoSeq connectedDevices;
+ RobotHealthInterfacePrx robotHealthTopic;
};
}
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index 08961cbed9f7226826721b7acb182049fcc5800c..4b588f433f2dfa75b02e75d26e487e2703fb5811 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -68,7 +68,10 @@ void GuiHealthClientWidgetController::onInitComponent()
void GuiHealthClientWidgetController::healthTimerClb()
{
- robotHealthTopicPrx->heartbeat(getName());
+ RobotHealthHeartbeatArgs rhha;
+ rhha.maximumCycleTimeWarningMS = 250;
+ rhha.maximumCycleTimeErrorMS = 500;
+ robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
}
void GuiHealthClientWidgetController::updateSummaryTimerClb()
{
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index 45afc70dc49ba6af74abb61c83258d4fea1c214c..bbad059d5a7468428a5a351b07b95e64bc27532b 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -29,10 +29,16 @@ module armarx
HealthOK, HealthWarning, HealthError
};
+ struct RobotHealthHeartbeatArgs
+ {
+ int maximumCycleTimeWarningMS = 100;
+ int maximumCycleTimeErrorMS = 200;
+ string message;
+ };
+
interface RobotHealthInterface
{
- void heartbeat(string componentName);
- void reportState(string componentName, RobotHealthState state, string message);
+ void heartbeat(string componentName, RobotHealthHeartbeatArgs args);
void unregister(string componentName);
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 751814b46d3f9f24e2087ac0c19b6cba422f2bea..dfb40405adbc4a8613233833a252145fbc8d2c57 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -129,7 +129,7 @@ namespace armarx
return;
}
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
+ double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
@@ -162,6 +162,9 @@ namespace armarx
getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
+ getWriterControlStruct().canVal = dmpCtrl->canVal;
+ getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
+
writeControlStruct();
}
@@ -204,7 +207,7 @@ namespace armarx
currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
+ Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(- currentTCPLinearVelocity);
Eigen::Vector3f currentTCPAngularVelocity;
currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
@@ -222,6 +225,10 @@ namespace armarx
Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+
+
+
+
// torque limit
ARMARX_CHECK_EXPRESSION(!targets.empty());
ARMARX_CHECK_LESS(targets.size(), 1000);
@@ -248,7 +255,17 @@ namespace armarx
}
}
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
+
+ debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
+ debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
+ debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
+ debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
+ debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
+ debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
+
+ debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
+ debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
+
debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
@@ -258,10 +275,6 @@ namespace armarx
debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
-
-
-
-
debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
@@ -270,6 +283,7 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
+ debugOutputData.getWriteBuffer().deltaT = deltaT;
debugOutputData.commitWrite();
@@ -358,17 +372,17 @@ namespace armarx
auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
for (auto& pair : values)
{
- datafields[pair.first] = new Variant(pair.second);
+ datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
}
auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
for (auto& pair : values_null)
{
- datafields[pair.first] = new Variant(pair.second);
+ datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
}
datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
-
+ datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
@@ -385,6 +399,14 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
+ datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
+ datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
+ datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
+ datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
+ datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
+
+ datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
debugObs->setDebugChannel(channelName, datafields);
@@ -393,7 +415,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::onInitComponent()
{
ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 0.3);
+ controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
}
void NJointTaskSpaceImpedanceDMPController::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index aa3ca440da64ff775c18d295a15a3ad252e5881f..143d752d358edf14a5abd5497649679b4aef8589 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -27,6 +27,8 @@ namespace armarx
Eigen::VectorXf targetVel;
Eigen::Matrix4f targetPose;
Eigen::VectorXf desiredNullSpaceJointValues;
+ double canVal;
+ double mpcFactor;
};
@@ -78,6 +80,7 @@ namespace armarx
struct DebugBufferData
{
double currentCanVal;
+ double mpcfactor;
float targetPose_x;
float targetPose_y;
float targetPose_z;
@@ -96,6 +99,14 @@ namespace armarx
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
+ float forceDesired_x;
+ float forceDesired_y;
+ float forceDesired_z;
+ float forceDesired_rx;
+ float forceDesired_ry;
+ float forceDesired_rz;
+
+ float deltaT;
};