Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • sw/armarx/robot-api
  • uwkce_singer/robot-api
  • untcg_hofmann/robot-api
  • ulqba_korosakov/RobotAPI
4 results
Show changes
Commits on Source (47)
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1349 additions and 651 deletions
source/RobotAPI/components/ArViz/Client/Client.h
View file @ 4ee3e4f3
......@@ -80,8 +80,8 @@ namespace viz
InteractionFeedbackRange interactions() const
{
InteractionFeedback* begin = (InteractionFeedback*) data_.interactions.data();
InteractionFeedback* end = begin + data_.interactions.size();
const InteractionFeedback* begin = reinterpret_cast<const InteractionFeedback*>(data_.interactions.data());
const InteractionFeedback* end = begin + data_.interactions.size();
return InteractionFeedbackRange{begin, end};
}
......
source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
View file @ 4ee3e4f3
......@@ -82,7 +82,7 @@ namespace armarx::viz
vertices.reserve(p3.size_of_vertices());
faces.reserve(p3.size_of_facets());
auto vbeg = p3.vertices_begin();
for (const auto& v : IteratorRange{vbeg, p3.vertices_end()})
for (const auto& v : CGAL::Iterator_range{vbeg, p3.vertices_end()})
{
const auto& p = v.point();
armarx::Vector3f visp;
......@@ -91,7 +91,7 @@ namespace armarx::viz
visp.e2 = p.z();
vertices.emplace_back(visp);
}
for (const auto& fidx : IteratorRange{p3.facets_begin(), p3.facets_end()})
for (const auto& fidx : CGAL::Iterator_range{p3.facets_begin(), p3.facets_end()})
{
auto circ = fidx.facet_begin();
ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ))
......
source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
View file @ 4ee3e4f3
......@@ -71,8 +71,8 @@ namespace armarx::viz
PointCloud& points(std::vector<ColoredPoint> const& ps)
{
std::size_t memorySize = ps.size() * sizeof(ps[0]);
Ice::Byte* begin = (Ice::Byte*)ps.data();
Ice::Byte* end = begin + memorySize;
const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(ps.data());
const Ice::Byte* const end = begin + memorySize;
data_->points.assign(begin, end);
return *this;
}
......@@ -92,8 +92,8 @@ namespace armarx::viz
PointCloud& addPointUnchecked(ColoredPoint const& p)
{
Ice::Byte* begin = (Ice::Byte*)&p;
Ice::Byte* end = begin + sizeof(p);
const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(&p);
const Ice::Byte* const end = begin + sizeof(p);
data_->points.insert(data_->points.end(), begin, end);
return *this;
}
......@@ -391,5 +391,3 @@ namespace armarx::viz
};
}
source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
View file @ 4ee3e4f3
armarx_component_set_name("GamepadControlUnit")
set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIInterfaces )
set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIComponentPlugins RobotAPIInterfaces )
set(SOURCES GamepadControlUnit.cpp)
set(HEADERS GamepadControlUnit.h)
......
source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
View file @ 4ee3e4f3
......@@ -22,15 +22,15 @@
#include "GamepadControlUnit.h"
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
namespace armarx
{
void GamepadControlUnit::onInitComponent()
void
GamepadControlUnit::onInitComponent()
{
ARMARX_INFO << "oninit GamepadControlUnit";
usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
......@@ -41,67 +41,80 @@ namespace armarx
scaleY = getProperty<float>("ScaleY").getValue();
scaleRotation = getProperty<float>("ScaleAngle").getValue();
ARMARX_INFO << "oninit GamepadControlUnit end";
}
void GamepadControlUnit::onConnectComponent()
void
GamepadControlUnit::onConnectComponent()
{
ARMARX_INFO << "onConnect GamepadControlUnit";
platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(
getProperty<std::string>("PlatformUnitName").getValue());
emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster");
}
void GamepadControlUnit::onDisconnectComponent()
{
if (enableHeartBeat)
{
this->heartbeatPlugin->signUp(armarx::core::time::Duration::MilliSeconds(1000),
armarx::core::time::Duration::MilliSeconds(1500),
{"Gamepad"},
"The GamepadControlUnit");
}
}
void
GamepadControlUnit::onDisconnectComponent()
{
}
void GamepadControlUnit::onExitComponent()
void
GamepadControlUnit::onExitComponent()
{
ARMARX_INFO << "exit GamepadControlUnit";
}
armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr
GamepadControlUnit::createPropertyDefinitions()
{
auto defs = armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
getConfigIdentifier()));
defs->topic(robotHealthTopic);
defs->optional(enableHeartBeat, "EnableHeartBeat", "Flag to enable send a heart beat to the robot healh topic");
return defs;
auto defs = armarx::PropertyDefinitionsPtr(
new GamepadControlUnitPropertyDefinitions(getConfigIdentifier()));
defs->optional(enableHeartBeat,
"EnableHeartBeat",
"Flag to enable send a heart beat to the robot healh topic");
return defs;
}
void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
void
GamepadControlUnit::reportGamepadState(const std::string& device,
const std::string& name,
const GamepadData& data,
const TimestampBasePtr& timestamp,
const Ice::Current& c)
{
// struct GamepadData {
// float leftStickX;
// float leftStickY;
// float rightStickX;
// float rightStickY;
// float dPadX;
// float dPadY;
// float leftTrigger;
// float rightTrigger;
//
// bool leftButton;
// bool rightButton;
// bool backButton;
// bool startButton;
// bool xButton;
// bool yButton;
// bool aButton;
// bool bButton;
// bool theMiddleButton;
// bool leftStickButton;
// bool rightStickButton;
//
// };
// struct GamepadData {
// float leftStickX;
// float leftStickY;
// float rightStickX;
// float rightStickY;
// float dPadX;
// float dPadY;
// float leftTrigger;
// float rightTrigger;
//
// bool leftButton;
// bool rightButton;
// bool backButton;
// bool startButton;
// bool xButton;
// bool yButton;
// bool aButton;
// bool bButton;
// bool theMiddleButton;
// bool leftStickButton;
// bool rightStickButton;
//
// };
if (data.leftTrigger > 0)
......@@ -116,7 +129,9 @@ namespace armarx
//scales are for the robot axis
if (data.rightTrigger > 0)
{
platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
platformUnitPrx->move(data.leftStickY * scaleX,
data.leftStickX * scaleY,
data.rightStickX * scaleRotation);
}
else
{
......@@ -124,64 +139,68 @@ namespace armarx
}
if (data.leftButton)
{
if(leftHandTime <= 0.0)
{
leftHandTime = IceUtil::Time::now().toMicroSeconds();
}
else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000* 1000)
{
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
if (handUnit)
{
std::string shapeName = (leftHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName);
leftHandOpen = !leftHandOpen;
leftHandTime = 0.0;
}
}
}
else
{
leftHandTime = 0.0;
}
if(data.rightButton)
{
if(rightHandTime <= 0.0)
{
rightHandTime = IceUtil::Time::now().toMicroSeconds();
}
else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000* 1000)
{
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
if (handUnit)
{
std::string shapeName = (rightHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName);
rightHandOpen = !rightHandOpen;
rightHandTime = 0.0;
}
}
}
else
{
rightHandTime = 0.0;
}
if(enableHeartBeat)
{
RobotHealthHeartbeatArgs args;
args.maximumCycleTimeErrorMS = 1000;
robotHealthTopic->heartbeat(getDefaultName(), args);
}
if (data.leftButton)
{
if (leftHandTime <= 0.0)
{
leftHandTime = IceUtil::Time::now().toMicroSeconds();
}
else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000 * 1000)
{
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
if (handUnit)
{
std::string shapeName = (leftHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName);
leftHandOpen = !leftHandOpen;
leftHandTime = 0.0;
}
}
}
else
{
leftHandTime = 0.0;
}
if (data.rightButton)
{
if (rightHandTime <= 0.0)
{
rightHandTime = IceUtil::Time::now().toMicroSeconds();
}
else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000 * 1000)
{
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
if (handUnit)
{
std::string shapeName = (rightHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName);
rightHandOpen = !rightHandOpen;
rightHandTime = 0.0;
}
}
}
else
{
rightHandTime = 0.0;
}
if (enableHeartBeat)
{
auto now = armarx::core::time::dto::DateTime();
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
heartbeatPlugin->heartbeat();
}
//ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
}
GamepadControlUnit::GamepadControlUnit()
{
addPlugin(heartbeatPlugin);
}
} // namespace armarx
source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
View file @ 4ee3e4f3
......@@ -30,6 +30,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
......@@ -72,6 +73,8 @@ namespace armarx
virtual public GamepadUnitListener
{
public:
GamepadControlUnit();
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
......@@ -109,10 +112,11 @@ namespace armarx
private:
PlatformUnitInterfacePrx platformUnitPrx;
plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
bool enableHeartBeat = false;
RobotHealthInterfacePrx robotHealthTopic;
bool enableHeartBeat = false;
float scaleX;
float scaleY;
float scaleRotation;
......
source/RobotAPI/components/RobotHealth/RobotHealth.h
View file @ 4ee3e4f3
......@@ -22,54 +22,28 @@
#pragma once
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <array>
#include <atomic>
#include <deque>
#include <mutex>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <atomic>
#include <mutex>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
namespace armarx
{
/**
* @class RobotHealthPropertyDefinitions
* @brief
*/
class RobotHealthPropertyDefinitions:
public armarx::ComponentPropertyDefinitions
{
public:
RobotHealthPropertyDefinitions(std::string prefix):
armarx::ComponentPropertyDefinitions(prefix)
{
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");
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
}
};
/**
* @defgroup Component-RobotHealth RobotHealth
* @ingroup RobotAPI-Components
......@@ -90,34 +64,12 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
std::string getDefaultName() const override
std::string
getDefaultName() const override
{
return "RobotHealth";
}
struct Entry
{
Entry(std::string name, long maximumCycleTimeWarn, long maximumCycleTimeErr)
: name(name), maximumCycleTimeWarn(maximumCycleTimeWarn), maximumCycleTimeErr(maximumCycleTimeErr), history(100, -1)
{}
Entry(Entry&&) = default;
Entry& operator=(Entry&&) = default;
std::string name;
long maximumCycleTimeWarn;
long maximumCycleTimeErr;
long lastUpdate = 0;
long lastDelta = 0;
RobotHealthState state = HealthOK;
std::string message = "";
bool isRunning = false;
bool required = false;
bool enabled = true;
std::mutex messageMutex;
std::vector<long> history;
size_t historyIndex = 0;
};
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
......@@ -144,32 +96,100 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void monitorHealthTaskClb();
Entry& findOrCreateEntry(const std::string& name);
private:
struct UpdateEntry
{
UpdateEntry(const std::string& name,
const armarx::core::time::Duration& maximumCycleTimeWarn,
const armarx::core::time::Duration& maximumCycleTimeErr) :
name(name),
maximumCycleTimeWarn(maximumCycleTimeWarn),
maximumCycleTimeErr(maximumCycleTimeErr)
{
}
void reportDebugObserver();
std::string name;
std::vector<std::string> tags;
RobotHealthState state = HealthOK;
std::string description = "";
bool required = false;
bool enabled = false;
mutable std::shared_mutex mutex;
std::mutex mutex;
std::deque<Entry> entries;
std::atomic_size_t validEntries {0};
PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
int defaultMaximumCycleTimeWarn;
int defaultMaximumCycleTimeErr;
EmergencyStopListenerPrx emergencyStopTopicPrx;
//RobotUnitInterfacePrx robotUnitPrx;
//bool robotUnitRequired;
RemoteGuiInterfacePrx remoteGuiPrx;
AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
TextListenerInterfacePrx textToSpeechTopic;
bool reportErrorsWithSpeech;
int speechMinimumReportInterval;
IceUtil::Time lastSpeechOutput;
struct TimeInfo
{
//< Timestamp sent by component
armarx::core::time::DateTime referenceTime;
//< Timestamp on this PC, set by this component
armarx::core::time::DateTime arrivalTime;
};
std::deque<TimeInfo> history;
armarx::core::time::Duration maximumCycleTimeWarn;
armarx::core::time::Duration maximumCycleTimeErr;
};
void monitorHealthUpdateTaskClb();
UpdateEntry* findUpdateEntry(const std::string& name);
std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name);
void reportDebugObserver();
// RobotHealthInterface interface
public:
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;
void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
void unregister(const std::string& identifier, const Ice::Current&) override;
void addRequiredTags(const std::string& requesterIdentifier,
const std::vector<std::string>& tags,
const Ice::Current& current) override;
void removeRequiredTags(const std::string& requesterIdentifier,
const std::vector<std::string>& tags,
const Ice::Current& current) override;
void resetRequiredTags(const Ice::Current& current) override;
void heartbeat(const std::string& identifier,
const core::time::dto::DateTime& referenceTime,
const Ice::Current& current) override;
std::string getTopicName(const Ice::Current& current) override;
// RobotHealthComponentInterface interface
public:
RobotHealthInfo getSummary(const Ice::Current& current) override;
private:
void updateRequiredElements();
std::set<std::string> requestedTags() const;
mutable std::shared_mutex updateMutex;
std::deque<UpdateEntry> updateEntries;
std::map<std::string, std::set<std::string>> tagsPerRequester;
PeriodicTask<RobotHealth>::pointer_type monitorUpdateHealthTask;
armarx::core::time::Duration defaultMaximumCycleTimeWarn;
armarx::core::time::Duration defaultMaximumCycleTimeErr;
struct Properties
{
EmergencyStopListenerPrx emergencyStopTopicPrx;
std::string robotHealthTopicName = "RobotHealthTopic";
AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
RemoteGuiInterfacePrx remoteGuiPrx;
long maximumCycleTimeWarnMS = 50;
long maximumCycleTimeErrMS = 100;
std::string requiredTags;
} p;
};
}
} // namespace armarx
source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
View file @ 4ee3e4f3
......@@ -21,26 +21,35 @@
*/
#include "RobotHealthDummy.h"
#include <time.h>
#include <thread>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
namespace armarx
{
void RobotHealthDummy::onInitComponent()
void
RobotHealthDummy::onInitComponent()
{
dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
getProperty(sleepmode, "SleepMode");
}
void RobotHealthDummy::onConnectComponent()
void
RobotHealthDummy::onConnectComponent()
{
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(
getProperty<std::string>("RobotHealthTopicName").getValue());
dummyTask->start();
}
#define NANOSECS_PER_SEC 1000000000
int RobotHealthDummy::NanoSleep(long nanosec)
#define NANOSECS_PER_SEC 1000000000
int
RobotHealthDummy::NanoSleep(long nanosec)
{
struct timespec ts;
ts.tv_sec = nanosec / NANOSECS_PER_SEC;
......@@ -48,48 +57,55 @@ namespace armarx
return nanosleep(&ts, nullptr); // jitter up to +100ms!
}
void RobotHealthDummy::sleepwait(long microseconds)
void
RobotHealthDummy::sleepwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
NanoSleep(1000);
}
while (TimeUtil::GetTime().toMicroSeconds() < end);
} while (TimeUtil::GetTime().toMicroSeconds() < end);
}
void RobotHealthDummy::yieldwait(long microseconds)
void
RobotHealthDummy::yieldwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
std::this_thread::yield();
}
while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
} while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
}
void RobotHealthDummy::busydwait(long microseconds)
void
RobotHealthDummy::busydwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
;
}
while (TimeUtil::GetTime().toMicroSeconds() < end);
} while (TimeUtil::GetTime().toMicroSeconds() < end);
}
void RobotHealthDummy::runTask()
void
RobotHealthDummy::runTask()
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
//ARMARX_INFO << "send heartbeat";
robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
if (sleepmode == "nanosleep")
{
......@@ -126,23 +142,24 @@ namespace armarx
}
}
void RobotHealthDummy::onDisconnectComponent()
void
RobotHealthDummy::onDisconnectComponent()
{
//ARMARX_IMPORTANT << "onDisconnectComponent";
dummyTask->stop();
}
void RobotHealthDummy::onExitComponent()
void
RobotHealthDummy::onExitComponent()
{
//ARMARX_IMPORTANT << "onExitComponent";
dummyTask->stop();
}
armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr
RobotHealthDummy::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions(
getConfigIdentifier()));
return armarx::PropertyDefinitionsPtr(
new RobotHealthDummyPropertyDefinitions(getConfigIdentifier()));
}
}
} // namespace armarx
source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
View file @ 4ee3e4f3
......@@ -24,10 +24,10 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx
{
......@@ -35,15 +35,18 @@ namespace armarx
* @class RobotHealthDummyPropertyDefinitions
* @brief
*/
class RobotHealthDummyPropertyDefinitions:
public armarx::ComponentPropertyDefinitions
class RobotHealthDummyPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
RobotHealthDummyPropertyDefinitions(std::string prefix):
RobotHealthDummyPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
defineOptionalProperty<std::string>("SleepMode", "nanosleep", "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
defineOptionalProperty<std::string>(
"RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
defineOptionalProperty<std::string>(
"SleepMode",
"nanosleep",
"Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
......@@ -61,14 +64,14 @@ namespace armarx
*
* Detailed description of class RobotHealthDummy.
*/
class RobotHealthDummy :
virtual public armarx::Component
class RobotHealthDummy : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
std::string getDefaultName() const override
std::string
getDefaultName() const override
{
return "RobotHealthDummy";
}
......@@ -77,6 +80,7 @@ namespace armarx
void yieldwait(long microseconds);
void busydwait(long microseconds);
void sleepwait(long microseconds);
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
......@@ -109,6 +113,5 @@ namespace armarx
RunningTask<RobotHealthDummy>::pointer_type dummyTask;
std::string sleepmode;
};
}
} // namespace armarx
source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
View file @ 4ee3e4f3
......@@ -10,7 +10,8 @@ set(COMPONENT_LIBS
RobotAPICore RobotAPIInterfaces
armem
aronopencvconverter aronjsonconverter
# ExampleMemory_aron
ExampleMemory # actually, the aron version is sufficient
${OpenCV_LIBRARIES}
)
......
source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
View file @ 4ee3e4f3
......@@ -20,13 +20,13 @@ An example data containing different native ARON types.
<GenerateTypes>
<Object name='armarx::armem::example::InnerClass'>
<ObjectChild key='element_int'>
<Int />
<int32 default="42" />
</ObjectChild>
<ObjectChild key='element_float'>
<Float />
<float32 default="666.66" />
</ObjectChild>
<ObjectChild key='element_string'>
<String />
<String default="foobar" />
</ObjectChild>
</Object>
......
source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
View file @ 4ee3e4f3
......@@ -78,7 +78,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries)
{
ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
ARMARX_VERBOSE << "There are " << writePosition - onePastLoggingReadPosition
<< " unlogged entries, but only the last " << numEntries << " are saved! "
"There seems to be something wrong (e.g. the rt logging threw an exception, "
"the system load is too high or the logging takes to long). "
......@@ -89,7 +89,7 @@ namespace armarx
auto numNewEntries = writePosition - onePastLoggingReadPosition;
if (numNewEntries >= numEntries)
{
ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
return;
}
//consume all
......@@ -109,7 +109,7 @@ namespace armarx
auto newNumNewEntries = writePosition - onePastLoggingReadPosition;
if (newNumNewEntries * 2 > numEntries)
{
ARMARX_WARNING << deactivateSpam(0.5)
ARMARX_VERBOSE << deactivateSpam(5)
<< "RT-Logging is slow! "
<< "The RT-Thread writes faster new data than the RT-Logging thread consumes it! "
<< " old/new/max number of entries: "
......@@ -118,7 +118,7 @@ namespace armarx
numNewEntries = newNumNewEntries;
if (numNewEntries >= numEntries)
{
ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
return;
}
}
......@@ -132,7 +132,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries)
{
ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
ARMARX_VERBOSE << "There are " << writePosition - onePastLoggingReadPosition
<< " unlogged entries, but only the last " << numEntries << " are saved! "
"There seems to be something wrong (e.g. the rt logging threw an exception, "
"the system load is too high or the logging takes to long). "
......
source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
View file @ 4ee3e4f3
......@@ -22,17 +22,25 @@
#include "HokuyoLaserUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <HokuyoLaserScannerDriver/urg_utils.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <HokuyoLaserScannerDriver/urg_utils.h>
using namespace armarx;
HokuyoLaserUnit::HokuyoLaserUnit()
{
addPlugin(heartbeat);
}
void HokuyoLaserUnit::onInitComponent()
void
HokuyoLaserUnit::onInitComponent()
{
offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName");
......@@ -72,19 +80,21 @@ void HokuyoLaserUnit::onInitComponent()
}
catch (std::exception const& ex)
{
ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
<< " (port is " << deviceInfo[1] << ") : " << ex.what();
ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
<< deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
continue;
}
}
}
void HokuyoLaserUnit::onConnectComponent()
void
HokuyoLaserUnit::onConnectComponent()
{
robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
ARMARX_TRACE;
topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
debugObserver = getTopic<DebugObserverInterfacePrx>(
getProperty<std::string>("DebugObserverName").getValue());
connectedDevices.clear();
for (HokuyoLaserScanDevice& device : devices)
......@@ -95,12 +105,21 @@ void HokuyoLaserUnit::onConnectComponent()
device.task = nullptr;
}
// connecting devices for first time
if (!device.reconnect())
{
ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip << ", Port: " << device.port;
ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip
<< ", Port: " << device.port;
continue;
}
ARMARX_CHECK_NOT_NULL(heartbeat);
heartbeat->signUp(device.ip,
armarx::core::time::Duration::MilliSeconds(500),
armarx::core::time::Duration::MilliSeconds(800),
{"LaserScanner", "Localization"},
"HokuyoLaserScanDevice");
LaserScannerInfo info;
info.device = device.ip;
info.frame = device.frame;
......@@ -115,23 +134,22 @@ void HokuyoLaserUnit::onConnectComponent()
device.lengthData.resize(lengthDataSize);
device.scanTopic = topic;
device.robotHealthTopic = robotHealthTopic;
device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
connectedDevices.push_back(info);
ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
<< info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
<< ", " << info.maxAngle << ", " << info.stepSize;
device.task = new RunningTask<HokuyoLaserScanDevice>(&device, &HokuyoLaserScanDevice::run,
"HokuyoLaserScanUpdate_" + device.ip);
device.task = new RunningTask<HokuyoLaserScanDevice>(
&device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
device.task->start();
}
}
void HokuyoLaserUnit::onDisconnectComponent()
void
HokuyoLaserUnit::onDisconnectComponent()
{
for (HokuyoLaserScanDevice& device : devices)
{
......@@ -148,29 +166,32 @@ void HokuyoLaserUnit::onDisconnectComponent()
}
}
void HokuyoLaserUnit::onExitComponent()
void
HokuyoLaserUnit::onExitComponent()
{
}
armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr
HokuyoLaserUnit::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions(
getConfigIdentifier()));
return armarx::PropertyDefinitionsPtr(
new HokuyoLaserUnitPropertyDefinitions(getConfigIdentifier()));
}
std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
return topicName;
}
LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
LaserScannerInfoSeq
HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
return connectedDevices;
}
bool HokuyoLaserScanDevice::reconnect()
bool
HokuyoLaserScanDevice::reconnect()
{
if (connected)
{
......@@ -183,8 +204,8 @@ bool HokuyoLaserScanDevice::reconnect()
connected = (ret == 0);
if (!connected)
{
ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
<< ip << ", Port: " << port << ", Error: " << ret << ")";
ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
<< ", Port: " << port << ", Error: " << ret << ")";
return false;
}
ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
......@@ -196,21 +217,24 @@ bool HokuyoLaserScanDevice::reconnect()
}
else
{
ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: "
<< ip << ", Port: " << port << ", Error: " << ret << ")";
ARMARX_WARNING
<< "Could not start measurement for laser scanner device using URG driver (IP: " << ip
<< ", Port: " << port << ", Error: " << ret << ")";
return false;
}
}
void HokuyoLaserScanDevice::run()
void
HokuyoLaserScanDevice::run()
{
ARMARX_TRACE;
while (!task->isStopped())
{
IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10)
{
ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
// assume dead
reconnect();
}
......@@ -219,8 +243,9 @@ void HokuyoLaserScanDevice::run()
int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
if (lengthDataSize < 0)
{
ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
<< ip << ", Port: " << port << ", Error: " << lengthDataSize << ")";
ARMARX_WARNING << deactivateSpam(1)
<< "Could not get measurement for laser scanner (IP: " << ip
<< ", Port: " << port << ", Error: " << lengthDataSize << ")";
errorCounter++;
continue;
}
......@@ -236,7 +261,8 @@ void HokuyoLaserScanDevice::run()
// TODO: Extract the min and max valid value for distance into parameters?
if (distance >= 21 && distance <= 30000)
{
step.angle = angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
step.angle =
angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
step.distance = (float)distance; // Data is already in mm
scan.push_back(step);
}
......@@ -256,16 +282,14 @@ void HokuyoLaserScanDevice::run()
}
IceUtil::Time time_topicSensor = TimeUtil::GetTime();
RobotHealthHeartbeatArgs args;
args.maximumCycleTimeWarningMS = 500;
args.maximumCycleTimeErrorMS = 800;
if (robotHealthTopic)
if (robotHealthPlugin != nullptr)
{
robotHealthTopic->heartbeat(componentName + "_" + ip, args);
robotHealthPlugin->heartbeatOnChannel(ip);
}
else
{
ARMARX_WARNING << "No robot health topic available: IP: " << ip << ", Port: " << port;
ARMARX_WARNING << "No robot health topic available: IP: " << ip
<< ", Port: " << port;
}
IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
......@@ -274,20 +298,30 @@ void HokuyoLaserScanDevice::run()
StringVariantBaseMap durations;
durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
durations["measure_ms"] = new Variant((time_measure - time_start).toMilliSecondsDouble());
durations["update_ms"] = new Variant((time_update - time_measure).toMilliSecondsDouble());
durations["topic_sensor_ms"] = new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] = new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
debugObserver->setDebugChannel("LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
durations["measure_ms"] =
new Variant((time_measure - time_start).toMilliSecondsDouble());
durations["update_ms"] =
new Variant((time_update - time_measure).toMilliSecondsDouble());
durations["topic_sensor_ms"] =
new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] =
new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
debugObserver->setDebugChannel(
"LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
if (duration.toSecondsDouble() > 0.1)
{
ARMARX_WARNING << "Laserscanner reports are slow"
<< "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
<< "Measure: " << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
<< "Update: " << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
<< "TopicSensor: " << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
<< "TopicHeart: " << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble() << "ms\n";
<< "Measure: "
<< (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
<< "Update: "
<< (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
<< "TopicSensor: "
<< (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
<< "TopicHeart: "
<< (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()
<< "ms\n";
}
}
}
......
source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
View file @ 4ee3e4f3
......@@ -22,14 +22,17 @@
#pragma once
#include <vector>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <HokuyoLaserScannerDriver/urg_sensor.h>
#include <vector>
namespace armarx
{
......@@ -37,19 +40,30 @@ namespace armarx
* @class HokuyoLaserUnitPropertyDefinitions
* @brief
*/
class HokuyoLaserUnitPropertyDefinitions:
public armarx::ComponentPropertyDefinitions
class HokuyoLaserUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
HokuyoLaserUnitPropertyDefinitions(std::string prefix):
HokuyoLaserUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
defineOptionalProperty<std::string>(
"LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
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");
defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
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>(
"RobotHealthComponentName", "RobotHealth", "Name of the RobotHealth component");
defineOptionalProperty<std::string>(
"RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
defineOptionalProperty<std::string>("DebugObserverName",
"DebugObserver",
"Name of the topic the DebugObserver listens on");
}
};
......@@ -66,6 +80,7 @@ namespace armarx
bool reconnect();
void run();
RunningTask<HokuyoLaserScanDevice>::pointer_type task;
......@@ -73,7 +88,7 @@ namespace armarx
std::string componentName;
LaserScannerUnitListenerPrx scanTopic;
RobotHealthInterfacePrx robotHealthTopic;
armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
......@@ -93,10 +108,13 @@ namespace armarx
virtual public armarx::SensorActorUnit
{
public:
HokuyoLaserUnit();
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
std::string getDefaultName() const override
std::string
getDefaultName() const override
{
return "HokuyoLaserUnit";
}
......@@ -138,8 +156,8 @@ namespace armarx
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
LaserScannerInfoSeq connectedDevices;
RobotHealthInterfacePrx robotHealthTopic;
DebugObserverInterfacePrx debugObserver;
};
}
plugins::HeartbeatComponentPlugin* heartbeat = nullptr;
};
} // namespace armarx
source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
View file @ 4ee3e4f3
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>GuiHealthClientWidget</class>
<widget class="QWidget" name="GuiHealthClientWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>500</width>
<height>71</height>
</rect>
</property>
<property name="windowTitle">
<string>GuiHealthClientWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="1">
<widget class="QPushButton" name="pushButtonToggleOwnHeartbeats">
<property name="text">
<string>Toggle</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="labelState">
<property name="text">
<string>TextLabel</string>
</property>
</widget>
</item>
<item row="0" column="2">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0" colspan="3">
<widget class="QLabel" name="labelHealthState">
<property name="text">
<string>TextLabel</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
<class>GuiHealthClientWidget</class>
<widget class="QWidget" name="GuiHealthClientWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>500</width>
<height>71</height>
</rect>
</property>
<property name="windowTitle">
<string>GuiHealthClientWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0" colspan="2">
<widget class="QLabel" name="labelTags">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QPushButton" name="pushButtonUnrequireAll">
<property name="text">
<string>Reset required tags</string>
</property>
</widget>
</item>
<item row="1" column="0" colspan="3">
<widget class="QTableWidget" name="tableHealthState">
</widget>
</item>
</layout>
</widget>
<resources />
<connections />
</ui>
source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
View file @ 4ee3e4f3
......@@ -22,67 +22,96 @@
#include "GuiHealthClientWidgetController.h"
#include <QLabel>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <qcolor.h>
#include <qnamespace.h>
#include <qrgb.h>
#include <qtablewidget.h>
#include <string>
#include <QLabel>
#include <QPushButton>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
namespace armarx
{
std::string serializeList(const std::vector<std::string>& l)
{
std::string s;
for(std::size_t i = 0; i < l.size(); i++)
{
const auto& tag = l.at(i);
s += tag;
if(i != l.size() - 1)
{
s += ", ";
}
}
return s;
}
GuiHealthClientWidgetController::GuiHealthClientWidgetController()
{
widget.setupUi(getWidget());
//ARMARX_IMPORTANT << "ctor start";
healthTimer = new QTimer(this);
healthTimer->setInterval(10);
connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb()));
connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats()));
connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
connect(this,
SIGNAL(invokeConnectComponentQt()),
this,
SLOT(onConnectComponentQt()),
Qt::QueuedConnection);
connect(this,
SIGNAL(invokeDisconnectComponentQt()),
this,
SLOT(onDisconnectComponentQt()),
Qt::QueuedConnection);
connect(widget.pushButtonUnrequireAll,
SIGNAL(clicked()),
this,
SLOT(unrequireAll()));
updateSummaryTimer = new QTimer(this);
updateSummaryTimer->setInterval(100);
connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
widget.labelState->setText("idle.");
widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
//ARMARX_IMPORTANT << "ctor end";
}
GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
{
}
void GuiHealthClientWidgetController::loadSettings(QSettings* settings)
void
GuiHealthClientWidgetController::loadSettings(QSettings* settings)
{
}
void GuiHealthClientWidgetController::saveSettings(QSettings* settings)
void
GuiHealthClientWidgetController::saveSettings(QSettings* settings)
{
}
void GuiHealthClientWidgetController::onInitComponent()
void
GuiHealthClientWidgetController::onInitComponent()
{
//ARMARX_IMPORTANT << "onInitComponent";
usingProxy("RobotHealth");
offeringTopic("RobotHealthTopic");
}
void GuiHealthClientWidgetController::healthTimerClb()
void
GuiHealthClientWidgetController::healthTimerClb()
{
RobotHealthHeartbeatArgs rhha;
rhha.maximumCycleTimeWarningMS = 250;
rhha.maximumCycleTimeErrorMS = 500;
robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
}
void GuiHealthClientWidgetController::updateSummaryTimerClb()
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{
//ARMARX_IMPORTANT << "updateSummaryTimerClb";
if (robotHealthComponentPrx)
......@@ -90,59 +119,152 @@ namespace armarx
//ARMARX_IMPORTANT << "has robotHealthComponentPrx";
try
{
//ARMARX_IMPORTANT << "before set text";
widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
//ARMARX_IMPORTANT << "after set text";
auto summary = robotHealthComponentPrx->getSummary();
const std::size_t nCols = 10;
auto& tableWidget = widget.tableHealthState;
tableWidget->setRowCount(summary.entries.size());
tableWidget->setColumnCount(nCols);
const auto summaryVals = simox::alg::get_values(summary.entries);
tableWidget->setHorizontalHeaderLabels({
"identifier", "required", "keeps frequency", "tags", "time since\nlast arrival [ms]", "time to\nreference [ms]", "time sync \n+ Ice [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]", "hostname"
});
tableWidget->setColumnWidth(0, 250);
tableWidget->setColumnWidth(1, 80);
tableWidget->setColumnWidth(2, 120);
tableWidget->setColumnWidth(3, 180);
tableWidget->setColumnWidth(4, 120);
tableWidget->setColumnWidth(5, 120);
tableWidget->setColumnWidth(6, 120);
tableWidget->setColumnWidth(7, 120);
for(std::size_t i = 0; i < summary.entries.size(); i++)
{
const auto& entry = summaryVals.at(i);
std::string stateRepr;
QColor color;
switch(entry.state)
{
case HealthOK:
stateRepr = "yes";
color.setRgb(0, 255, 0); // green
break;
case HealthWarning:
stateRepr = "yes";
color.setRgb(255, 165, 0); // orange
break;
case HealthError:
stateRepr = "no";
color.setRgb(255, 0, 0); // red
break;
}
const std::string hostname = entry.lastReferenceTimestamp.hostname;
const std::string timeSinceLastArrivalRepr = std::to_string(entry.timeSinceLastArrival.microSeconds / 1000);
const std::string timeToLastReferenceRepr = std::to_string(entry.timeSinceLastUpdateReference.microSeconds / 1000);
const std::string tagsRepr = serializeList(entry.tags);
const long syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000;
tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
requiredItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
tableWidget->setItem(i, 1, requiredItem);
auto* stateItem = new QTableWidgetItem(QString::fromStdString(stateRepr));
stateItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
tableWidget->setItem(i, 2, stateItem);
tableWidget->item(i, 2)->setBackgroundColor(color);
tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
tableWidget->setItem(i, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastArrivalRepr)));
tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(timeToLastReferenceRepr)));
tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(syncErrorMilliSeconds))));
if(syncErrorMilliSeconds > 20)
{
QColor timeSyncColor;
timeSyncColor.setRgb(255, 0, 0);
tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
}else {
QColor timeSyncColor;
timeSyncColor.setRgb(0, 255, 0);
tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
}
tableWidget->setItem(i, 7, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
tableWidget->setItem(i, 8, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
std::string tagsText = "Active tags: [";
{
std::sort(summary.activeTags.begin(), summary.activeTags.end());
tagsText += serializeList(summary.activeTags);
tagsText += "]";
}
widget.labelTags->setText(QString::fromStdString(tagsText));
}
catch (...)
{}
{
}
}
}
void GuiHealthClientWidgetController::toggleSendOwnHeartbeats()
void
GuiHealthClientWidgetController::unrequireAll()
{
if (ownHeartbeatsActive)
{
ownHeartbeatsActive = false;
healthTimer->stop();
robotHealthTopicPrx->unregister(getName());
widget.labelState->setText("idle.");
//widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
}
else
ARMARX_INFO << "UnrequireAll.";
if (robotHealthComponentPrx)
{
ownHeartbeatsActive = true;
healthTimer->start();
widget.labelState->setText("sending heartbeats...");
widget.pushButtonToggleOwnHeartbeats->setText("unregister self");
try
{
robotHealthComponentPrx->resetRequiredTags();
}
catch(...){}
}
}
void GuiHealthClientWidgetController::onConnectComponent()
void
GuiHealthClientWidgetController::onConnectComponent()
{
//ARMARX_IMPORTANT << "onConnectComponent";
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
invokeConnectComponentQt();
}
void GuiHealthClientWidgetController::onConnectComponentQt()
void
GuiHealthClientWidgetController::onConnectComponentQt()
{
//healthTimer->start();
//ARMARX_IMPORTANT << "updateSummaryTimer->start";
//ARMARX_IMPORTANT <<ame "updateSummaryTimer->start";
updateSummaryTimer->start();
}
void GuiHealthClientWidgetController::onDisconnectComponent()
void
GuiHealthClientWidgetController::onDisconnectComponent()
{
invokeDisconnectComponentQt();
}
void GuiHealthClientWidgetController::onDisconnectComponentQt()
void
GuiHealthClientWidgetController::onDisconnectComponentQt()
{
healthTimer->stop();
updateSummaryTimer->stop();
}
}
} // namespace armarx
source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
View file @ 4ee3e4f3
......@@ -110,7 +110,7 @@ namespace armarx
void onDisconnectComponentQt();
void healthTimerClb();
void updateSummaryTimerClb();
void toggleSendOwnHeartbeats();
void unrequireAll();
signals:
/* QT signal declarations */
......@@ -122,13 +122,9 @@ namespace armarx
* Widget Form
*/
Ui::GuiHealthClientWidget widget;
bool ownHeartbeatsActive = false;
RobotHealthInterfacePrx robotHealthTopicPrx;
RobotHealthComponentInterfacePrx robotHealthComponentPrx;
QTimer* healthTimer;
QTimer* updateSummaryTimer;
};
}
source/RobotAPI/interface/aron/Aron.ice
View file @ 4ee3e4f3
......@@ -4,23 +4,27 @@
* Changelog:
* - Update 1.1.0: Allow templates in aron and anyobjects
* - Update 1.1.1: Switch to armarx type. Also remove position, orientation and pose types as they are nothing else that matrices or quaternions
* - Update 2.0.0: Add support for aron defaults. Also add marshalling checks in aron variants.
**/
#include <ArmarXCore/interface/core/BasicTypes.ice>
#define ARON_MAJOR "1"
#define ARON_MINOR "1"
#define ARON_PATCH "2"
#define ARON_VERSION "1.1.2"
// Those macros stay defined
#define ARON_MAJOR "2" // ice interface changes
#define ARON_MINOR "0" // aron header changes
#define ARON_PATCH "0" // aron cpp changes
#define ARON_VERSION "2.0.0"
module armarx
{
module aron
{
// also make version number available to other languages without a preprocessor
const string MAJOR = ARON_MAJOR;
const string MINOR = ARON_MINOR;
const string PATCH = ARON_PATCH;
const string VERSION = ARON_VERSION;
/*************************
* Aron Types ************
************************/
......@@ -48,6 +52,13 @@ module armarx
FLOAT32,
FLOAT64
};
module default_value
{
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
};
module image
......@@ -57,6 +68,14 @@ module armarx
RGB24,
DEPTH32
};
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
};
module pointcloud
......@@ -71,6 +90,14 @@ module armarx
POINT_XYZRGBA,
POINT_XYZHSV
};
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
};
module matrix
......@@ -83,6 +110,14 @@ module armarx
FLOAT32,
FLOAT64
};
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
};
module quaternion
......@@ -92,46 +127,154 @@ module armarx
FLOAT32,
FLOAT64
};
module default_value
{
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
};
module aron_enum
{
module default_value
{
const string DEFAULT = "";
}
}
module dto
{
class GenericType {
class GenericType
{
string VERSION = ARON_VERSION;
Maybe maybe = Maybe::NONE;
}
};
sequence<GenericType> GenericTypeSeq;
dictionary<string, GenericType> GenericTypeDict;
/* ***** Container types ***** */
class List extends GenericType { GenericType acceptedType; }
class Tuple extends GenericType { GenericTypeSeq elementTypes; }
class Pair extends GenericType { GenericType acceptedType1; GenericType acceptedType2; }
class AronObject extends GenericType { AronObject parent; Ice::StringSeq templates; string objectName; Ice::StringSeq templateInstantiations; GenericTypeDict elementTypes; }
class Dict extends GenericType { GenericType acceptedType; }
class List extends GenericType
{
GenericType acceptedType;
};
class Tuple extends GenericType
{
GenericTypeSeq elementTypes;
};
class Pair extends GenericType
{
GenericType acceptedType1;
GenericType acceptedType2;
};
class AronObject extends GenericType
{
Ice::StringSeq templates;
string objectName;
AronObject parent;
Ice::StringSeq templateInstantiations;
GenericTypeDict elementTypes;
};
class Dict extends GenericType
{
GenericType acceptedType;
}
/* ***** Complex Types (serialize to ndarray) ***** */
class NDArray extends GenericType { int ndimensions; ndarray::ElementType elementType; }
class Matrix extends GenericType { int rows; int cols; matrix::ElementType elementType; }
class Quaternion extends GenericType { quaternion::ElementType elementType; }
class Image extends GenericType { image::PixelType pixelType; }
class PointCloud extends GenericType { pointcloud::VoxelType voxelType; }
class NDArray extends GenericType
{
int ndimensions;
ndarray::ElementType elementType;
string defaultValue = type::ndarray::default_value::DEFAULT;
// ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
};
class Matrix extends GenericType
{
int rows;
int cols;
matrix::ElementType elementType;
string defaultValue = type::matrix::default_value::DEFAULT;
// IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
};
class Quaternion extends GenericType
{
quaternion::ElementType elementType;
string defaultValue = type::quaternion::default_value::DEFAULT;
// ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value sequence> | ""
};
class Image extends GenericType
{
image::PixelType pixelType;
string defaultValue = type::image::default_value::DEFAULT;
// IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
};
class PointCloud extends GenericType
{
pointcloud::VoxelType voxelType;
string defaultValue = type::pointcloud::default_value::DEFAULT;
// IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
};
/* ***** Enum types ***** */
class IntEnum extends GenericType { string enumName; StringIntDict acceptedValues; }
class IntEnum extends GenericType
{
string enumName;
StringIntDict acceptedValues;
string defaultValue = aron_enum::default_value::DEFAULT;
// in acceptedValues.keys | ""
};
//class FloatEnum extends GenericType { string enumName; StringFloatDict acceptedValues; }
//class StringEnum extends GenericType { string enumName; StringStringDict acceptedValues; }
/* ***** Any Types ***** */
class AnyObject extends GenericType { };
class AnyObject extends GenericType
{
};
/* ***** Primitive Types ***** */
class AronInt extends GenericType { };
class AronLong extends GenericType { };
class AronDouble extends GenericType { };
class AronFloat extends GenericType { };
class AronString extends GenericType { };
class AronBool extends GenericType { };
class AronInt extends GenericType
{
int defaultValue = 0; // <value>
};
class AronLong extends GenericType
{
long defaultValue = 0; // <value>
};
class AronDouble extends GenericType
{
double defaultValue = 0; // <value>
};
class AronFloat extends GenericType
{
float defaultValue = 0; // <value>
};
class AronString extends GenericType
{
string defaultValue = ""; // <value>
};
class AronBool extends GenericType
{
bool defaultValue = false; // <value>
};
};
};
......@@ -143,30 +286,67 @@ module armarx
{
module dto
{
class GenericData {
class GenericData
{
string VERSION = ARON_VERSION;
};
sequence<GenericData> GenericDataSeq;
dictionary<string, GenericData> GenericDataDict;
/* ***** Container Data ***** */
class List extends GenericData { GenericDataSeq elements; };
class Dict extends GenericData { GenericDataDict elements; };
class List extends GenericData
{
GenericDataSeq elements;
};
class Dict extends GenericData
{
GenericDataDict elements;
};
/* ***** Complex Data ***** */
// The NDArray contains more or less the same information as an AronType, but there is no other way to do it
// Especially, note the difference between the type's typeName (e.g. "GRAY_SCALE_IMAGE" => language dependent) and the data's type ("0")
// Further, note the difference between the type's dimensions (e.g. 128x128) and the data's dimensions (128x128x3 for RGB)
class NDArray extends GenericData { Ice::IntSeq shape; string type; Ice::ByteSeq data; }
class NDArray extends GenericData
{
Ice::IntSeq shape;
string type;
Ice::ByteSeq data;
};
/* ***** Primitive Data ***** */
class AronInt extends GenericData { int value; };
class AronLong extends GenericData { long value; };
class AronDouble extends GenericData { double value; };
class AronFloat extends GenericData { float value; };
class AronString extends GenericData { string value; };
class AronBool extends GenericData { bool value; };
// They must be prefixed with 'Aron' as e.g. Int cannot be used as class name
class AronInt extends GenericData
{
int value;
};
class AronLong extends GenericData
{
long value;
};
class AronDouble extends GenericData
{
double value;
};
class AronFloat extends GenericData
{
float value;
};
class AronString extends GenericData
{
string value;
};
class AronBool extends GenericData
{
bool value;
};
// useful for memory ice_conversions
sequence<Dict> AronDictSeq;
......
source/RobotAPI/interface/components/RobotHealthInterface.ice
View file @ 4ee3e4f3
......@@ -22,8 +22,12 @@
#pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/time.ice>
module armarx
{
enum RobotHealthState
{
HealthOK, HealthWarning, HealthError
......@@ -31,24 +35,74 @@ module armarx
struct RobotHealthHeartbeatArgs
{
int maximumCycleTimeWarningMS = 100;
int maximumCycleTimeErrorMS = 200;
string message;
string identifier;
// bool requiredByDefault;
string description;
Ice::StringSeq tags;
armarx::core::time::dto::Duration maximumCycleTimeWarning;
armarx::core::time::dto::Duration maximumCycleTimeError;
};
interface RobotHealthInterface
{
void heartbeat(string componentName, RobotHealthHeartbeatArgs args);
void unregister(string componentName);
void signUp(RobotHealthHeartbeatArgs args);
void unregister(string identifier);
void addRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
void removeRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
void resetRequiredTags();
void heartbeat(string identifier, armarx::core::time::dto::DateTime referenceTime);
string getTopicName();
};
// Used by the RobotHealth to send an overall status update ehich e.g. the emergency stop listens to.
interface AggregatedRobotHealthInterface
{
void aggregatedHeartbeat(RobotHealthState overallHealthState);
};
struct RobotHealthInfoEntry
{
string identifier;
RobotHealthState state;
// string message;
armarx::core::time::dto::Duration minDelta;
armarx::core::time::dto::Duration maxDelta;
armarx::core::time::dto::DateTime lastReferenceTimestamp;
//< Time delta to now() when arrived at heart beat component
armarx::core::time::dto::Duration timeSinceLastArrival;
//< Time delta to reference timestamp sent by component
armarx::core::time::dto::Duration timeSinceLastUpdateReference;
armarx::core::time::dto::Duration timeSyncDelayAndIce;
bool required; //
bool enabled;
armarx::core::time::dto::Duration maximumCycleTimeWarning;
armarx::core::time::dto::Duration maximumCycleTimeError;
Ice::StringSeq tags;
};
dictionary<string, RobotHealthInfoEntry> RobotHealthEntries;
struct RobotHealthInfo
{
Ice::StringSeq activeTags;
RobotHealthEntries entries;
};
interface RobotHealthComponentInterface extends RobotHealthInterface
{
string getSummary();
RobotHealthInfo getSummary();
};
};