Ouster lidar - log UDP

config/test-ouster_lidar-udp.json

@@ -0,0 +1,52 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "lidar_udp": {
+          "driver": "udp",
+          "out": ["raw"],
+          "init": {
+              "host": "192.168.1.91",
+              "port": 7502,
+              "bufsize": 100000
+          }
+      },
+      "imu_udp": {
+          "driver": "udp",
+          "out": ["raw"],
+          "init": {
+              "host": "192.168.1.91",
+              "port": 7503,
+              "bufsize": 1024
+        }
+      },
+      "lidar_http": {
+          "driver": "http",
+          "in": ["raw"],
+          "out": ["response"],
+          "init": {
+              "output": true,
+              "timeout": 1.0
+          }
+      },
+
+      "app": {
+          "driver": "osgar.drivers.ouster_lidar:OusterLidarDummy",
+          "in": ["udp_packet", "http_response"],
+          "out": ["http_request"],
+          "init": {
+            "lidar_url": "http://192.168.1.91/api/v1/sensor/config",
+            "config_params": {
+              "lidar_mode": "512x10",
+              "udp_profile_lidar": "LEGACY"
+            }
+        }
+      }
+    },
+    "links": [
+      ["lidar_udp.raw", "app.udp_packet"],
+      ["lidar_http.response", "app.response"],
+      ["app.http_request", "lidar_http.raw"]
+    ]
+  }
+}

osgar/drivers/logsocket.py

@@ -175,18 +175,29 @@ def _send(self, data):
 
 class LogHTTP:
     def __init__(self, config, bus):
-        bus.register('raw')
-        self.input_thread = Thread(target=self.run_input, daemon=True)
+        bus.register('raw', 'response')
+        self.url = config.get('url')
+        self.output = config.get('output', False)
+        assert self.url or self.output
+        if self.url:
+            self.input_thread = Thread(target=self.run_input, daemon=True)
+        if self.output:
+            self.output_thread = Thread(target=self.run_output, daemon=True)
 
-        self.url = config['url']
         self.sleep = config.get('sleep', None)
         self.bus = bus
 
     def start(self):
-        self.input_thread.start()
+        if self.url:
+            self.input_thread.start()
+        if self.output:
+            self.output_thread.start()
 
     def join(self, timeout=None):
-        self.input_thread.join(timeout=timeout)
+        if self.url:
+            self.input_thread.join(timeout=timeout)
+        if self.output:
+            self.output_thread.join(timeout=timeout)
 
     def run_input(self):
         while self.bus.is_alive():
@@ -202,6 +213,31 @@ def run_input(self):
             if self.sleep is not None:
                 self.bus.sleep(self.sleep)
 
+    def run_output(self):
+        try:
+            while True:
+                __, channel, out_data = self.bus.listen()
+                if channel == 'raw':
+                    if isinstance(out_data, list):
+                        assert len(out_data) == 3, out_data  # unsupported output data
+                        url, header, data = out_data
+                        request = urllib.request.Request(url, data=data, headers=header, method='POST')
+                    elif isinstance(out_data, str):  # url only
+                        request = out_data
+                    else:
+                        assert False, out_data  # unsupported output data
+
+                    with urllib.request.urlopen(request, timeout=0.5) as f:
+                        response = f.read()
+                    if len(response) > 0:
+                        self.bus.publish('response', response)
+
+                else:
+                    assert False, channel  # unsupported channel
+
+        except BusShutdownException:
+            pass
+
     def request_stop(self):
         self.bus.shutdown()
 

osgar/drivers/ouster_lidar.py

@@ -0,0 +1,47 @@
+"""
+  Ouster lidar drivers
+"""
+
+import json
+
+from osgar.node import Node
+
+
+class OusterLidarDummy(Node):
+    def __init__(self, config, bus):
+        super().__init__(config, bus)
+        bus.register('http_request', 'lidar_config')
+        self.lidar_url = config["lidar_url"]
+        self.config_params = config.get("config_params")
+        self.headers = {'Accept': 'application/json', 'Content-Type': 'application/json'}
+        self.configuration_done = False
+        self.configuration_saved = False
+        self.verbose = False
+
+    def send_conf_params(self):
+        data = json.dumps(self.config_params).encode('utf-8')
+        self.publish("http_request", [self.lidar_url, self.headers, data])
+
+    def request_configuration(self):
+        self.publish("http_request", self.lidar_url)
+
+    def process_udp(self, packet):
+        pass
+        # print(len(packet))
+
+    def on_udp_packet(self, data):
+        if not self.configuration_done:
+            if self.config_params:
+                self.send_conf_params()
+
+            self.request_configuration()
+            self.configuration_done = True
+        self.process_udp(data)
+
+    def on_response(self, data):
+        assert self.configuration_done
+        assert not self.configuration_saved  # The configuration should be delivered only once.
+        if self.verbose:
+            print(data)
+        self.publish("lidar_config", data.decode())
+        self.configuration_saved = True