From 848fbaa5d2c479538432785d42e39fdab7ac842f Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Wed, 28 Jul 2021 10:43:27 +0200
Subject: [PATCH] clean up properties
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 146 ++++--------------
.../drivers/SickLaserUnit/SickLaserUnit.h | 15 +-
.../drivers/SickLaserUnit/SickScanAdapter.cpp | 3 +-
3 files changed, 37 insertions(+), 127 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 6c84b806f..1dc755147 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -39,9 +39,18 @@ namespace armarx
switch (runState)
{
case RunState::scannerInit:
- initScanner();
- //read the scanner parameters for initialization
- result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ if (initCnt < 5)
+ {
+ initCnt++;
+ initScanner();
+ //read the scanner parameters for initialization
+ result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ }
+ else
+ {
+ ARMARX_WARNING << "Maximum number of reinitializations reached - going to idle state";
+ runState = RunState::scannerFinalize;
+ }
break;
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
@@ -66,7 +75,8 @@ namespace armarx
}
break;
case RunState::scannerFinalize:
- ARMARX_WARNING << "scanner offline";
+ ARMARX_WARNING << "Scanner offline - stopping task.";
+ this->task->stop();
break;
default:
ARMARX_WARNING << "Invalid run state in task loop";
@@ -124,12 +134,10 @@ namespace armarx
def->optional(properties.topicName, "topicName", "Name of the topic");
//Scanner parameters
def->optional(properties.devices, "devices", "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
- def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
+ 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.timeIncrement, "timeIncrement", "timeIncrement??");
return def;
}
@@ -137,12 +145,12 @@ namespace armarx
{
// 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);
- ARMARX_INFO_S << "initializing SickLaserUnit.";
-
std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
scanDevices.clear();
scanDevices.reserve(splitDeviceStrings.size());
@@ -152,10 +160,10 @@ namespace armarx
if (deviceInfo.size() != 3)
{
ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << deviceInfo.size() << ")";
+ << " (split size: " << deviceInfo.size() << ", expected: 3)";
continue;
}
- SickLaserScanDevice& device = scanDevices.emplace_back();
+ SickLaserScanDevice device;
device.scanTopic = topic;
device.isSensorInitialized = false;
device.frameName = deviceInfo[0];
@@ -165,37 +173,36 @@ namespace armarx
device.ip = deviceInfo[1];
}
device.port = deviceInfo[2];
- device.angleOffset = properties.angleOffset;
- device.timelimit = properties.timelimit;
device.scannerType = properties.scannerType;
+ device.timelimit = properties.timelimit;
device.rangeMin = properties.rangeMin;
device.rangeMax = properties.rangeMax;
- device.timeIncrement = properties.timeIncrement;
+ //scanInfo
+ device.scanInfo.device = device.ip;
+ device.scanInfo.frame = device.frameName;
//scanner Parameters
try
{
device.parser = new sick_scan::SickGenericParser(device.scannerType);
device.parser->set_range_min(properties.rangeMin);
device.parser->set_range_max(properties.rangeMax);
- device.parser->set_time_increment(properties.timeIncrement);
+ device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
}
catch (std::exception const& e)
{
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
+ scanDevices.push_back(device);
}
- device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- device.colaDialectId = 'A';
- }
- //addPlugin(heartbeat);
- //configureHeartbeatChannel();
+ //addPlugin(heartbeat);
+ //configureHeartbeatChannel();
+ }
}
void SickLaserUnit::onConnectComponent()
{
topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
- offeringTopic(properties.topicName);
for (SickLaserScanDevice& device : scanDevices)
{
@@ -217,23 +224,9 @@ namespace armarx
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
}
*/
}
@@ -266,81 +259,4 @@ namespace armarx
return "SickLaserUnit";
}
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- void SickLaserUnit::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- // Setup the widgets.
-
- tab.boxLayerName.setValue(properties.boxLayerName);
-
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
-
- tab.drawBoxes.setLabel("Draw Boxes");
-
- // Setup the layout.
-
- GridLayout grid;
- int row = 0;
- {
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
-
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
-
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
- }
-
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
-
- void SickLaserUnit::RemoteGui_update()
- {
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
- {
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
-
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- }
- if (tab.drawBoxes.wasClicked())
- {
- // Lock shared variables in methods running in seperate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex, arvizMutex);
- drawBoxes(properties, arviz);
- }
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client&
- arviz)
- {
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
-
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
- {
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
- }
- arviz.commit(layer);
- }
- */
-
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 69d6385c1..2829a8e8f 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -56,26 +56,23 @@ namespace armarx
{
//scanner parameters
std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
- double rangeMin;
- double rangeMax;
- double timeIncrement;
//communication parameters
std::string ip;
std::string port;
int timelimit = 5;
+ double rangeMin = 0.0;
+ double rangeMax = 10.0;
bool useTcp = false;
- char colaDialectId = 'A';
//data and task pointers
IceUtil::Time scanTime;
LaserScan scanData;
LaserScannerInfo scanInfo;
+ int initCnt = 0;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
-
- sick_scan::SickScanConfig cfg;
+ //scanner pointers
sick_scan::SickGenericParser* parser;
SickScanAdapter* scanner;
int result = sick_scan::ExitError;
@@ -134,12 +131,10 @@ namespace armarx
std::string topicName = "SICKLaserScanner";
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
- int timelimit = 5;
std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
+ int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
- double timeIncrement = 0.1;
};
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 1e9a011f3..0d2e9368b 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -111,7 +111,6 @@ namespace armarx
port_(port),
timelimit_(timelimit)
{
-
if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
{
this->setProtocolType(CoLa_A);
@@ -347,6 +346,7 @@ namespace armarx
// 23: Starting angle (FFF92230)
if (updateScannerInfo)
{
+ scanInfo.device = hostname_;
uint starting_angle = (uint) - 1;
sscanf(fields[23], "%x", &starting_angle);
scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
@@ -438,7 +438,6 @@ namespace armarx
ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
return sick_scan::ExitError;
}
- //TODO: Write ScanSteps with intensity
scanData.reserve(distVal.size());
for (int i = 0; i < (int) distVal.size(); i++)
{
--
GitLab