From 2924426914e2fb03ca17a78d00d529c729d3f004 Mon Sep 17 00:00:00 2001
From: Michael Heine <michael.heine1@gmx.net>
Date: Wed, 10 Jan 2024 14:58:42 +0100
Subject: [PATCH] All parameters are now affected by the config file (#103)

---
 cfg/sick_usb.yaml           |  3 ++-
 src/sick_tim551_2050001.cpp | 40 +++++++++++++++++++------------------
 2 files changed, 23 insertions(+), 20 deletions(-)

diff --git a/cfg/sick_usb.yaml b/cfg/sick_usb.yaml
index 4f72de2..307442d 100644
--- a/cfg/sick_usb.yaml
+++ b/cfg/sick_usb.yaml
@@ -14,4 +14,5 @@ sick_driver:
     skip: 0
     frame_id: "laser"
     time_offset: -0.001
-    publish_datagram: false
\ No newline at end of file
+    publish_datagram: false
+    auto_reboot: true
\ No newline at end of file
diff --git a/src/sick_tim551_2050001.cpp b/src/sick_tim551_2050001.cpp
index fce638b..6ab32ab 100644
--- a/src/sick_tim551_2050001.cpp
+++ b/src/sick_tim551_2050001.cpp
@@ -47,26 +47,14 @@ int main(int argc, char ** argv)
   // rclcpp::NodeOptions options;
   // exec.add_node(node);
 
-  // check for TCP - use if ~hostname is set.
-  bool useTCP = false;
-  std::string hostname;
-  hostname = node->declare_parameter("hostname", "");
-  if (hostname != "") {
-    useTCP = true;
-  }
-  std::string port;
-  port = node->declare_parameter("port", "2112");
-
-  int timelimit;
-  timelimit = node->declare_parameter("timelimit", 5);
-
-  bool subscribe_datagram = false;
-  int device_number = 0;
+  node->declare_parameter("hostname", "");
+  node->declare_parameter("port", "2112");
+  node->declare_parameter("time_limit", 5);
   node->declare_parameter("subscribe_datagram", false);
   node->declare_parameter("device_number", 0);
-  // datagram publisher (only for debug)
-  node->declare_parameter("publish_datagram", false);
-  // Declare Sick Tim Parameters
+  node->declare_parameter("range_min", 0.05);
+  node->declare_parameter("range_max", 25.0);
+  node->declare_parameter("time_increment", 0.000061722);
   node->declare_parameter("min_ang", -0.75 * M_PI);
   node->declare_parameter("max_ang", 0.75 * M_PI);
   node->declare_parameter("intensity", true);
@@ -74,6 +62,20 @@ int main(int argc, char ** argv)
   node->declare_parameter("frame_id", "laser");
   node->declare_parameter("time_offset", -0.001);
   node->declare_parameter("auto_reboot", true);
+  // datagram publisher (only for debug)
+  node->declare_parameter("publish_datagram", false);
+
+  std::string hostname = node->get_parameter("hostname").as_string();
+  std::string port = node->get_parameter("port").as_string();
+  int time_limit = node->get_parameter("time_limit").as_int();
+  bool subscribe_datagram = node->get_parameter("subscribe_datagram").as_bool();
+  int device_number = node->get_parameter("device_number").as_int();
+
+  // check for TCP - use if ~hostname is set.
+  bool useTCP = false;
+  if (!hostname.empty()) {
+    useTCP = true;
+  }
 
   sick_tim::SickTim5512050001Parser * parser = new sick_tim::SickTim5512050001Parser(
     node->shared_from_this());
@@ -101,7 +103,7 @@ int main(int argc, char ** argv)
     if (subscribe_datagram) {
       s = new sick_tim::SickTimCommonMockup(parser, node, diagnostics);
     } else if (useTCP) {
-      s = new sick_tim::SickTimCommonTcp(hostname, port, timelimit, parser, node, diagnostics);
+      s = new sick_tim::SickTimCommonTcp(hostname, port, time_limit, parser, node, diagnostics);
     } else {
       s = new sick_tim::SickTimCommonUsb(parser, device_number, node, diagnostics);
     }