diff --git a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
index 57a92b84c82797807a38c8635dbe1fb7d3d3d43d..33ff12f5046dad45169c1661741ff7216055c26d 100644
--- a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
+++ b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
@@ -151,22 +151,6 @@
# ArmarX.SkillsMemory.ObjectName = ""
-# ArmarX.SkillsMemory.StatechartCoreSegmentName: Name of the core segment for statecharts.
-# Attributes:
-# - Default: Statechart
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillsMemory.StatechartCoreSegmentName = Statechart
-
-
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName: Name of the provider segment for statechart transitions.
-# Attributes:
-# - Default: Transitions
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName = Transitions
-
-
# ArmarX.SkillsMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Skills
diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
index 5ff571ebdb58c6f447e14154934e7b4cac213a35..b988f3b13d77676d1b3a85206cf37013b3ea36a4 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
@@ -143,11 +143,19 @@
# ArmarX.SickLaserUnit.ObjectName = ""
-# ArmarX.SickLaserUnit.angleOffset: offset to the scanning angle
+# ArmarX.SickLaserUnit.TopicName: Name of the laserscanner topic to report to.
# Attributes:
-# - Default: 0
+# - Default: LaserScans
# - Case sensitivity: yes
# - Required: no
+# ArmarX.SickLaserUnit.TopicName = LaserScans
+
+
+# ArmarX.SickLaserUnit.angleOffset: No Description
+# Attributes:
+# - Default: 0.0
+# - Case sensitivity: no
+# - Required: no
ArmarX.SickLaserUnit.angleOffset = 0.0
@@ -156,7 +164,45 @@ ArmarX.SickLaserUnit.angleOffset = 0.0
# - Default: LaserScannerFront,192.168.8.133,2112
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112
+ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112;LaserScannerBack,192.168.8.133,2112
+
+
+# ArmarX.SickLaserUnit.heartbeat.TopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeat.TopicName = DebugObserver
+
+
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS: TODO: maximumCycleTimeErrorMS
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS = ::_NOT_SET_::
+
+
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS: TODO: maximumCycleTimeWarningMS
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS = ::_NOT_SET_::
+
+
+# ArmarX.SickLaserUnit.heartbeatErrorMS: maximum cycle time before heartbeat Error
+# Attributes:
+# - Default: 800
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeatErrorMS = 800
+
+
+# ArmarX.SickLaserUnit.heartbeatWarnMS: maximum cycle time before heartbeat Warning
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SickLaserUnit.heartbeatWarnMS = 500
# ArmarX.SickLaserUnit.hostname: No Description
@@ -196,7 +242,7 @@ ArmarX.SickLaserUnit.protocol = ASCII
# - Default: 10
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.rangeMax = 10.00
+# ArmarX.SickLaserUnit.rangeMax = 10
# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
@@ -204,20 +250,21 @@ ArmarX.SickLaserUnit.rangeMax = 10.00
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.rangeMin = 0.01
+# ArmarX.SickLaserUnit.rangeMin = 0
# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
# Attributes:
+# - Default: sick_tim_5xx
# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
+# - Required: no
+# ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-# ArmarX.SickLaserUnit.timeIncrement: timeIncrement??
+# ArmarX.SickLaserUnit.timeIncrement: No Description
# Attributes:
-# - Default: 0.10000000000000001
-# - Case sensitivity: yes
+# - Default: 0.1
+# - Case sensitivity: no
# - Required: no
ArmarX.SickLaserUnit.timeIncrement = 0.1
@@ -230,22 +277,6 @@ ArmarX.SickLaserUnit.timeIncrement = 0.1
# ArmarX.SickLaserUnit.timelimit = 5
-# ArmarX.SickLaserUnit.topicName: Name of the topic
-# Attributes:
-# - Default: SICKLaserScanner
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.topicName = "myLaserTopic1"
-
-
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit: Name of the `LaserScannerUnit` topic to subscribe to.
-# Attributes:
-# - Default: SICKLaserScanner
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = SICKLaserScanner
-
-
# ArmarX.SickLaserUnit.updatePeriod: No Description
# Attributes:
# - Default: 1
diff --git a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
index 8fe3ebb2c6901804392acd2cbb09edd72dc472bf..c32f3a428572868209dc0789f6fbee00f37c1327 100644
--- a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
+++ b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
@@ -1,7 +1,7 @@
armarx_component_set_name(RobotControlUI)
-set(COMPONENT_LIBS RobotAPIOperations)
+set(COMPONENT_LIBS RobotAPIOperations Simox::SimoxUtility)
set(SOURCES main.cpp
RobotControlUI.cpp
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index f718a45434ec538fbd5d2541fb9413568f3e79f0..3cebd4612e8c94ddb015480e8079dd5859ed741e 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -39,6 +39,9 @@ using namespace Ice;
// ArmarX
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <opencv2/core/core_c.h>
using namespace Eigen;
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
index b12677c3ff545b9d18e7e11b95d2197c36434dd1..23861609c8852bae3dcfcc013e03ecb13d41c704 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -3,6 +3,10 @@ armarx_component_set_name("EarlyVisionGraph")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+set(COMPONENT_LIBS
+ Eigen3::Eigen
+)
+
set(SOURCES
./MathTools.cpp
./SphericalGraph.cpp
@@ -26,6 +30,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 ")
armarx_add_component("${SOURCES}" "${HEADERS}")
if(Eigen3_FOUND)
- target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR})
+ target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
endif()
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 5d0608286c7bf28b28f7ff2583ec3a111d471758..29fb7883065ee2f18e68dd5c909e17e8f8d8246c 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -24,6 +24,7 @@
#include "../EigenForwardDeclarations.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 04fa0b34bf676e53beae6ba6cdaf185598d72447..8fdadffeb177990068d890b26551b88d5712cc6f 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -25,10 +25,6 @@
// Include headers you only need in function definitions in the .cpp.
-// #include <Eigen/Core>
-
-// #include <SimoxUtility/color/Color.h>
-
namespace armarx
{
@@ -53,12 +49,12 @@ namespace armarx
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
+ scanData.clear();
result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
if (scanTopic)
{
TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
- scanData.clear();
+ scanTopic->reportSensorValues(scannerName, scannerName, scanData, scanT);
//trigger heartbeat
scannerHeartbeat->heartbeat(scannerName);
}
@@ -85,12 +81,12 @@ namespace armarx
void SickLaserScanDevice::initScanner()
{
- ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
- << "] [Port: " << this->port << "]";
+ ARMARX_INFO_S << "Start initialising scanner " << scannerName
+ << " [Ip: " << this->ip << "] [Port: " << this->port << "]";
// attempt to connect/reconnect
if (this->scanner)
{
- ARMARX_WARNING_S << "Scanner already initized.";
+ ARMARX_WARNING_S << "Scanner already initialized - reinit.";
this->scanner.reset(); // disconnect scanner
}
this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
@@ -108,7 +104,6 @@ namespace armarx
}
else
{
- ARMARX_INFO_S << "Reinitializing scanner.";
this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
}
}
@@ -131,12 +126,13 @@ namespace armarx
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
+ def->optional(properties.heartbeatWarnMS, "heartbeatWarnMS", "maximum cycle time before heartbeat Warning");
+ def->optional(properties.heartbeatErrorMS, "heartbeatErrorMS", "maximum cycle time before heartbeat Error");
return def;
}
SickLaserUnit::SickLaserUnit()
{
- //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
addPlugin(heartbeat);
ARMARX_CHECK_NOT_NULL(heartbeat);
}
@@ -145,6 +141,7 @@ namespace armarx
{
ARMARX_INFO << "On init";
// Topics and properties defined above are automagically registered.
+ //offeringTopic(properties.topicName);
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
@@ -163,18 +160,8 @@ namespace armarx
continue;
}
SickLaserScanDevice& device = scanDevices.emplace_back();
- device.scanTopic = topic;
device.scannerName = deviceInfo[0];
- if (deviceInfo[1] != "")
- {
- device.ip = deviceInfo[1];
- }
- else
- {
- ARMARX_FATAL << "TCP is not switched on. Probably hostname or port not set.";
- return;
-
- }
+ device.ip = deviceInfo[1];
device.port = deviceInfo[2];
device.timelimit = properties.timelimit;
//scanInfo
@@ -193,8 +180,10 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
+ armarx::RobotHealthHeartbeatArgs heartbeatArgs =
+ armarx::RobotHealthHeartbeatArgs(properties.heartbeatWarnMS, properties.heartbeatErrorMS, "No LaserScan data available");
device.scannerHeartbeat = heartbeat;
- device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
+ device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, heartbeatArgs);
}
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 662260de57aea4188e45bc6052061a9d01d19598..8fa16e4bf6961f13e1cd8acdf64b9acea8e40332 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-//#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <vector>
@@ -55,7 +54,7 @@ namespace armarx
struct SickLaserScanDevice
{
- std::string scannerName = "LaserScannerFront";
+ std::string scannerName;
//communication parameters
std::string ip;
std::string port;
@@ -125,18 +124,20 @@ namespace armarx
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string topicName = "SICKLaserScanner";
+ std::string topicName = "LaserScans";
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
std::string scannerType = "sick_tim_5xx";
int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
+ int heartbeatWarnMS = 500;
+ int heartbeatErrorMS = 800;
};
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
- plugins::HeartbeatComponentPlugin* heartbeat;
+ plugins::HeartbeatComponentPlugin* heartbeat = NULL;
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 715265cb484cabb48629d08b770ece7171a567d6..649d40ccf31ae409e140f651c40010c7fd22ad8b 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -88,15 +88,15 @@
//std::vector<unsigned char> receivedData(65536);
//static long receivedDataLen = 0;
-static int getDiagnosticErrorCode()
-{
-#ifdef _MSC_VER
-#undef ERROR
- return (2);
-#else
- return (diagnostic_msgs::DiagnosticStatus::ERROR);
-#endif
-}
+//static int getDiagnosticErrorCode()
+//{
+//#ifdef _MSC_VER
+//#undef ERROR
+// return (2);
+//#else
+// return (diagnostic_msgs::DiagnosticStatus::ERROR);
+//#endif
+//}
namespace armarx
{
@@ -154,9 +154,8 @@ namespace armarx
int actual_length = 0;
int packetsInLoop = 0;
- ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- //use ASCII always
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
+ //always use ASCII
+ int result = getDatagramIceTime(scanTime, receiveBuffer, &actual_length, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -168,19 +167,13 @@ namespace armarx
return sick_scan::ExitSuccess;
} // return success to continue looping
- //convert ros::Time recvTimeStamp to IceUtil
- ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
- uint64_t recvMsec = correctedStamp.toNSec() / 1000;
- scanTime = IceUtil::Time::microSeconds(recvMsec);
-
//datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
char* buffer_pos = (char*) receiveBuffer;
- char* dstart, *dend;
+ char* dstart = NULL;
+ char* dend = NULL;
bool dataToProcess = true;
std::vector<float> vang_vec;
vang_vec.clear();
- dstart = NULL;
- dend = NULL;
while (dataToProcess)
{
@@ -309,7 +302,7 @@ namespace armarx
}
}
- //ros::Time start_time = ros::Time::now(); // will be adjusted in the end
+ //IceUtil::Time start_time = IceUtil::Time::now(); // will be adjusted in the end
// <STX> (\x02)
// 0: Type of command (SN)
@@ -668,7 +661,7 @@ namespace armarx
* hereingekommen sind.
*/
- void SickScanAdapter::processFrame(ros::Time timeStamp, SopasEventMessage& frame)
+ void SickScanAdapter::processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame)
{
if (getProtocolType() == CoLa_A)
@@ -697,11 +690,8 @@ namespace armarx
void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
{
- ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
+ IceUtil::Time rcvTimeStamp = IceUtil::Time::now(); // stamp received datagram
bool beVerboseHere = false;
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
- beVerboseHere);
ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
@@ -949,10 +939,14 @@ namespace armarx
return sick_scan::ExitSuccess;
}
-
- int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize,
- int* actual_length,
+ int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
+ {
+ return sick_scan::ExitError;
+ }
+
+ int SickScanAdapter::getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer,
+ int* actual_length, int* numberOfRemainingFifoEntries)
{
if (NULL != numberOfRemainingFifoEntries)
{
@@ -988,61 +982,6 @@ namespace armarx
memcpy(receiveBuffer, &(dataBuffer[0]), size);
*actual_length = size;
-#if 0
- static int cnt = 0;
- char szFileName[255];
- sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
-
- FILE* fout;
-
- fout = fopen(szFileName, "wb");
- if (fout != NULL)
- {
- fwrite(receiveBuffer, size, 1, fout);
- fclose(fout);
- }
-#endif
- return sick_scan::ExitSuccess;
-
- if (!socket_.is_open())
- {
- ROS_ERROR("get_datagram: socket not open");
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
- return sick_scan::ExitError;
- }
-
- /*
- * Write a SOPAS variable read request to the device.
- */
- std::vector<unsigned char> reply;
-
- // Wait at most 5000ms for a new scan
- size_t timeout = 30000;
- bool exception_occured = false;
-
- char* buffer = reinterpret_cast<char*>(receiveBuffer);
-
- if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
- sick_scan::ExitSuccess)
- {
- ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
-
- // Attempt to reconnect when the connection was terminated
- if (!socket_.is_open())
- {
-#ifdef _MSC_VER
- Sleep(1000);
-#else
- sleep(1);
-#endif
- ARMARX_INFO << "Failure - attempting to reconnect";
- return init();
- }
-
- return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess; // keep on trying
- }
-
return sick_scan::ExitSuccess;
}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 1707952d9583de18bd6de004f986e216e9c13781..6fe671d13f2974a6fba85835dfb09cf35282858a 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -57,14 +57,14 @@ namespace armarx
class DatagramWithTimeStamp
{
public:
- DatagramWithTimeStamp(ros::Time timeStamp_, std::vector<unsigned char> datagram_)
+ DatagramWithTimeStamp(IceUtil::Time timeStamp_, std::vector<unsigned char> datagram_)
{
timeStamp = timeStamp_;
datagram = datagram_;
}
// private:
- ros::Time timeStamp;
+ IceUtil::Time timeStamp;
std::vector<unsigned char> datagram;
};
@@ -97,7 +97,7 @@ namespace armarx
SopasEventMessage findFrameInReceiveBuffer();
- void processFrame(ros::Time timeStamp, SopasEventMessage& frame);
+ void processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame);
// Queue<std::vector<unsigned char> > recvQueue;
Queue<DatagramWithTimeStamp> recvQueue;
@@ -129,9 +129,10 @@ namespace armarx
* \param [out] actual_length the actual amount of data written
* \param [in] isBinaryProtocol true=binary False=ASCII
*/
- virtual int
- get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
+ virtual int get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
+ bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
+
+ int getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer, int* actual_length, int* numberOfRemainingFifoEntries);
// Helpers for boost asio
int readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read = 0,
@@ -143,7 +144,6 @@ namespace armarx
private:
-
// Response buffer
UINT32 m_numberOfBytesInResponseBuffer; ///< Number of bytes in buffer
UINT8 m_responseBuffer[1024]; ///< Receive buffer for everything except scan data and eval case data.
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 899c86434cea6757fb0cbb899203131452dea07d..128043174d57794f4df610b2dd9d4658898e4d65 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -21,8 +21,10 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "WeissHapticSensor.h"
#include "TransmissionException.h"
+
#include <boost/regex.hpp>
#include <boost/format.hpp>
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
index 25191aab19608735c499bacfe3f9d7ae0fb1e401..169d1cf3690bcc2c53e091ab5343836dc93916cf 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -23,17 +23,19 @@
*/
#pragma once
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
#include "SerialInterface.h"
#include "TactileSensor.h"
+#include "TextWriter.h"
+
#include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
+
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
-//#include <ArmarXCore/util/variants/eigen3/Eigen3LibRegistry.h>
-#include "TextWriter.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <mutex>
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
index 640185fc12c57b7c880307132401d000225910c9..651d4534de98e41bc9972249aafce732a9231a90 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp
@@ -6,8 +6,7 @@
#include <SimoxUtility/meta/enum/adapt_enum.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-
-#include "box_to_grasp_candidates.ipp"
+#include <ArmarXCore/core/logging/Logging.h>
namespace simox::meta
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index def35d7f7fd3d23b19f301f7fd77aebdb2b7bbae..71a4240c81060d86e75cbf204ef3de726bd685a3 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -42,6 +42,7 @@ namespace armarx::VariantType
// variant types
const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
+
void suppressWarningUnusedVariableForLinkedPoseAndDirection();
}
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
index 4232b3c8dcf97dfe7ce3ad70987c36c558561fa5..9f1ee0943cdbc899358d2f074bac5151932e3703 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/util/StringHelpers.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
index addca00cdc7e3487aac217a3b03ee488f2287253..6c936c777acfbdd0b1d066ca0f5b2fcbbfef4072 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
@@ -23,13 +23,12 @@
*/
#pragma once
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ImportExport.h>
-#include <ArmarXCore/observers/ConditionCheck.h>
-
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
+#include <ArmarXCore/observers/ConditionCheck.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
index d2245928ea648583f10fc6ff132cafd063e3f1e9..679878188285c3ef977746d7f7e1431ea7e00319 100644
--- a/source/RobotAPI/libraries/core/math/StatUtils.h
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -22,7 +22,9 @@
#pragma once
-#include <math.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+
+#include <cmath>
#include <vector>
namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index 2e330626166b264497319869534e99a2f093896e..569afc8d18e400dac922e9793b8ba0dc256e1cc5 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -22,9 +22,12 @@
#pragma once
-#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <RobotAPI/interface/observers/ObserverFilters.h>
+
+#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <algorithm>
namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
index fbe93784ccb11229682a57d2c2d6ba7577c8adf8..699dc495760c1fb1862fe4eb0a28986361448ce8 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
@@ -21,6 +21,7 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "MedianDerivativeFilterV3.h"
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
index 3fffba8a157154642b2f7548ed055346b86fb5ef..47e3606321c73477abeb0a31bb4cf9990469c075 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -21,6 +21,7 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
#include "PoseMedianOffsetFilter.h"
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index 1b002dc20e5d631dec870f4f935c2a1a19e5cc36..e6cc65c23c13e154382042424db8483d879a89b4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -23,12 +23,13 @@
*/
#include "RemoteRobot.h"
-#include <VirtualRobot/VirtualRobot.h>
-
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx