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 ...@@ -80,8 +80,8 @@ namespace viz
InteractionFeedbackRange interactions() const InteractionFeedbackRange interactions() const
{ {
InteractionFeedback* begin = (InteractionFeedback*) data_.interactions.data(); const InteractionFeedback* begin = reinterpret_cast<const InteractionFeedback*>(data_.interactions.data());
InteractionFeedback* end = begin + data_.interactions.size(); const InteractionFeedback* end = begin + data_.interactions.size();
return InteractionFeedbackRange{begin, end}; return InteractionFeedbackRange{begin, end};
} }
......
source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
View file @ 4ee3e4f3
...@@ -82,7 +82,7 @@ namespace armarx::viz ...@@ -82,7 +82,7 @@ namespace armarx::viz
vertices.reserve(p3.size_of_vertices()); vertices.reserve(p3.size_of_vertices());
faces.reserve(p3.size_of_facets()); faces.reserve(p3.size_of_facets());
auto vbeg = p3.vertices_begin(); 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(); const auto& p = v.point();
armarx::Vector3f visp; armarx::Vector3f visp;
...@@ -91,7 +91,7 @@ namespace armarx::viz ...@@ -91,7 +91,7 @@ namespace armarx::viz
visp.e2 = p.z(); visp.e2 = p.z();
vertices.emplace_back(visp); 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(); auto circ = fidx.facet_begin();
ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ)) 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 ...@@ -71,8 +71,8 @@ namespace armarx::viz
PointCloud& points(std::vector<ColoredPoint> const& ps) PointCloud& points(std::vector<ColoredPoint> const& ps)
{ {
std::size_t memorySize = ps.size() * sizeof(ps[0]); std::size_t memorySize = ps.size() * sizeof(ps[0]);
Ice::Byte* begin = (Ice::Byte*)ps.data(); const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(ps.data());
Ice::Byte* end = begin + memorySize; const Ice::Byte* const end = begin + memorySize;
data_->points.assign(begin, end); data_->points.assign(begin, end);
return *this; return *this;
} }
...@@ -92,8 +92,8 @@ namespace armarx::viz ...@@ -92,8 +92,8 @@ namespace armarx::viz
PointCloud& addPointUnchecked(ColoredPoint const& p) PointCloud& addPointUnchecked(ColoredPoint const& p)
{ {
Ice::Byte* begin = (Ice::Byte*)&p; const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(&p);
Ice::Byte* end = begin + sizeof(p); const Ice::Byte* const end = begin + sizeof(p);
data_->points.insert(data_->points.end(), begin, end); data_->points.insert(data_->points.end(), begin, end);
return *this; return *this;
} }
...@@ -391,5 +391,3 @@ namespace armarx::viz ...@@ -391,5 +391,3 @@ namespace armarx::viz
}; };
} }
source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
View file @ 4ee3e4f3
armarx_component_set_name("GamepadControlUnit") 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(SOURCES GamepadControlUnit.cpp)
set(HEADERS GamepadControlUnit.h) set(HEADERS GamepadControlUnit.h)
......
source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
View file @ 4ee3e4f3
...@@ -22,15 +22,15 @@ ...@@ -22,15 +22,15 @@
#include "GamepadControlUnit.h" #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> #include <ArmarXCore/observers/variant/TimestampVariant.h>
namespace armarx namespace armarx
{ {
void GamepadControlUnit::onInitComponent() void
GamepadControlUnit::onInitComponent()
{ {
ARMARX_INFO << "oninit GamepadControlUnit"; ARMARX_INFO << "oninit GamepadControlUnit";
usingProxy(getProperty<std::string>("PlatformUnitName").getValue()); usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
...@@ -41,67 +41,80 @@ namespace armarx ...@@ -41,67 +41,80 @@ namespace armarx
scaleY = getProperty<float>("ScaleY").getValue(); scaleY = getProperty<float>("ScaleY").getValue();
scaleRotation = getProperty<float>("ScaleAngle").getValue(); scaleRotation = getProperty<float>("ScaleAngle").getValue();
ARMARX_INFO << "oninit GamepadControlUnit end"; ARMARX_INFO << "oninit GamepadControlUnit end";
} }
void
void GamepadControlUnit::onConnectComponent() GamepadControlUnit::onConnectComponent()
{ {
ARMARX_INFO << "onConnect GamepadControlUnit"; ARMARX_INFO << "onConnect GamepadControlUnit";
platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue()); platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(
getProperty<std::string>("PlatformUnitName").getValue());
emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster"); 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_INFO << "exit GamepadControlUnit";
} }
armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions() armarx::PropertyDefinitionsPtr
GamepadControlUnit::createPropertyDefinitions()
{ {
auto defs = armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions( auto defs = armarx::PropertyDefinitionsPtr(
getConfigIdentifier())); new GamepadControlUnitPropertyDefinitions(getConfigIdentifier()));
defs->topic(robotHealthTopic); defs->optional(enableHeartBeat,
defs->optional(enableHeartBeat, "EnableHeartBeat", "Flag to enable send a heart beat to the robot healh topic"); "EnableHeartBeat",
return defs; "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 { // struct GamepadData {
// float leftStickX; // float leftStickX;
// float leftStickY; // float leftStickY;
// float rightStickX; // float rightStickX;
// float rightStickY; // float rightStickY;
// float dPadX; // float dPadX;
// float dPadY; // float dPadY;
// float leftTrigger; // float leftTrigger;
// float rightTrigger; // float rightTrigger;
// //
// bool leftButton; // bool leftButton;
// bool rightButton; // bool rightButton;
// bool backButton; // bool backButton;
// bool startButton; // bool startButton;
// bool xButton; // bool xButton;
// bool yButton; // bool yButton;
// bool aButton; // bool aButton;
// bool bButton; // bool bButton;
// bool theMiddleButton; // bool theMiddleButton;
// bool leftStickButton; // bool leftStickButton;
// bool rightStickButton; // bool rightStickButton;
// //
// }; // };
if (data.leftTrigger > 0) if (data.leftTrigger > 0)
...@@ -116,7 +129,9 @@ namespace armarx ...@@ -116,7 +129,9 @@ namespace armarx
//scales are for the robot axis //scales are for the robot axis
if (data.rightTrigger > 0) 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 else
{ {
...@@ -124,64 +139,68 @@ namespace armarx ...@@ -124,64 +139,68 @@ namespace armarx
} }
if (data.leftButton) if (data.leftButton)
{ {
if(leftHandTime <= 0.0) if (leftHandTime <= 0.0)
{ {
leftHandTime = IceUtil::Time::now().toMicroSeconds(); leftHandTime = IceUtil::Time::now().toMicroSeconds();
} }
else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000* 1000) else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000 * 1000)
{ {
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit"); HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
if (handUnit) if (handUnit)
{ {
std::string shapeName = (leftHandOpen) ? "Close" : "Open"; std::string shapeName = (leftHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName); handUnit->setShape(shapeName);
leftHandOpen = !leftHandOpen; leftHandOpen = !leftHandOpen;
leftHandTime = 0.0; leftHandTime = 0.0;
} }
} }
} }
else else
{ {
leftHandTime = 0.0; leftHandTime = 0.0;
} }
if(data.rightButton) if (data.rightButton)
{ {
if(rightHandTime <= 0.0) if (rightHandTime <= 0.0)
{ {
rightHandTime = IceUtil::Time::now().toMicroSeconds(); rightHandTime = IceUtil::Time::now().toMicroSeconds();
} }
else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000* 1000) else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000 * 1000)
{ {
HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit"); HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
if (handUnit) if (handUnit)
{ {
std::string shapeName = (rightHandOpen) ? "Close" : "Open"; std::string shapeName = (rightHandOpen) ? "Close" : "Open";
handUnit->setShape(shapeName); handUnit->setShape(shapeName);
rightHandOpen = !rightHandOpen; rightHandOpen = !rightHandOpen;
rightHandTime = 0.0; rightHandTime = 0.0;
} }
} }
} }
else else
{ {
rightHandTime = 0.0; rightHandTime = 0.0;
} }
if(enableHeartBeat) if (enableHeartBeat)
{ {
RobotHealthHeartbeatArgs args; auto now = armarx::core::time::dto::DateTime();
args.maximumCycleTimeErrorMS = 1000; armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopic->heartbeat(getDefaultName(), args); heartbeatPlugin->heartbeat();
}
}
//ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation; //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 @@ ...@@ -30,6 +30,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h> #include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h> #include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h> #include <RobotAPI/interface/units/PlatformUnitInterface.h>
...@@ -72,6 +73,8 @@ namespace armarx ...@@ -72,6 +73,8 @@ namespace armarx
virtual public GamepadUnitListener virtual public GamepadUnitListener
{ {
public: public:
GamepadControlUnit();
/** /**
* @see armarx::ManagedIceObject::getDefaultName() * @see armarx::ManagedIceObject::getDefaultName()
*/ */
...@@ -109,10 +112,11 @@ namespace armarx ...@@ -109,10 +112,11 @@ namespace armarx
private: private:
PlatformUnitInterfacePrx platformUnitPrx; PlatformUnitInterfacePrx platformUnitPrx;
plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
bool enableHeartBeat = false;
RobotHealthInterfacePrx robotHealthTopic; bool enableHeartBeat = false;
float scaleX; float scaleX;
float scaleY; float scaleY;
float scaleRotation; float scaleRotation;
......
source/RobotAPI/components/RobotHealth/RobotHealth.h
View file @ 4ee3e4f3
...@@ -22,54 +22,28 @@ ...@@ -22,54 +22,28 @@
#pragma once #pragma once
#include <RobotAPI/interface/components/RobotHealthInterface.h> #include <array>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h> #include <atomic>
#include <RobotAPI/interface/speech/SpeechInterface.h> #include <deque>
#include <mutex>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.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/interface/components/EmergencyStopInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h> #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <atomic> #include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <mutex>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
namespace armarx 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 * @defgroup Component-RobotHealth RobotHealth
* @ingroup RobotAPI-Components * @ingroup RobotAPI-Components
...@@ -90,34 +64,12 @@ namespace armarx ...@@ -90,34 +64,12 @@ namespace armarx
/** /**
* @see armarx::ManagedIceObject::getDefaultName() * @see armarx::ManagedIceObject::getDefaultName()
*/ */
std::string getDefaultName() const override std::string
getDefaultName() const override
{ {
return "RobotHealth"; 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: protected:
/** /**
* @see armarx::ManagedIceObject::onInitComponent() * @see armarx::ManagedIceObject::onInitComponent()
...@@ -144,32 +96,100 @@ namespace armarx ...@@ -144,32 +96,100 @@ namespace armarx
*/ */
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void monitorHealthTaskClb(); private:
Entry& findOrCreateEntry(const std::string& name); 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; struct TimeInfo
std::deque<Entry> entries; {
std::atomic_size_t validEntries {0}; //< Timestamp sent by component
PeriodicTask<RobotHealth>::pointer_type monitorHealthTask; armarx::core::time::DateTime referenceTime;
int defaultMaximumCycleTimeWarn;
int defaultMaximumCycleTimeErr; //< Timestamp on this PC, set by this component
EmergencyStopListenerPrx emergencyStopTopicPrx; armarx::core::time::DateTime arrivalTime;
//RobotUnitInterfacePrx robotUnitPrx; };
//bool robotUnitRequired;
RemoteGuiInterfacePrx remoteGuiPrx; std::deque<TimeInfo> history;
AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
TextListenerInterfacePrx textToSpeechTopic; armarx::core::time::Duration maximumCycleTimeWarn;
bool reportErrorsWithSpeech; armarx::core::time::Duration maximumCycleTimeErr;
int speechMinimumReportInterval; };
IceUtil::Time lastSpeechOutput;
void monitorHealthUpdateTaskClb();
UpdateEntry* findUpdateEntry(const std::string& name);
std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name);
void reportDebugObserver();
// RobotHealthInterface interface // RobotHealthInterface interface
public: public:
void heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&) override; void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
void unregister(const std::string& componentName, const Ice::Current&) override; void unregister(const std::string& identifier, const Ice::Current&) override;
std::string getSummary(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 @@ ...@@ -21,26 +21,35 @@
*/ */
#include "RobotHealthDummy.h" #include "RobotHealthDummy.h"
#include <time.h> #include <time.h>
#include <thread> #include <thread>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
namespace armarx namespace armarx
{ {
void RobotHealthDummy::onInitComponent() void
RobotHealthDummy::onInitComponent()
{ {
dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask); dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
getProperty(sleepmode, "SleepMode"); getProperty(sleepmode, "SleepMode");
} }
void
void RobotHealthDummy::onConnectComponent() RobotHealthDummy::onConnectComponent()
{ {
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue()); robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(
getProperty<std::string>("RobotHealthTopicName").getValue());
dummyTask->start(); dummyTask->start();
} }
#define NANOSECS_PER_SEC 1000000000 #define NANOSECS_PER_SEC 1000000000
int RobotHealthDummy::NanoSleep(long nanosec)
int
RobotHealthDummy::NanoSleep(long nanosec)
{ {
struct timespec ts; struct timespec ts;
ts.tv_sec = nanosec / NANOSECS_PER_SEC; ts.tv_sec = nanosec / NANOSECS_PER_SEC;
...@@ -48,48 +57,55 @@ namespace armarx ...@@ -48,48 +57,55 @@ namespace armarx
return nanosleep(&ts, nullptr); // jitter up to +100ms! return nanosleep(&ts, nullptr); // jitter up to +100ms!
} }
void RobotHealthDummy::sleepwait(long microseconds) void
RobotHealthDummy::sleepwait(long microseconds)
{ {
long start = TimeUtil::GetTime().toMicroSeconds(); long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds; auto end = start + microseconds;
do do
{ {
NanoSleep(1000); 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(); long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds; auto end = start + microseconds;
do do
{ {
std::this_thread::yield(); 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(); long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds; auto end = start + microseconds;
do 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"; ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped()) while (!dummyTask->isStopped())
{ {
long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds(); long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
//ARMARX_INFO << "send heartbeat"; //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(); long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
if (sleepmode == "nanosleep") if (sleepmode == "nanosleep")
{ {
...@@ -126,23 +142,24 @@ namespace armarx ...@@ -126,23 +142,24 @@ namespace armarx
} }
} }
void
void RobotHealthDummy::onDisconnectComponent() RobotHealthDummy::onDisconnectComponent()
{ {
//ARMARX_IMPORTANT << "onDisconnectComponent"; //ARMARX_IMPORTANT << "onDisconnectComponent";
dummyTask->stop(); dummyTask->stop();
} }
void
void RobotHealthDummy::onExitComponent() RobotHealthDummy::onExitComponent()
{ {
//ARMARX_IMPORTANT << "onExitComponent"; //ARMARX_IMPORTANT << "onExitComponent";
dummyTask->stop(); dummyTask->stop();
} }
armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions() armarx::PropertyDefinitionsPtr
RobotHealthDummy::createPropertyDefinitions()
{ {
return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions( return armarx::PropertyDefinitionsPtr(
getConfigIdentifier())); new RobotHealthDummyPropertyDefinitions(getConfigIdentifier()));
} }
} } // namespace armarx
source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
View file @ 4ee3e4f3
...@@ -24,10 +24,10 @@ ...@@ -24,10 +24,10 @@
#include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h> #include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h> #include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx namespace armarx
{ {
...@@ -35,15 +35,18 @@ namespace armarx ...@@ -35,15 +35,18 @@ namespace armarx
* @class RobotHealthDummyPropertyDefinitions * @class RobotHealthDummyPropertyDefinitions
* @brief * @brief
*/ */
class RobotHealthDummyPropertyDefinitions: class RobotHealthDummyPropertyDefinitions : public armarx::ComponentPropertyDefinitions
public armarx::ComponentPropertyDefinitions
{ {
public: public:
RobotHealthDummyPropertyDefinitions(std::string prefix): RobotHealthDummyPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix) armarx::ComponentPropertyDefinitions(prefix)
{ {
defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic"); defineOptionalProperty<std::string>(
defineOptionalProperty<std::string>("SleepMode", "nanosleep", "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait"); "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"); //defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description"); //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
...@@ -61,14 +64,14 @@ namespace armarx ...@@ -61,14 +64,14 @@ namespace armarx
* *
* Detailed description of class RobotHealthDummy. * Detailed description of class RobotHealthDummy.
*/ */
class RobotHealthDummy : class RobotHealthDummy : virtual public armarx::Component
virtual public armarx::Component
{ {
public: public:
/** /**
* @see armarx::ManagedIceObject::getDefaultName() * @see armarx::ManagedIceObject::getDefaultName()
*/ */
std::string getDefaultName() const override std::string
getDefaultName() const override
{ {
return "RobotHealthDummy"; return "RobotHealthDummy";
} }
...@@ -77,6 +80,7 @@ namespace armarx ...@@ -77,6 +80,7 @@ namespace armarx
void yieldwait(long microseconds); void yieldwait(long microseconds);
void busydwait(long microseconds); void busydwait(long microseconds);
void sleepwait(long microseconds); void sleepwait(long microseconds);
protected: protected:
/** /**
* @see armarx::ManagedIceObject::onInitComponent() * @see armarx::ManagedIceObject::onInitComponent()
...@@ -109,6 +113,5 @@ namespace armarx ...@@ -109,6 +113,5 @@ namespace armarx
RunningTask<RobotHealthDummy>::pointer_type dummyTask; RunningTask<RobotHealthDummy>::pointer_type dummyTask;
std::string sleepmode; std::string sleepmode;
}; };
} } // namespace armarx
source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
View file @ 4ee3e4f3
...@@ -10,7 +10,8 @@ set(COMPONENT_LIBS ...@@ -10,7 +10,8 @@ set(COMPONENT_LIBS
RobotAPICore RobotAPIInterfaces RobotAPICore RobotAPIInterfaces
armem armem
aronopencvconverter aronjsonconverter aronopencvconverter aronjsonconverter
# ExampleMemory_aron
ExampleMemory # actually, the aron version is sufficient
${OpenCV_LIBRARIES} ${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. ...@@ -20,13 +20,13 @@ An example data containing different native ARON types.
<GenerateTypes> <GenerateTypes>
<Object name='armarx::armem::example::InnerClass'> <Object name='armarx::armem::example::InnerClass'>
<ObjectChild key='element_int'> <ObjectChild key='element_int'>
<Int /> <int32 default="42" />
</ObjectChild> </ObjectChild>
<ObjectChild key='element_float'> <ObjectChild key='element_float'>
<Float /> <float32 default="666.66" />
</ObjectChild> </ObjectChild>
<ObjectChild key='element_string'> <ObjectChild key='element_string'>
<String /> <String default="foobar" />
</ObjectChild> </ObjectChild>
</Object> </Object>
......
source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
View file @ 4ee3e4f3
...@@ -78,7 +78,7 @@ namespace armarx ...@@ -78,7 +78,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(isInitialized); ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries) 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! " << " unlogged entries, but only the last " << numEntries << " are saved! "
"There seems to be something wrong (e.g. the rt logging threw an exception, " "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). " "the system load is too high or the logging takes to long). "
...@@ -89,7 +89,7 @@ namespace armarx ...@@ -89,7 +89,7 @@ namespace armarx
auto numNewEntries = writePosition - onePastLoggingReadPosition; auto numNewEntries = writePosition - onePastLoggingReadPosition;
if (numNewEntries >= numEntries) 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; return;
} }
//consume all //consume all
...@@ -109,7 +109,7 @@ namespace armarx ...@@ -109,7 +109,7 @@ namespace armarx
auto newNumNewEntries = writePosition - onePastLoggingReadPosition; auto newNumNewEntries = writePosition - onePastLoggingReadPosition;
if (newNumNewEntries * 2 > numEntries) if (newNumNewEntries * 2 > numEntries)
{ {
ARMARX_WARNING << deactivateSpam(0.5) ARMARX_VERBOSE << deactivateSpam(5)
<< "RT-Logging is slow! " << "RT-Logging is slow! "
<< "The RT-Thread writes faster new data than the RT-Logging thread consumes it! " << "The RT-Thread writes faster new data than the RT-Logging thread consumes it! "
<< " old/new/max number of entries: " << " old/new/max number of entries: "
...@@ -118,7 +118,7 @@ namespace armarx ...@@ -118,7 +118,7 @@ namespace armarx
numNewEntries = newNumNewEntries; numNewEntries = newNumNewEntries;
if (numNewEntries >= numEntries) 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; return;
} }
} }
...@@ -132,7 +132,7 @@ namespace armarx ...@@ -132,7 +132,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(isInitialized); ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries) 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! " << " unlogged entries, but only the last " << numEntries << " are saved! "
"There seems to be something wrong (e.g. the rt logging threw an exception, " "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). " "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 @@ ...@@ -22,17 +22,25 @@
#include "HokuyoLaserUnit.h" #include "HokuyoLaserUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <SimoxUtility/algorithm/string/string_tools.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; using namespace armarx;
HokuyoLaserUnit::HokuyoLaserUnit()
{
addPlugin(heartbeat);
}
void HokuyoLaserUnit::onInitComponent() void
HokuyoLaserUnit::onInitComponent()
{ {
offeringTopicFromProperty("RobotHealthTopicName"); offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName"); offeringTopicFromProperty("DebugObserverName");
...@@ -72,19 +80,21 @@ void HokuyoLaserUnit::onInitComponent() ...@@ -72,19 +80,21 @@ void HokuyoLaserUnit::onInitComponent()
} }
catch (std::exception const& ex) catch (std::exception const& ex)
{ {
ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
<< " (port is " << deviceInfo[1] << ") : " << ex.what(); << deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
continue; continue;
} }
} }
} }
void
void HokuyoLaserUnit::onConnectComponent() HokuyoLaserUnit::onConnectComponent()
{ {
robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue()); ARMARX_TRACE;
topic = getTopic<LaserScannerUnitListenerPrx>(topicName); topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue()); debugObserver = getTopic<DebugObserverInterfacePrx>(
getProperty<std::string>("DebugObserverName").getValue());
connectedDevices.clear(); connectedDevices.clear();
for (HokuyoLaserScanDevice& device : devices) for (HokuyoLaserScanDevice& device : devices)
...@@ -95,12 +105,21 @@ void HokuyoLaserUnit::onConnectComponent() ...@@ -95,12 +105,21 @@ void HokuyoLaserUnit::onConnectComponent()
device.task = nullptr; device.task = nullptr;
} }
// connecting devices for first time
if (!device.reconnect()) 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; 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; LaserScannerInfo info;
info.device = device.ip; info.device = device.ip;
info.frame = device.frame; info.frame = device.frame;
...@@ -115,23 +134,22 @@ void HokuyoLaserUnit::onConnectComponent() ...@@ -115,23 +134,22 @@ void HokuyoLaserUnit::onConnectComponent()
device.lengthData.resize(lengthDataSize); device.lengthData.resize(lengthDataSize);
device.scanTopic = topic; device.scanTopic = topic;
device.robotHealthTopic = robotHealthTopic; device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver; device.debugObserver = debugObserver;
connectedDevices.push_back(info); connectedDevices.push_back(info);
ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
<< info.minAngle << ", " << info.maxAngle << ", " << info.stepSize; << ", " << info.maxAngle << ", " << info.stepSize;
device.task = new RunningTask<HokuyoLaserScanDevice>(&device, &HokuyoLaserScanDevice::run, device.task = new RunningTask<HokuyoLaserScanDevice>(
"HokuyoLaserScanUpdate_" + device.ip); &device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
device.task->start(); device.task->start();
} }
} }
void
void HokuyoLaserUnit::onDisconnectComponent() HokuyoLaserUnit::onDisconnectComponent()
{ {
for (HokuyoLaserScanDevice& device : devices) for (HokuyoLaserScanDevice& device : devices)
{ {
...@@ -148,29 +166,32 @@ void HokuyoLaserUnit::onDisconnectComponent() ...@@ -148,29 +166,32 @@ void HokuyoLaserUnit::onDisconnectComponent()
} }
} }
void
void HokuyoLaserUnit::onExitComponent() HokuyoLaserUnit::onExitComponent()
{ {
} }
armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions() armarx::PropertyDefinitionsPtr
HokuyoLaserUnit::createPropertyDefinitions()
{ {
return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions( return armarx::PropertyDefinitionsPtr(
getConfigIdentifier())); new HokuyoLaserUnitPropertyDefinitions(getConfigIdentifier()));
} }
std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{ {
return topicName; return topicName;
} }
LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const LaserScannerInfoSeq
HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{ {
return connectedDevices; return connectedDevices;
} }
bool HokuyoLaserScanDevice::reconnect() bool
HokuyoLaserScanDevice::reconnect()
{ {
if (connected) if (connected)
{ {
...@@ -183,8 +204,8 @@ bool HokuyoLaserScanDevice::reconnect() ...@@ -183,8 +204,8 @@ bool HokuyoLaserScanDevice::reconnect()
connected = (ret == 0); connected = (ret == 0);
if (!connected) if (!connected)
{ {
ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
<< ip << ", Port: " << port << ", Error: " << ret << ")"; << ", Port: " << port << ", Error: " << ret << ")";
return false; return false;
} }
ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0); ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
...@@ -196,21 +217,24 @@ bool HokuyoLaserScanDevice::reconnect() ...@@ -196,21 +217,24 @@ bool HokuyoLaserScanDevice::reconnect()
} }
else else
{ {
ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: " ARMARX_WARNING
<< ip << ", Port: " << port << ", Error: " << ret << ")"; << "Could not start measurement for laser scanner device using URG driver (IP: " << ip
<< ", Port: " << port << ", Error: " << ret << ")";
return false; return false;
} }
} }
void HokuyoLaserScanDevice::run() void
HokuyoLaserScanDevice::run()
{ {
ARMARX_TRACE;
while (!task->isStopped()) while (!task->isStopped())
{ {
IceUtil::Time time_start = TimeUtil::GetTime(); IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10) if (errorCounter > 10)
{ {
ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!"; ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
// assume dead // assume dead
reconnect(); reconnect();
} }
...@@ -219,8 +243,9 @@ void HokuyoLaserScanDevice::run() ...@@ -219,8 +243,9 @@ void HokuyoLaserScanDevice::run()
int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr); int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
if (lengthDataSize < 0) if (lengthDataSize < 0)
{ {
ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: " ARMARX_WARNING << deactivateSpam(1)
<< ip << ", Port: " << port << ", Error: " << lengthDataSize << ")"; << "Could not get measurement for laser scanner (IP: " << ip
<< ", Port: " << port << ", Error: " << lengthDataSize << ")";
errorCounter++; errorCounter++;
continue; continue;
} }
...@@ -236,7 +261,8 @@ void HokuyoLaserScanDevice::run() ...@@ -236,7 +261,8 @@ void HokuyoLaserScanDevice::run()
// TODO: Extract the min and max valid value for distance into parameters? // TODO: Extract the min and max valid value for distance into parameters?
if (distance >= 21 && distance <= 30000) 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 step.distance = (float)distance; // Data is already in mm
scan.push_back(step); scan.push_back(step);
} }
...@@ -256,16 +282,14 @@ void HokuyoLaserScanDevice::run() ...@@ -256,16 +282,14 @@ void HokuyoLaserScanDevice::run()
} }
IceUtil::Time time_topicSensor = TimeUtil::GetTime(); IceUtil::Time time_topicSensor = TimeUtil::GetTime();
RobotHealthHeartbeatArgs args; if (robotHealthPlugin != nullptr)
args.maximumCycleTimeWarningMS = 500;
args.maximumCycleTimeErrorMS = 800;
if (robotHealthTopic)
{ {
robotHealthTopic->heartbeat(componentName + "_" + ip, args); robotHealthPlugin->heartbeatOnChannel(ip);
} }
else 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(); IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
...@@ -274,20 +298,30 @@ void HokuyoLaserScanDevice::run() ...@@ -274,20 +298,30 @@ void HokuyoLaserScanDevice::run()
StringVariantBaseMap durations; StringVariantBaseMap durations;
durations["total_ms"] = new Variant(duration.toMilliSecondsDouble()); durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
durations["measure_ms"] = new Variant((time_measure - time_start).toMilliSecondsDouble()); durations["measure_ms"] =
durations["update_ms"] = new Variant((time_update - time_measure).toMilliSecondsDouble()); new Variant((time_measure - time_start).toMilliSecondsDouble());
durations["topic_sensor_ms"] = new Variant((time_topicSensor - time_update).toMilliSecondsDouble()); durations["update_ms"] =
durations["topic_health_ms"] = new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()); new Variant((time_update - time_measure).toMilliSecondsDouble());
debugObserver->setDebugChannel("LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations); 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) if (duration.toSecondsDouble() > 0.1)
{ {
ARMARX_WARNING << "Laserscanner reports are slow" ARMARX_WARNING << "Laserscanner reports are slow"
<< "Total time: " << duration.toMilliSecondsDouble() << "ms\n" << "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
<< "Measure: " << (time_measure - time_start).toMilliSecondsDouble() << "ms \n" << "Measure: "
<< "Update: " << (time_update - time_measure).toMilliSecondsDouble() << "ms\n" << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
<< "TopicSensor: " << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n" << "Update: "
<< "TopicHeart: " << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble() << "ms\n"; << (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 @@ ...@@ -22,14 +22,17 @@
#pragma once #pragma once
#include <vector>
#include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h> #include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/components/units/SensorActorUnit.h> #include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/components/RobotHealthInterface.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 <HokuyoLaserScannerDriver/urg_sensor.h>
#include <vector>
namespace armarx namespace armarx
{ {
...@@ -37,19 +40,30 @@ namespace armarx ...@@ -37,19 +40,30 @@ namespace armarx
* @class HokuyoLaserUnitPropertyDefinitions * @class HokuyoLaserUnitPropertyDefinitions
* @brief * @brief
*/ */
class HokuyoLaserUnitPropertyDefinitions: class HokuyoLaserUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
public armarx::ComponentPropertyDefinitions
{ {
public: public:
HokuyoLaserUnitPropertyDefinitions(std::string prefix): HokuyoLaserUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(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<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them"); defineOptionalProperty<float>("AngleOffset",
defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'"); -2.3561944902,
defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic"); "Offset is applied the raw angles before reporting them");
defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on"); 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 ...@@ -66,6 +80,7 @@ namespace armarx
bool reconnect(); bool reconnect();
void run(); void run();
RunningTask<HokuyoLaserScanDevice>::pointer_type task; RunningTask<HokuyoLaserScanDevice>::pointer_type task;
...@@ -73,7 +88,7 @@ namespace armarx ...@@ -73,7 +88,7 @@ namespace armarx
std::string componentName; std::string componentName;
LaserScannerUnitListenerPrx scanTopic; LaserScannerUnitListenerPrx scanTopic;
RobotHealthInterfacePrx robotHealthTopic; armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver; DebugObserverInterfacePrx debugObserver;
}; };
...@@ -93,10 +108,13 @@ namespace armarx ...@@ -93,10 +108,13 @@ namespace armarx
virtual public armarx::SensorActorUnit virtual public armarx::SensorActorUnit
{ {
public: public:
HokuyoLaserUnit();
/** /**
* @see armarx::ManagedIceObject::getDefaultName() * @see armarx::ManagedIceObject::getDefaultName()
*/ */
std::string getDefaultName() const override std::string
getDefaultName() const override
{ {
return "HokuyoLaserUnit"; return "HokuyoLaserUnit";
} }
...@@ -138,8 +156,8 @@ namespace armarx ...@@ -138,8 +156,8 @@ namespace armarx
float angleOffset = 0.0f; float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices; std::vector<HokuyoLaserScanDevice> devices;
LaserScannerInfoSeq connectedDevices; LaserScannerInfoSeq connectedDevices;
RobotHealthInterfacePrx robotHealthTopic;
DebugObserverInterfacePrx debugObserver; 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"?> <?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0"> <ui version="4.0">
<class>GuiHealthClientWidget</class> <class>GuiHealthClientWidget</class>
<widget class="QWidget" name="GuiHealthClientWidget"> <widget class="QWidget" name="GuiHealthClientWidget">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>500</width> <width>500</width>
<height>71</height> <height>71</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
<string>GuiHealthClientWidget</string> <string>GuiHealthClientWidget</string>
</property> </property>
<layout class="QGridLayout" name="gridLayout"> <layout class="QGridLayout" name="gridLayout">
<item row="0" column="1">
<widget class="QPushButton" name="pushButtonToggleOwnHeartbeats"> <item row="0" column="0" colspan="2">
<property name="text"> <widget class="QLabel" name="labelTags">
<string>Toggle</string> <property name="text">
</property> <string>...</string>
</widget> </property>
</item> </widget>
<item row="0" column="0"> </item>
<widget class="QLabel" name="labelState">
<property name="text"> <item row="0" column="2">
<string>TextLabel</string> <widget class="QPushButton" name="pushButtonUnrequireAll">
</property> <property name="text">
</widget> <string>Reset required tags</string>
</item> </property>
<item row="0" column="2"> </widget>
<spacer name="horizontalSpacer"> </item>
<property name="orientation">
<enum>Qt::Horizontal</enum> <item row="1" column="0" colspan="3">
</property> <widget class="QTableWidget" name="tableHealthState">
<property name="sizeHint" stdset="0"> </widget>
<size> </item>
<width>40</width> </layout>
<height>20</height> </widget>
</size> <resources />
</property> <connections />
</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/>
</ui> </ui>
source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
View file @ 4ee3e4f3
...@@ -22,67 +22,96 @@ ...@@ -22,67 +22,96 @@
#include "GuiHealthClientWidgetController.h" #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 <string>
#include <QLabel>
#include <QPushButton> #include <QPushButton>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
namespace armarx 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() GuiHealthClientWidgetController::GuiHealthClientWidgetController()
{ {
widget.setupUi(getWidget()); widget.setupUi(getWidget());
//ARMARX_IMPORTANT << "ctor start";
healthTimer = new QTimer(this); connect(this,
healthTimer->setInterval(10); SIGNAL(invokeConnectComponentQt()),
connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb())); this,
connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats())); SLOT(onConnectComponentQt()),
connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection); Qt::QueuedConnection);
connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection); connect(this,
SIGNAL(invokeDisconnectComponentQt()),
this,
SLOT(onDisconnectComponentQt()),
Qt::QueuedConnection);
connect(widget.pushButtonUnrequireAll,
SIGNAL(clicked()),
this,
SLOT(unrequireAll()));
updateSummaryTimer = new QTimer(this); updateSummaryTimer = new QTimer(this);
updateSummaryTimer->setInterval(100); updateSummaryTimer->setInterval(100);
connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb())); connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
widget.labelState->setText("idle.");
widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
//ARMARX_IMPORTANT << "ctor end";
} }
GuiHealthClientWidgetController::~GuiHealthClientWidgetController() GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
{ {
} }
void
void GuiHealthClientWidgetController::loadSettings(QSettings* settings) GuiHealthClientWidgetController::loadSettings(QSettings* settings)
{ {
} }
void GuiHealthClientWidgetController::saveSettings(QSettings* settings) void
GuiHealthClientWidgetController::saveSettings(QSettings* settings)
{ {
} }
void
void GuiHealthClientWidgetController::onInitComponent() GuiHealthClientWidgetController::onInitComponent()
{ {
//ARMARX_IMPORTANT << "onInitComponent"; //ARMARX_IMPORTANT << "onInitComponent";
usingProxy("RobotHealth"); usingProxy("RobotHealth");
offeringTopic("RobotHealthTopic"); offeringTopic("RobotHealthTopic");
} }
void GuiHealthClientWidgetController::healthTimerClb() void
GuiHealthClientWidgetController::healthTimerClb()
{ {
RobotHealthHeartbeatArgs rhha; armarx::core::time::dto::DateTime now;
rhha.maximumCycleTimeWarningMS = 250; armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
rhha.maximumCycleTimeErrorMS = 500; robotHealthTopicPrx->heartbeat(getName(), now);
robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
} }
void GuiHealthClientWidgetController::updateSummaryTimerClb()
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{ {
//ARMARX_IMPORTANT << "updateSummaryTimerClb"; //ARMARX_IMPORTANT << "updateSummaryTimerClb";
if (robotHealthComponentPrx) if (robotHealthComponentPrx)
...@@ -90,59 +119,152 @@ namespace armarx ...@@ -90,59 +119,152 @@ namespace armarx
//ARMARX_IMPORTANT << "has robotHealthComponentPrx"; //ARMARX_IMPORTANT << "has robotHealthComponentPrx";
try try
{ {
//ARMARX_IMPORTANT << "before set text"; auto summary = robotHealthComponentPrx->getSummary();
widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
//ARMARX_IMPORTANT << "after set text"; 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 (...) catch (...)
{} {
}
} }
} }
void GuiHealthClientWidgetController::toggleSendOwnHeartbeats() void
GuiHealthClientWidgetController::unrequireAll()
{ {
if (ownHeartbeatsActive) ARMARX_INFO << "UnrequireAll.";
{ if (robotHealthComponentPrx)
ownHeartbeatsActive = false;
healthTimer->stop();
robotHealthTopicPrx->unregister(getName());
widget.labelState->setText("idle.");
//widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
}
else
{ {
ownHeartbeatsActive = true; try
healthTimer->start(); {
widget.labelState->setText("sending heartbeats..."); robotHealthComponentPrx->resetRequiredTags();
widget.pushButtonToggleOwnHeartbeats->setText("unregister self"); }
catch(...){}
} }
} }
void
void GuiHealthClientWidgetController::onConnectComponent() GuiHealthClientWidgetController::onConnectComponent()
{ {
//ARMARX_IMPORTANT << "onConnectComponent"; //ARMARX_IMPORTANT << "onConnectComponent";
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic"); robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth"); robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
invokeConnectComponentQt(); invokeConnectComponentQt();
} }
void GuiHealthClientWidgetController::onConnectComponentQt()
void
GuiHealthClientWidgetController::onConnectComponentQt()
{ {
//healthTimer->start(); //healthTimer->start();
//ARMARX_IMPORTANT << "updateSummaryTimer->start"; //ARMARX_IMPORTANT <<ame "updateSummaryTimer->start";
updateSummaryTimer->start(); updateSummaryTimer->start();
} }
void
void GuiHealthClientWidgetController::onDisconnectComponent() GuiHealthClientWidgetController::onDisconnectComponent()
{ {
invokeDisconnectComponentQt(); invokeDisconnectComponentQt();
} }
void GuiHealthClientWidgetController::onDisconnectComponentQt()
void
GuiHealthClientWidgetController::onDisconnectComponentQt()
{ {
healthTimer->stop();
updateSummaryTimer->stop(); updateSummaryTimer->stop();
} }
} } // namespace armarx
source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
View file @ 4ee3e4f3
...@@ -110,7 +110,7 @@ namespace armarx ...@@ -110,7 +110,7 @@ namespace armarx
void onDisconnectComponentQt(); void onDisconnectComponentQt();
void healthTimerClb(); void healthTimerClb();
void updateSummaryTimerClb(); void updateSummaryTimerClb();
void toggleSendOwnHeartbeats(); void unrequireAll();
signals: signals:
/* QT signal declarations */ /* QT signal declarations */
...@@ -122,13 +122,9 @@ namespace armarx ...@@ -122,13 +122,9 @@ namespace armarx
* Widget Form * Widget Form
*/ */
Ui::GuiHealthClientWidget widget; Ui::GuiHealthClientWidget widget;
bool ownHeartbeatsActive = false;
RobotHealthInterfacePrx robotHealthTopicPrx; RobotHealthInterfacePrx robotHealthTopicPrx;
RobotHealthComponentInterfacePrx robotHealthComponentPrx; RobotHealthComponentInterfacePrx robotHealthComponentPrx;
QTimer* healthTimer;
QTimer* updateSummaryTimer; QTimer* updateSummaryTimer;
}; };
} }
source/RobotAPI/interface/aron/Aron.ice
View file @ 4ee3e4f3
...@@ -4,23 +4,27 @@ ...@@ -4,23 +4,27 @@
* Changelog: * Changelog:
* - Update 1.1.0: Allow templates in aron and anyobjects * - 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 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> #include <ArmarXCore/interface/core/BasicTypes.ice>
#define ARON_MAJOR "1" // Those macros stay defined
#define ARON_MINOR "1" #define ARON_MAJOR "2" // ice interface changes
#define ARON_PATCH "2" #define ARON_MINOR "0" // aron header changes
#define ARON_VERSION "1.1.2" #define ARON_PATCH "0" // aron cpp changes
#define ARON_VERSION "2.0.0"
module armarx module armarx
{ {
module aron 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; const string VERSION = ARON_VERSION;
/************************* /*************************
* Aron Types ************ * Aron Types ************
************************/ ************************/
...@@ -48,6 +52,13 @@ module armarx ...@@ -48,6 +52,13 @@ module armarx
FLOAT32, FLOAT32,
FLOAT64 FLOAT64
}; };
module default_value
{
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
}; };
module image module image
...@@ -57,6 +68,14 @@ module armarx ...@@ -57,6 +68,14 @@ module armarx
RGB24, RGB24,
DEPTH32 DEPTH32
}; };
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
}; };
module pointcloud module pointcloud
...@@ -71,6 +90,14 @@ module armarx ...@@ -71,6 +90,14 @@ module armarx
POINT_XYZRGBA, POINT_XYZRGBA,
POINT_XYZHSV POINT_XYZHSV
}; };
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
}; };
module matrix module matrix
...@@ -83,6 +110,14 @@ module armarx ...@@ -83,6 +110,14 @@ module armarx
FLOAT32, FLOAT32,
FLOAT64 FLOAT64
}; };
module default_value
{
const string IDENTITY = "identity";
const string ONES = "1";
const string ZEROS = "0";
const string DEFAULT = "";
}
}; };
module quaternion module quaternion
...@@ -92,46 +127,154 @@ module armarx ...@@ -92,46 +127,154 @@ module armarx
FLOAT32, FLOAT32,
FLOAT64 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 module dto
{ {
class GenericType { class GenericType
{
string VERSION = ARON_VERSION; string VERSION = ARON_VERSION;
Maybe maybe = Maybe::NONE; Maybe maybe = Maybe::NONE;
} };
sequence<GenericType> GenericTypeSeq; sequence<GenericType> GenericTypeSeq;
dictionary<string, GenericType> GenericTypeDict; dictionary<string, GenericType> GenericTypeDict;
/* ***** Container types ***** */ /* ***** Container types ***** */
class List extends GenericType { GenericType acceptedType; } class List extends GenericType
class Tuple extends GenericType { GenericTypeSeq elementTypes; } {
class Pair extends GenericType { GenericType acceptedType1; GenericType acceptedType2; } GenericType acceptedType;
class AronObject extends GenericType { AronObject parent; Ice::StringSeq templates; string objectName; Ice::StringSeq templateInstantiations; GenericTypeDict elementTypes; } };
class Dict 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) ***** */ /* ***** Complex Types (serialize to ndarray) ***** */
class NDArray extends GenericType { int ndimensions; ndarray::ElementType elementType; } class NDArray extends GenericType
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; } int ndimensions;
class PointCloud extends GenericType { pointcloud::VoxelType voxelType; } 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 ***** */ /* ***** 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 FloatEnum extends GenericType { string enumName; StringFloatDict acceptedValues; }
//class StringEnum extends GenericType { string enumName; StringStringDict acceptedValues; } //class StringEnum extends GenericType { string enumName; StringStringDict acceptedValues; }
/* ***** Any Types ***** */ /* ***** Any Types ***** */
class AnyObject extends GenericType { }; class AnyObject extends GenericType
{
};
/* ***** Primitive Types ***** */ /* ***** Primitive Types ***** */
class AronInt extends GenericType { }; class AronInt extends GenericType
class AronLong extends GenericType { }; {
class AronDouble extends GenericType { }; int defaultValue = 0; // <value>
class AronFloat extends GenericType { }; };
class AronString extends GenericType { };
class AronBool extends GenericType { }; 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 ...@@ -143,30 +286,67 @@ module armarx
{ {
module dto module dto
{ {
class GenericData { class GenericData
{
string VERSION = ARON_VERSION; string VERSION = ARON_VERSION;
}; };
sequence<GenericData> GenericDataSeq; sequence<GenericData> GenericDataSeq;
dictionary<string, GenericData> GenericDataDict; dictionary<string, GenericData> GenericDataDict;
/* ***** Container Data ***** */ /* ***** Container Data ***** */
class List extends GenericData { GenericDataSeq elements; }; class List extends GenericData
class Dict extends GenericData { GenericDataDict elements; }; {
GenericDataSeq elements;
};
class Dict extends GenericData
{
GenericDataDict elements;
};
/* ***** Complex Data ***** */ /* ***** Complex Data ***** */
// The NDArray contains more or less the same information as an AronType, but there is no other way to do it // 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") // 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) // 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 ***** */ /* ***** Primitive Data ***** */
class AronInt extends GenericData { int value; }; // They must be prefixed with 'Aron' as e.g. Int cannot be used as class name
class AronLong extends GenericData { long value; }; class AronInt extends GenericData
class AronDouble extends GenericData { double value; }; {
class AronFloat extends GenericData { float value; }; int value;
class AronString extends GenericData { string value; }; };
class AronBool extends GenericData { bool 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 // useful for memory ice_conversions
sequence<Dict> AronDictSeq; sequence<Dict> AronDictSeq;
......
source/RobotAPI/interface/components/RobotHealthInterface.ice
View file @ 4ee3e4f3
...@@ -22,8 +22,12 @@ ...@@ -22,8 +22,12 @@
#pragma once #pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/time.ice>
module armarx module armarx
{ {
enum RobotHealthState enum RobotHealthState
{ {
HealthOK, HealthWarning, HealthError HealthOK, HealthWarning, HealthError
...@@ -31,24 +35,74 @@ module armarx ...@@ -31,24 +35,74 @@ module armarx
struct RobotHealthHeartbeatArgs struct RobotHealthHeartbeatArgs
{ {
int maximumCycleTimeWarningMS = 100; string identifier;
int maximumCycleTimeErrorMS = 200; // bool requiredByDefault;
string message; string description;
Ice::StringSeq tags;
armarx::core::time::dto::Duration maximumCycleTimeWarning;
armarx::core::time::dto::Duration maximumCycleTimeError;
}; };
interface RobotHealthInterface interface RobotHealthInterface
{ {
void heartbeat(string componentName, RobotHealthHeartbeatArgs args); void signUp(RobotHealthHeartbeatArgs args);
void unregister(string componentName); 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 interface AggregatedRobotHealthInterface
{ {
void aggregatedHeartbeat(RobotHealthState overallHealthState); 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 interface RobotHealthComponentInterface extends RobotHealthInterface
{ {
string getSummary(); RobotHealthInfo getSummary();
}; };
}; };