From 6e11930b343f5137e7aed6a9fd69bd87b1795ea9 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Fri, 18 Jun 2021 12:01:48 +0200
Subject: [PATCH] implement parameters as ArmarX properties
---
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 476 +++++++++---------
.../drivers/SickLaserUnit/SickLaserUnit.h | 57 ++-
2 files changed, 273 insertions(+), 260 deletions(-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index a38cc31d3..4ee9c8b0b 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -28,216 +28,222 @@
// #include <SimoxUtility/color/Color.h>
-namespace armarx {
-
-armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions() {
- armarx::PropertyDefinitionsPtr def =
- new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
-
- // Subscribe to a topic (passing the topic name).
- // def->topic<PlatformUnitListener>("MyTopic");
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
-
- // Add a required property.
- def->required(properties.laserScannerTopicName, "laserScannerTopicName",
- "Name of the published Topic");
-
- // Add an optionalproperty.
- def->optional(properties.updatePeriod, "updatePeriod", "Update every ? ms");
- def->optional(properties.angleOffset, "angleOffset",
- "Offset to the LaserScanner Angle");
- def->optional(properties.devices, "devices", "List of Device Names");
-
- return def;
-}
-
-void SickLaserUnit::onInitComponent() {
- // Topics and properties defined above are automagically registered.
-
- // Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
-
- std::string topicName =
- getProperty<std::string>("laserScannerTopicName").getValue();
- offeringTopic(topicName);
- ARMARX_INFO << "Going to report on topic " << topicName;
- int updatePeriod = getProperty<int>("updatePeriod").getValue();
- float angleOffset = getProperty<float>("angleOffset").getValue();
-
- std::string deviceStrings = getProperty<std::string>("devices").getValue();
- std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
-
- ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
-
- std::string scannerName;
- if (false == nhPriv.getParam("scanner_type", scannerName)) {
- ROS_ERROR("cannot find parameter "
- "scanner_type"
- " in the param set. Please specify scanner_type.");
- ROS_ERROR("Try to set %s as fallback.\n", nodeName.c_str());
- scannerName = nodeName;
- }
-
- bool useTCP = false;
- std::string hostname;
- if (nhPriv.getParam("hostname", hostname)) {
- useTCP = true;
- }
- bool changeIP = false;
- std::string sNewIp;
- if (nhPriv.getParam("new_IP_address", sNewIp)) {
- changeIP = true;
- }
- std::string port;
- nhPriv.param<std::string>("port", port, "2112");
-
- int timelimit;
- nhPriv.param("timelimit", timelimit, 5);
-
- bool subscribe_datagram;
- int device_number;
- nhPriv.param("subscribe_datagram", subscribe_datagram, false);
- nhPriv.param("device_number", device_number, 0);
-
- sick_scan::SickGenericParser *parser =
- new sick_scan::SickGenericParser(scannerName);
-
- double param;
- char colaDialectId = 'A'; // A or B (Ascii or Binary)
-
- if (nhPriv.getParam("range_min", param)) {
- parser->set_range_min(param);
- }
- if (nhPriv.getParam("range_max", param)) {
- parser->set_range_max(param);
- }
- if (nhPriv.getParam("time_increment", param)) {
- parser->set_time_increment(param);
- }
-
- /*
- * Check, if parameter for protocol type is set
- */
- bool use_binary_protocol = true;
- if (true == nhPriv.getParam("emul_sensor", emulSensor)) {
- ROS_INFO("Found emul_sensor overwriting default settings. Emulation: %s\n",
- emulSensor ? "True" : "False");
- }
- if (true == nhPriv.getParam("use_binary_protocol", use_binary_protocol)) {
- ROS_INFO("Found sopas_protocol_type param overwriting default protocol:");
- if (use_binary_protocol == true) {
- ROS_INFO("Binary protocol activated");
- } else {
- if (parser->getCurrentParamPtr()->getNumberOfLayers() > 4) {
- nhPriv.setParam("sopas_protocol_type", true);
- use_binary_protocol = true;
- ROS_WARN(
- "This scanner type does not support ASCII communication.\n"
- "Binary communication has been activated.\n"
- "The parameter \"sopas_protocol_type\" has been set to \"True\".");
- } else {
- ROS_INFO("ASCII protocol activated");
- }
+namespace armarx
+{
+
+ std::string protocolToString(SickLaserUnit::Protocol protocol)
+ {
+ std::string protocolStr;
+
+ switch (protocol)
+ {
+ case SickLaserUnit::Protocol::ASCII:
+ protocolStr = "ASCII";
+ break;
+ case SickLaserUnit::Protocol::Binary:
+ protocolStr = "Binary";
+ break;
+ }
+ return modeStr;
}
- parser->getCurrentParamPtr()->setUseBinaryProtocol(use_binary_protocol);
- }
-
- if (parser->getCurrentParamPtr()->getUseBinaryProtocol()) {
- colaDialectId = 'B';
- } else {
- colaDialectId = 'A';
- }
-
- int result = sick_scan::ExitError;
-
- sick_scan::SickScanConfig cfg;
-}
-
-void SickLaserUnit::onConnectComponent() {
- ROS_INFO("Start initialising scanner [Ip: %s] [Port: %s]", hostname.c_str(),
- port.c_str());
- // attempt to connect/reconnect
- delete s; // disconnect scanner
- if (useTCP) {
- s = new sick_scan::SickScanCommonTcp(hostname, port, timelimit, parser,
- colaDialectId);
- } else {
- ROS_ERROR("TCP is not switched on. Probably hostname or port not set. Use "
- "roslaunch to start node.");
- exit(-1);
- }
-
- if (emulSensor) {
- s->setEmulSensor(true);
- }
- result = s->init();
-
- isInitialized = true;
- signal(SIGINT,
- SIG_DFL); // change back to standard signal handler after initialising
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- if (changeIP) {
- runState = scanner_finalize;
+
+ armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+ //communication parameters
+ def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
+ def->optional(properties.newIpAddress, "newIpAddress", "New IP address for the LaserScanner");
+ def->optional(properties.port, "port", "port to use on the LaserScanner");
+ def->optional(properties.timelimit, "timelimit", "timelimit for communication");
+ def->optional(properties.subscribeDatagram, "subscribeDatagram", "subscribe to Datagram in communication or not");
+ def->optional(properties.protocol, "protocol", "Either use ASCII or Binary protocol")
+ .map(protocolToString(Protocol::ASCII), Protocol::ASCII)
+ .map(protocolToString(Protocol::Binary), Protocol::Binary);
+ def->optional(properties.sopasProtocolType, "sopasProtocolType", "Automatically set to true if the Scanner does not support ASCII communication");
+ //Scanner parameters
+ def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
+ def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
+ def->required(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
+ def->required(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
+ def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
+ //Additional configuration
+ def->optional(properties.emulSensor, "emulateSensor", "overwrite the default Settings and don't connect to Scanner");
+
+ return def;
}
- runState = scanner_run; // after initialising switch to run state
- } else {
- runState = scanner_init; // If there was an error, try to restart scanner
- }
- // Do things after connecting to topics and components.
-
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
- // Use the debug observer to log data over time.
- // 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();
- }
- */
-}
-
-void SickLaserUnit::run() {
- while (!task->isStopped()) {
- if (result == sick_scan::ExitSuccess) // OK -> loop again
+
+ void SickLaserUnit::onInitComponent()
{
- ros::spinOnce();
- result = s->loopOnce();
- } else {
- runState = scanner_finalize; // interrupt
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+
+ ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
+
+ bool useTCP = false;
+ if (properties.hostname != "")
+ {
+ useTCP = true;
+ }
+ bool changeIP = false;
+ if (sNewIp != "")
+ {
+ changeIP = true;
+ }
+ //scanner Parameters
+ sick_scan::SickGenericParser* parser = new sick_scan::SickGenericParser(properties.scannerType);
+ parser->set_range_min(properties.rangeMin);
+ parser->set_range_max(properties.rangeMax);
+ parser->set_time_increment(properties.timeIncrement);
+ char colaDialectId = 'A'; // A or B (Ascii or Binary)
+
+ if (properties.emulSensor)
+ {
+ ARMARX_INFO("Found paraemter emulSensor overwriting default settings. Emulation: True\n");
+ }
+ switch (properties.protocol)
+ {
+ case Protocol::ASCII:
+ if (parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
+ {
+ ARMARX_WARNING(
+ "This scanner type does not support ASCII communication.\n"
+ "Binary communication has been activated.\n"
+ "The parameter \"sopasProtocolType\" has been set to \"true\".");
+ properties.sopasProtocolType = true;
+ properties.protocol = Protocol::Binary;
+ }
+ else
+ {
+ ARMARX_INFO("ASCII protocol activated");
+ }
+ break;
+ case Protocol::Binary:
+ ARMARX_INFO("Binary protocol activated");
+ break;
+ default:
+ ARMARX_WARNING("Unknown protocol type. Defaulting to Binary protocol.\n");
+ properties.protocol = Protocol::Binary;
+ }
+ if (properties.protocol == Protocol::ASCII)
+ {
+ parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
+ colaDialectId = 'A';
+ }
+ else
+ {
+ parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
+ colaDialectId = 'B';
+ }
}
- }
-}
-void SickLaserUnit::onDisconnectComponent() {}
+ void SickLaserUnit::onConnectComponent()
+ {
+ ARMARX_INFO("Start initialising scanner [Ip: %s] [Port: %s]", hostname.c_str(),
+ port.c_str());
+ // attempt to connect/reconnect
+ delete s; // disconnect scanner
+ if (useTCP)
+ {
+ s = new sick_scan::SickScanCommonTcp(hostname, port, timelimit, parser,
+ colaDialectId);
+ }
+ else
+ {
+ ROS_ERROR("TCP is not switched on. Probably hostname or port not set. Use "
+ "roslaunch to start node.");
+ exit(-1);
+ }
-void SickLaserUnit::onExitComponent() {}
+ if (emulSensor)
+ {
+ s->setEmulSensor(true);
+ }
+ result = s->init();
-std::string SickLaserUnit::getDefaultName() const { return "SickLaserUnit"; }
+ isInitialized = true;
+ signal(SIGINT,
+ SIG_DFL); // change back to standard signal handler after initialising
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ if (changeIP)
+ {
+ runState = scanner_finalize;
+ }
+ runState = scanner_run; // after initialising switch to run state
+ }
+ else
+ {
+ runState = scanner_init; // If there was an error, try to restart scanner
+ }
+ // Do things after connecting to topics and components.
-/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
-void SickLaserUnit::createRemoteGuiTab()
-{
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // 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();
+ }
+ */
+ }
+
+ void SickLaserUnit::run()
+ {
+ while (!task->isStopped())
+ {
+ if (result == sick_scan::ExitSuccess) // OK -> loop again
+ {
+ ros::spinOnce();
+ result = s->loopOnce();
+ }
+ else
+ {
+ runState = scanner_finalize; // interrupt
+ }
+ }
+ }
+
+ void SickLaserUnit::onDisconnectComponent() {}
+
+ void SickLaserUnit::onExitComponent() {}
+
+ std::string SickLaserUnit::getDefaultName() const
+ {
+ return "SickLaserUnit";
+ }
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void SickLaserUnit::createRemoteGuiTab()
+ {
using namespace armarx::RemoteGui::Client;
// Setup the widgets.
@@ -254,62 +260,62 @@ void SickLaserUnit::createRemoteGuiTab()
GridLayout grid;
int row = 0;
{
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
+ 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(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
}
VBoxLayout root = {grid, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
-}
+ }
-void SickLaserUnit::RemoteGui_update()
-{
+ 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();
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
+ {
+ 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);
+ // 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)
-{
+ /* (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()));
+ 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 5eaf5a13c..b4233d8c9 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -22,19 +22,19 @@
#pragma once
-
// #include <mutex>
#include <ArmarXCore/core/Component.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+// #include
+// <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+// #include
+// <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-# include <vector>
-
+#include <vector>
namespace armarx
{
@@ -50,20 +50,21 @@ namespace armarx
*
* Detailed description of class SickLaserUnit.
*/
- class SickLaserUnit :
- virtual public armarx::Component
+ class SickLaserUnit : virtual public armarx::Component
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
-
+ enum class Protocol
+ {
+ ASCII,
+ Binary
+ };
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -79,7 +80,6 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
-
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
/// need to re-create the Remote GUI tab.
@@ -91,9 +91,7 @@ namespace armarx
void RemoteGui_update() override;
*/
-
private:
-
// Private methods go here.
// Forward declare `Properties` if you used it before its defined.
@@ -104,27 +102,39 @@ namespace armarx
void drawBoxes(const Properties& p, viz::Client& arviz);
*/
-
private:
-
// Private member variables go here.
-
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string laserScannerTopicName;
- int updatePeriod = 1;
- float angleOffset = 0.0;
- std::string devices = "Device1; Scanner2";
+ //communication parameters
+ std::string hostname = "";
+ std::string newIpAddress = "";
+ std::string port = "2112";
+ int timelimit = 5;
+ bool subscribeDatagram = false;
+ Protocol protocol = Protocol::Binary;
+ bool sopasProtocolType = false;
+ //scanner parameters
+ std::string scannerType;
+ int deviceNumber = 0;
+ double rangeMin;
+ double rangeMax;
+ double timeIncrement;
+ //additional parameters
+ bool emulSensor = false;
};
Properties properties;
+
+ char colaDialectId = 'B';
+ int result = sick_scan::ExitError;
+ sick_scan::SickScanConfig cfg;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
*/
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
/// Tab shown in the Remote GUI.
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
@@ -137,13 +147,10 @@ namespace armarx
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
--
GitLab