diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 15153b3dca0bac00d1f8a6192191cead86227a05..f1e94f9b2478222809011ad2c150f2b97d0c9e72 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -33,7 +33,7 @@ armarx_add_component(
# RobotAPI
RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 5498ee1d8538633506bb7045128f5cf150ac1b55..04fa0b34bf676e53beae6ba6cdaf185598d72447 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -43,8 +43,6 @@ namespace armarx
{
initCnt++;
initScanner();
- //read the scanner parameters for initialization
- result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
}
else
{
@@ -59,10 +57,10 @@ namespace armarx
if (scanTopic)
{
TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(ip, frameName, scanData, scanT);
+ scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
scanData.clear();
//trigger heartbeat
- //heartbeat.heartbeat(channel);
+ scannerHeartbeat->heartbeat(scannerName);
}
else
{
@@ -87,25 +85,25 @@ namespace armarx
void SickLaserScanDevice::initScanner()
{
- this->isSensorInitialized = false;
ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
<< "] [Port: " << this->port << "]";
// attempt to connect/reconnect
if (this->scanner)
{
ARMARX_WARNING_S << "Scanner already initized.";
- delete this->scanner; // disconnect scanner
+ this->scanner.reset(); // disconnect scanner
}
-
- this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
-
+ this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
this->result = this->scanner->init();
-
+ if (this->result == sick_scan::ExitSuccess)
+ {
+ //read the scanner parameters for initialization
+ this->result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ }
if (this->result == sick_scan::ExitSuccess) // OK -> loop again
{
- this->isSensorInitialized = true;
- ARMARX_INFO_S << "Scanner initialized.";
+ ARMARX_INFO_S << "Scanner successfully initialized.";
this->runState = RunState::scannerRun; // after initialising switch to run state
}
else
@@ -123,9 +121,6 @@ namespace armarx
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
- // Subscribe to a topic (passing the topic name).
- //def->topic<LaserScannerUnitListenerPrx>(properties.topicName);
-
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
@@ -139,12 +134,18 @@ namespace armarx
return def;
}
+ SickLaserUnit::SickLaserUnit()
+ {
+ //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
+ addPlugin(heartbeat);
+ ARMARX_CHECK_NOT_NULL(heartbeat);
+ }
+
void SickLaserUnit::onInitComponent()
{
+ ARMARX_INFO << "On init";
// Topics and properties defined above are automagically registered.
- //offeringTopic(properties.topicName);
- //ARMARX_INFO_S << "SickLaserUnit is going to report on topic " << properties.topicName;
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
@@ -163,8 +164,7 @@ namespace armarx
}
SickLaserScanDevice& device = scanDevices.emplace_back();
device.scanTopic = topic;
- device.isSensorInitialized = false;
- device.frameName = deviceInfo[0];
+ device.scannerName = deviceInfo[0];
if (deviceInfo[1] != "")
{
device.ip = deviceInfo[1];
@@ -179,11 +179,11 @@ namespace armarx
device.timelimit = properties.timelimit;
//scanInfo
device.scanInfo.device = device.ip;
- device.scanInfo.frame = device.frameName;
+ device.scanInfo.frame = device.scannerName;
//scanner Parameters
try
{
- device.parser = new sick_scan::SickGenericParser(properties.scannerType);
+ device.parser = std::make_unique<sick_scan::SickGenericParser>(properties.scannerType);
device.parser->set_range_min(properties.rangeMin);
device.parser->set_range_max(properties.rangeMax);
device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
@@ -193,16 +193,13 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
-
- //addPlugin(heartbeat);
- //configureHeartbeatChannel();
+ device.scannerHeartbeat = heartbeat;
+ device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
}
}
void SickLaserUnit::onConnectComponent()
{
- //topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
-
for (SickLaserScanDevice& device : scanDevices)
{
device.scanTopic = topic;
@@ -246,20 +243,6 @@ namespace armarx
void SickLaserUnit::onExitComponent()
{
- for (SickLaserScanDevice& device : scanDevices)
- {
-
- if (device.scanner)
- {
- delete device.scanner;
- }
- if (device.parser)
- {
- delete device.parser;
- }
- }
-
-
}
std::string SickLaserUnit::getDefaultName() const
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index f6ffc67a1ad3992967c8a4526dd6640b21caa4fb..662260de57aea4188e45bc6052061a9d01d19598 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -27,7 +27,8 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+//#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <vector>
@@ -54,6 +55,7 @@ namespace armarx
struct SickLaserScanDevice
{
+ std::string scannerName = "LaserScannerFront";
//communication parameters
std::string ip;
std::string port;
@@ -63,15 +65,14 @@ namespace armarx
LaserScan scanData;
LaserScannerInfo scanInfo;
int initCnt = 0;
+ int result = sick_scan::ExitError;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
- std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
+ plugins::HeartbeatComponentPlugin* scannerHeartbeat;
//scanner pointers
- sick_scan::SickGenericParser* parser = 0;
- SickScanAdapter* scanner = 0;
- int result = sick_scan::ExitError;
- bool isSensorInitialized = false;
+ std::unique_ptr<sick_scan::SickGenericParser> parser;
+ std::unique_ptr<SickScanAdapter> scanner;
void initScanner();
void run();
@@ -90,13 +91,14 @@ namespace armarx
class SickLaserUnit :
//virtual public armarx::LaserScannerUnitInterface,
virtual public armarx::Component
- // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::RobotHealthComponentUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ SickLaserUnit();
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -134,29 +136,7 @@ namespace armarx
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
- //HeartbeatComponentPlugin heartbeat;
-
- /* Use a mutex if you access variables from different threads
- * (e.g. ice functions and RemoteGui_update()).
- std::mutex propertiesMutex;
- */
+ plugins::HeartbeatComponentPlugin* heartbeat;
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- /// Tab shown in the Remote GUI.
- struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
- {
- armarx::RemoteGui::Client::LineEdit boxLayerName;
- armarx::RemoteGui::Client::IntSpinBox numBoxes;
-
- armarx::RemoteGui::Client::Button drawBoxes;
- };
- RemoteGuiTab tab;
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- * When used from different threads, an ArViz client needs to be synchronized.
- /// Protects the arviz client inherited from the ArViz plugin.
- std::mutex arvizMutex;
- */
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 69d33ced49a00c415f7e08456d21177c27dfb8b6..982fe5c595528fb51e947433cabb6f44daa1887f 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -106,7 +106,6 @@ namespace armarx
sick_scan::SickScanCommon(parser),
socket_(io_service_),
deadline_(io_service_),
- parser_ptr(parser),
hostname_(hostname),
port_(port),
timelimit_(timelimit)
@@ -120,7 +119,7 @@ namespace armarx
{
this->setProtocolType(CoLa_B);
}
- assert(this->getProtocolType() != CoLa_Unknown);
+ ARMARX_CHECK(this->getProtocolType() != CoLa_Unknown);
m_numberOfBytesInReceiveBuffer = 0;
m_alreadyReceivedBytes = 0;
@@ -136,7 +135,7 @@ namespace armarx
SickScanAdapter::~SickScanAdapter()
{
- // stop_scanner();
+ //stopScanData();
close_device();
}
@@ -146,7 +145,7 @@ namespace armarx
/*!
- \brief parsing datagram and publishing ros messages
+ \brief parsing datagram into ARMARX message
\return error code
*/
int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
@@ -154,10 +153,10 @@ namespace armarx
unsigned char receiveBuffer[65536];
int actual_length = 0;
int packetsInLoop = 0;
- bool useBinaryProtocol = parser_ptr->getCurrentParamPtr()->getUseBinaryProtocol();
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
+ //use ASCII always
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -169,12 +168,10 @@ namespace armarx
return sick_scan::ExitSuccess;
} // return success to continue looping
- //TODO: convert ros::Time recvTimeStamp to IceUtil
+ //convert ros::Time recvTimeStamp to IceUtil
ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
uint64_t recvMsec = correctedStamp.toNSec() / 1000;
- scanTime = IceUtil::Time::milliSeconds(recvMsec);
- //scanTime = TimeUtil::GetTime();
- //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+ scanTime = IceUtil::Time::microSeconds(recvMsec);
//datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
char* buffer_pos = (char*) receiveBuffer;
@@ -461,7 +458,6 @@ namespace armarx
void SickScanAdapter::disconnectFunction()
{
-
}
void SickScanAdapter::disconnectFunctionS(void* obj)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 5b7e772f33217ed39e9c708275c6f7e5d10b2251..1707952d9583de18bd6de004f986e216e9c13781 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -72,10 +72,6 @@ namespace armarx
class SickScanAdapter : public sick_scan::SickScanCommon
{
public:
- int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
static void disconnectFunctionS(void* obj);
SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
@@ -83,6 +79,10 @@ namespace armarx
virtual ~SickScanAdapter();
+ int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
+ int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
@@ -163,7 +163,6 @@ namespace armarx
boost::system::error_code ec_;
size_t bytes_transfered_;
- sick_scan::SickGenericParser* parser_ptr;
std::string hostname_;
std::string port_;
int timelimit_;