implement parameters as ArmarX properties

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

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