initial pozyx positioning upload using anchors

src/pozyx_anchor/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pozyx_anchor</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/pozyx_anchor/pozyx_anchor/pozyx_pos.py

@@ -0,0 +1,94 @@
+from pypozyx import *
+from pythonosc import *
+from pythonosc import udp_client
+import rclpy
+from rclpy.node import Node
+
+### ALL MEASUREMENT UNITS ARE IN MILLIMETERS
+
+remote = False                                      # true if using given remote device for ranging
+use_processing = False                              # true if sending position data through OSC
+ip = '127.0.0.1'                                    # replace with IP for the OSC UDP
+network_port = 8888                                 # replace with network port for OSC UDP
+osc_udp_client = None if not use_processing else udp_client.SimpleUDPClient(ip, network_port)  # is using processing, setup UDP client
+remote_id = None if not remote else 0x605D          # replace with netowrk ID of remote device
+ranging_protocol = POZYX_RANGE_PROTOCOL_PRECISION   # ranging protocol
+algorithm = POZYX_POS_ALG_UWB_ONLY                  # positioning algorithm to use
+dimension = POZYX_2D                                # positioning dimension (should be 2D for our purposes)
+height = 1000                                       # height of device (shouldn't be needed for our purposes, just in case)
+
+# anchors used for positioning, replace device IDs and coordinates
+# this assumes anchors are making a 3000mm x 3000mm grid, all at heights of 2000mm
+anchors = [DeviceCoordinates(0x0001, 1, Coordinates(0, 0, 2000)),
+           DeviceCoordinates(0x0002, 1, Coordinates(3000, 0, 2000)),
+           DeviceCoordinates(0x0003, 1, Coordinates(0, 3000, 2000)),
+           DeviceCoordinates(0x0004, 1, Coordinates(3000, 3000, 2000)),]
+
+
+class AnchorNode(Node):
+    def __init__(self, port):
+        self.pozyx = PozyxSerial(port)
+        self.setup()
+        self.approx_loc()
+
+    def setup(self):
+        self.pozyx.clearDevices(remote_id)
+        self.set_anchors_manual()
+
+
+    def set_anchors_manual(self):
+        status = self.pozyx.clearDevices(remote_id)
+        for anchor in anchors:
+            status &= self.pozyx.addDevice(anchor, remote_id)
+        if len(anchors > 4):
+            status &= self.pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, len(anchors))
+        return
+
+    def approx_range(self, anchor_id):
+        while True:
+            device_range = DeviceRange()
+            status = self.pozyx.doRanging(anchor_id, device_range, remote_id)
+            if status == POZYX_SUCCESS:
+                self.get_logger().info(device_range)
+            else:
+                self.log_pos_error('ranging')
+
+    def approx_loc(self):
+        while True:
+            position = Coordinates()
+            status = self.pozyx.doPositioning(position, dimension, height, algorithm, remote_id=remote_id)
+            if status == POZYX_SUCCESS:
+                self.get_logger().info(position)
+            else:
+                self.log_pos_error('positioning')
+
+    def log_pos_error(self, operation):
+        error_code = SingleRegister()
+        network_id = remote_id
+        if network_id is None:
+            self.pozyx.getErrorCode(error_code)
+            self.get_logger().info(f'ERROR {operation}, local error code {str(error_code)}')
+            if osc_udp_client is not None:
+                osc_udp_client.send_message("/error", [operation, 0, error_code[0]])
+            return
+        status = self.pozyx.getErrorCode(error_code, remote_id)
+        if status == POZYX_SUCCESS:
+            self.get_logger().info(f'ERROR {operation} on ID {"0x%0.4x" % network_id}, error code {str(error_code)}')
+            if osc_udp_client is not None:
+                osc_udp_client.send_message("/error", [operation, network_id, error_code[0]])
+        else:
+            self.pozyx.getErrorCode(error_code)
+            self.get_logger().info(f'ERROR {operation}, couldn\'t retrieve remote error code, local error code {str(error_code)}')
+            if osc_udp_client is not None:
+                osc_udp_client.send_message("/error", [operation, 0, -1])
+
+
+def main():
+    # port = 'COM1'                             # replace with port of pozyx device
+    port = get_serial_ports()[0].device         # should get serial port automatically?     
+    rclpy.init()            
+    rclpy.spin(AnchorNode(port))
+
+
+if __name__ == "__main__":
+    main()

src/pozyx_anchor/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/pozyx_anchor
+[install]
+install-scripts=$base/lib/pozyx_anchor

src/pozyx_anchor/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'pozyx_anchor'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='root',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/pozyx_anchor/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/pozyx_anchor/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/pozyx_anchor/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'