diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 0c3653739f828306ca5a5b16d4c58ba45d5a57cf..1294f9b62f512fe5aec6f2c5a072b6dd57e6eb9e 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -32,4 +32,3 @@ add_subdirectory(ViewSelection)
add_subdirectory(SkillObserver)
add_subdirectory(ArticulatedObjectLocalizerExample)
-add_subdirectory(drivers/SickLaserUnit)
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index 731765e4bbcbdcb597b34f5fb6de5a5b9ae7b0ed..f8de74539d68dfc44a556fb478d97f75ccaf22d6 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -7,3 +7,4 @@ add_subdirectory(GamepadUnit)
add_subdirectory(MetaWearIMU)
add_subdirectory(KITProstheticHandDriver)
add_subdirectory(KITProsthesisIceDriver)
+add_subdirectory(SickLaserUnit)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 2ff404a3f777dc95230831c536b0a530190f4858..6d65f022c4f228e3a01d8b6292ee218484bb47df 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -22,7 +22,7 @@ armarx_add_component(
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
- ## RobotAPICore
+ RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 024eda12b630a43d2e333166970efa73ae12c915..2de3d841544012b560f42dbad14083f332378adf 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -28,169 +28,291 @@
// #include <SimoxUtility/color/Color.h>
+namespace armarx {
-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");
+armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions() {
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
+ // 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.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ // Add a required property.
+ def->required(properties.laserScannerTopicName, "laserScannerTopicName",
+ "Name of the published Topic");
- // Add an optionalproperty.
- def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+ // 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;
+}
- 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");
+ }
}
+ parser->getCurrentParamPtr()->setUseBinaryProtocol(use_binary_protocol);
+ }
+ if (parser->getCurrentParamPtr()->getUseBinaryProtocol()) {
+ colaDialectId = 'B';
+ } else {
+ colaDialectId = 'A';
+ }
- void SickLaserUnit::onInitComponent()
- {
- // Topics and properties defined above are automagically registered.
-
- // Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
- }
+ int result = sick_scan::ExitError;
+ sick_scan::SickScanConfig cfg;
+}
- void SickLaserUnit::onConnectComponent()
- {
- // 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::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;
}
+ 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::onDisconnectComponent()
+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()
- {
-
- }
+void SickLaserUnit::onExitComponent() {}
+std::string SickLaserUnit::getDefaultName() const { return "SickLaserUnit"; }
- std::string SickLaserUnit::getDefaultName() const
- {
- return "SickLaserUnit";
- }
+/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+void SickLaserUnit::createRemoteGuiTab()
+{
+ using namespace armarx::RemoteGui::Client;
+ // Setup the widgets.
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- void SickLaserUnit::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
+ tab.boxLayerName.setValue(properties.boxLayerName);
- // Setup the widgets.
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
- tab.boxLayerName.setValue(properties.boxLayerName);
+ tab.drawBoxes.setLabel("Draw Boxes");
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
+ // Setup the layout.
- tab.drawBoxes.setLabel("Draw Boxes");
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
- // Setup the layout.
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
- GridLayout grid;
- int row = 0;
- {
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+}
- 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();
+ }
}
-
-
- void SickLaserUnit::RemoteGui_update()
+ if (tab.drawBoxes.wasClicked())
{
- 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);
- }
+ // 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)
{
- // 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);
+ 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 8b681b1f5dfa17b947855f3849928d8fd7452b16..5eaf5a13cc3e47f949496e7f8e4e84b9417315a5 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -33,6 +33,8 @@
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+# include <vector>
+
namespace armarx
{
@@ -50,9 +52,9 @@ namespace armarx
*/
class SickLaserUnit :
virtual public armarx::Component
- // , virtual public armarx::DebugObserverComponentPluginUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
{
public:
@@ -111,8 +113,10 @@ namespace armarx
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string boxLayerName;
- int numBoxes = 10;
+ std::string laserScannerTopicName;
+ int updatePeriod = 1;
+ float angleOffset = 0.0;
+ std::string devices = "Device1; Scanner2";
};
Properties properties;
/* Use a mutex if you access variables from different threads